376 lines
12 KiB
C++
376 lines
12 KiB
C++
#include "ScanDataCache.h"
|
|
|
|
#include "LaserDataLoader.h"
|
|
#include "DebugDataSaver.h"
|
|
#include <cstring>
|
|
#include <memory>
|
|
|
|
namespace {
|
|
template <typename Point3D, typename Point2D>
|
|
bool CopyTyped(const SVzLaserLineData& source, SVzLaserLineData& destination)
|
|
{
|
|
destination = source;
|
|
destination.p3DPoint = nullptr;
|
|
destination.p2DPoint = nullptr;
|
|
if (source.nPointCount <= 0) return true;
|
|
if (!source.p3DPoint) return false;
|
|
|
|
auto points3D = std::make_unique<Point3D[]>(source.nPointCount);
|
|
auto points2D = std::make_unique<Point2D[]>(source.nPointCount);
|
|
std::memcpy(points3D.get(), source.p3DPoint,
|
|
sizeof(Point3D) * static_cast<std::size_t>(source.nPointCount));
|
|
if (source.p2DPoint) {
|
|
std::memcpy(points2D.get(), source.p2DPoint,
|
|
sizeof(Point2D) * static_cast<std::size_t>(source.nPointCount));
|
|
}
|
|
destination.p3DPoint = points3D.release();
|
|
destination.p2DPoint = points2D.release();
|
|
return true;
|
|
}
|
|
}
|
|
|
|
ScanDataCache::ScanDataCache()
|
|
: m_worker(&ScanDataCache::WorkerLoop, this)
|
|
{
|
|
}
|
|
|
|
ScanDataCache::~ScanDataCache()
|
|
{
|
|
Stop();
|
|
Clear();
|
|
}
|
|
|
|
void ScanDataCache::BeginScan(ScanId scanId)
|
|
{
|
|
(void)scanId;
|
|
Drain();
|
|
{
|
|
std::lock_guard<std::mutex> queueLock(m_queueMutex);
|
|
while (!m_queue.empty()) {
|
|
Data owned;
|
|
owned.emplace_back(m_queue.front().dataType, m_queue.front().line);
|
|
FreeData(owned);
|
|
m_queue.pop_front();
|
|
}
|
|
m_queuedBytes = 0;
|
|
}
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
for (auto& entry : m_buffers) {
|
|
FreeData(entry.second.data);
|
|
}
|
|
m_buffers.clear();
|
|
}
|
|
|
|
bool ScanDataCache::TryEnqueue(ScanId scanId, int cameraIndex,
|
|
EVzResultDataType dataType,
|
|
const SVzLaserLineData& source)
|
|
{
|
|
QueueItem item;
|
|
item.scanId = scanId;
|
|
item.cameraIndex = cameraIndex;
|
|
item.dataType = dataType;
|
|
try {
|
|
if (!CopyLine(dataType, source, item.line)) return false;
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
item.bytes = static_cast<std::size_t>(DebugDataSaver::GetLineStorageSize(dataType, item.line));
|
|
|
|
std::unique_lock<std::mutex> lock(m_queueMutex);
|
|
if (!m_running.load() || item.bytes == 0) {
|
|
FreeLine(dataType, item.line);
|
|
return false;
|
|
}
|
|
|
|
const std::size_t queuedBytes = item.bytes;
|
|
try {
|
|
m_queue.emplace_back(std::move(item));
|
|
} catch (...) {
|
|
FreeLine(dataType, item.line);
|
|
return false;
|
|
}
|
|
{
|
|
std::lock_guard<std::mutex> stateLock(m_stateMutex);
|
|
try {
|
|
auto& buffer = m_buffers[{scanId, cameraIndex}];
|
|
++buffer.pending;
|
|
} catch (...) {
|
|
QueueItem rejected = std::move(m_queue.back());
|
|
m_queue.pop_back();
|
|
FreeLine(rejected.dataType, rejected.line);
|
|
return false;
|
|
}
|
|
}
|
|
m_queuedBytes += queuedBytes;
|
|
lock.unlock();
|
|
m_queueCondition.notify_one();
|
|
return true;
|
|
}
|
|
|
|
void ScanDataCache::FinishCamera(ScanId scanId, int cameraIndex)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
auto& buffer = m_buffers[{scanId, cameraIndex}];
|
|
buffer.finishRequested = true;
|
|
buffer.completed = buffer.pending == 0;
|
|
}
|
|
|
|
bool ScanDataCache::TryTakeCompleted(ScanId scanId, int cameraIndex, Data& data)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
auto it = m_buffers.find({scanId, cameraIndex});
|
|
if (it == m_buffers.end() || !it->second.completed || it->second.data.empty()) return false;
|
|
data = std::move(it->second.data);
|
|
it->second.data.clear();
|
|
it->second.completed = false;
|
|
return true;
|
|
}
|
|
|
|
void ScanDataCache::Restore(ScanId scanId, int cameraIndex, Data&& data)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
auto& buffer = m_buffers[{scanId, cameraIndex}];
|
|
FreeData(buffer.data);
|
|
buffer.data = std::move(data);
|
|
buffer.completed = true;
|
|
}
|
|
|
|
void ScanDataCache::ReplaceCompleted(ScanId scanId, int cameraIndex, Data&& data)
|
|
{
|
|
Restore(scanId, cameraIndex, std::move(data));
|
|
}
|
|
|
|
void ScanDataCache::SetLineObserver(
|
|
std::function<void(ScanId, int, EVzResultDataType, const SVzLaserLineData&)> observer)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
m_lineObserver = std::move(observer);
|
|
}
|
|
|
|
void ScanDataCache::SetReadyNotifier(std::function<void(ScanId, int)> notifier)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
m_readyNotifier = std::move(notifier);
|
|
}
|
|
|
|
bool ScanDataCache::CopyData(ScanId scanId, int cameraIndex, Data& data) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
const auto it = m_buffers.find({scanId, cameraIndex});
|
|
if (it == m_buffers.end()) return false;
|
|
for (const auto& line : it->second.data) {
|
|
SVzLaserLineData copy{};
|
|
try {
|
|
if (!CopyLine(line.first, line.second, copy)) {
|
|
FreeData(data);
|
|
return false;
|
|
}
|
|
data.emplace_back(line.first, copy);
|
|
} catch (...) {
|
|
Data owned;
|
|
owned.emplace_back(line.first, copy);
|
|
FreeData(owned);
|
|
FreeData(data);
|
|
return false;
|
|
}
|
|
}
|
|
return !data.empty();
|
|
}
|
|
|
|
bool ScanDataCache::HasData(ScanId scanId, int cameraIndex) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
const auto it = m_buffers.find({scanId, cameraIndex});
|
|
return it != m_buffers.end() && !it->second.data.empty();
|
|
}
|
|
|
|
bool ScanDataCache::IsCompleted(ScanId scanId, int cameraIndex) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
const auto it = m_buffers.find({scanId, cameraIndex});
|
|
return it != m_buffers.end() && it->second.completed;
|
|
}
|
|
|
|
bool ScanDataCache::IsIncomplete(ScanId scanId, int cameraIndex) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
const auto it = m_buffers.find({scanId, cameraIndex});
|
|
return it != m_buffers.end() && it->second.incomplete;
|
|
}
|
|
|
|
std::size_t ScanDataCache::Size(ScanId scanId, int cameraIndex) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
const auto it = m_buffers.find({scanId, cameraIndex});
|
|
return it == m_buffers.end() ? 0 : it->second.data.size();
|
|
}
|
|
|
|
void ScanDataCache::CancelScan(ScanId scanId)
|
|
{
|
|
Drain();
|
|
{
|
|
std::lock_guard<std::mutex> queueLock(m_queueMutex);
|
|
std::deque<QueueItem> retained;
|
|
while (!m_queue.empty()) {
|
|
QueueItem item = std::move(m_queue.front());
|
|
m_queue.pop_front();
|
|
if (item.scanId == scanId) {
|
|
Data owned;
|
|
owned.emplace_back(item.dataType, item.line);
|
|
FreeData(owned);
|
|
} else {
|
|
retained.push_back(std::move(item));
|
|
}
|
|
}
|
|
m_queue.swap(retained);
|
|
m_queuedBytes = 0;
|
|
std::deque<QueueItem> measure = m_queue;
|
|
while (!measure.empty()) {
|
|
m_queuedBytes += measure.front().bytes;
|
|
measure.pop_front();
|
|
}
|
|
}
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
for (auto it = m_buffers.begin(); it != m_buffers.end();) {
|
|
if (it->first.first == scanId) {
|
|
FreeData(it->second.data);
|
|
it = m_buffers.erase(it);
|
|
} else ++it;
|
|
}
|
|
}
|
|
|
|
void ScanDataCache::Clear()
|
|
{
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_queueMutex);
|
|
while (!m_queue.empty()) {
|
|
Data owned;
|
|
owned.emplace_back(m_queue.front().dataType, m_queue.front().line);
|
|
FreeData(owned);
|
|
m_queue.pop_front();
|
|
}
|
|
m_queuedBytes = 0;
|
|
}
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
for (auto& entry : m_buffers) FreeData(entry.second.data);
|
|
m_buffers.clear();
|
|
}
|
|
|
|
void ScanDataCache::Drain()
|
|
{
|
|
std::unique_lock<std::mutex> lock(m_queueMutex);
|
|
m_queueCondition.wait(lock, [this]() {
|
|
return m_queue.empty() && m_inFlight == 0;
|
|
});
|
|
}
|
|
|
|
void ScanDataCache::Stop()
|
|
{
|
|
if (!m_running.exchange(false)) return;
|
|
m_queueCondition.notify_all();
|
|
if (m_worker.joinable()) m_worker.join();
|
|
}
|
|
|
|
void ScanDataCache::WorkerLoop()
|
|
{
|
|
while (true) {
|
|
QueueItem item;
|
|
{
|
|
std::unique_lock<std::mutex> lock(m_queueMutex);
|
|
m_queueCondition.wait(lock, [this]() {
|
|
return !m_running.load() || !m_queue.empty();
|
|
});
|
|
if (!m_running.load() && m_queue.empty()) break;
|
|
item = std::move(m_queue.front());
|
|
m_queue.pop_front();
|
|
m_queuedBytes -= item.bytes;
|
|
++m_inFlight;
|
|
}
|
|
|
|
std::function<void(ScanId, int, EVzResultDataType, const SVzLaserLineData&)> observer;
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
observer = m_lineObserver;
|
|
}
|
|
if (observer) {
|
|
try { observer(item.scanId, item.cameraIndex, item.dataType, item.line); }
|
|
catch (...) { }
|
|
}
|
|
|
|
std::function<void(ScanId, int)> notifier;
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_stateMutex);
|
|
auto& buffer = m_buffers[{item.scanId, item.cameraIndex}];
|
|
try {
|
|
buffer.data.emplace_back(item.dataType, item.line);
|
|
item.line = SVzLaserLineData{};
|
|
} catch (...) {
|
|
buffer.incomplete = true;
|
|
}
|
|
if (buffer.pending > 0) --buffer.pending;
|
|
if (buffer.finishRequested && buffer.pending == 0) buffer.completed = true;
|
|
if (buffer.completed) notifier = m_readyNotifier;
|
|
}
|
|
if (item.line.p3DPoint || item.line.p2DPoint) {
|
|
FreeLine(item.dataType, item.line);
|
|
}
|
|
if (notifier) notifier(item.scanId, item.cameraIndex);
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_queueMutex);
|
|
--m_inFlight;
|
|
}
|
|
m_queueCondition.notify_all();
|
|
}
|
|
}
|
|
|
|
bool ScanDataCache::CopyLine(EVzResultDataType type,
|
|
const SVzLaserLineData& source,
|
|
SVzLaserLineData& destination)
|
|
{
|
|
switch (type) {
|
|
case keResultDataType_Position:
|
|
return CopyTyped<SVzNL3DPosition, SVzNL2DPosition>(source, destination);
|
|
case keResultDataType_PositionF:
|
|
return CopyTyped<SVzNL3DPosition, SVzNL2DPositionF>(source, destination);
|
|
case keResultDataType_PointXYZRGBA:
|
|
return CopyTyped<SVzNLPointXYZRGBA, SVzNL2DLRPoint>(source, destination);
|
|
case keResultDataType_PointXYZI:
|
|
return CopyTyped<SVzNLPointXYZI, SVzNL2DLRPoint>(source, destination);
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void ScanDataCache::FreeData(Data& data)
|
|
{
|
|
LaserDataLoader loader;
|
|
loader.FreeLaserScanData(data);
|
|
data.clear();
|
|
}
|
|
|
|
void ScanDataCache::FreeLine(EVzResultDataType type, SVzLaserLineData& line)
|
|
{
|
|
switch (type) {
|
|
case keResultDataType_Position:
|
|
delete[] static_cast<SVzNL3DPosition*>(line.p3DPoint);
|
|
delete[] static_cast<SVzNL2DPosition*>(line.p2DPoint);
|
|
break;
|
|
case keResultDataType_PositionF:
|
|
delete[] static_cast<SVzNL3DPosition*>(line.p3DPoint);
|
|
delete[] static_cast<SVzNL2DPositionF*>(line.p2DPoint);
|
|
break;
|
|
case keResultDataType_PointXYZRGBA:
|
|
delete[] static_cast<SVzNLPointXYZRGBA*>(line.p3DPoint);
|
|
delete[] static_cast<SVzNL2DLRPoint*>(line.p2DPoint);
|
|
break;
|
|
case keResultDataType_PointXYZI:
|
|
delete[] static_cast<SVzNLPointXYZI*>(line.p3DPoint);
|
|
delete[] static_cast<SVzNL2DLRPoint*>(line.p2DPoint);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
line.p3DPoint = nullptr;
|
|
line.p2DPoint = nullptr;
|
|
}
|