#include "DebugDataSaver.h" #include "LaserDataLoader.h" #include "VrError.h" #include "VrLog.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace { std::pair PointStorageSizes(EVzResultDataType dataType) { switch (dataType) { case keResultDataType_Position: return {sizeof(SVzNL3DPosition), sizeof(SVzNL2DPosition)}; case keResultDataType_PositionF: return {sizeof(SVzNL3DPosition), sizeof(SVzNL2DPositionF)}; case keResultDataType_PointXYZRGBA: return {sizeof(SVzNLPointXYZRGBA), sizeof(SVzNL2DLRPoint)}; case keResultDataType_PointXYZI: return {sizeof(SVzNLPointXYZI), sizeof(SVzNL2DLRPoint)}; default: return {0, 0}; } } QString AppName() { QString name = QFileInfo(QCoreApplication::applicationFilePath()).baseName(); if (name.isEmpty()) { name = QStringLiteral("app"); } return name; } // 调试目录按 <应用名>_<年月日> 划分,同一天的扫描数据、二维图和结果文本放在一起。 QString SubDirPrefix() { return AppName() + QStringLiteral("_"); } // 只认 <应用名>_;旧版本 <应用名>_PointCloud_ 同样纳入 // 容量管理,避免升级后历史目录不再参与清理。删除逻辑依赖该判断, // 不能放宽成一个前缀匹配,否则可能误删同名开头的其它目录。 bool MatchesDebugSubDir(const QString& dirName) { const QString prefix = SubDirPrefix(); if (!dirName.startsWith(prefix)) { return false; } const QString legacyTag = QStringLiteral("PointCloud_"); QString suffix = dirName.mid(prefix.size()); if (suffix.startsWith(legacyTag)) { suffix = suffix.mid(legacyTag.size()); } if (suffix.size() != 8) { return false; } for (const QChar ch : suffix) { if (!ch.isDigit()) { return false; } } return true; } std::string NormalizeBasePath(const std::string& basePath) { return QDir(QString::fromStdString(basePath)).absolutePath().toStdString(); } std::string NormalizeFilePath(const std::string& filePath) { return QFileInfo(QString::fromStdString(filePath)).absoluteFilePath().toStdString(); } } // namespace DebugDataSaver::DebugDataSaver(int maxThreads) : m_maxThreads(std::max(1, std::min(maxThreads, 4))) , m_running(true) { for (int i = 0; i < m_maxThreads; ++i) { m_workers.emplace_back(&DebugDataSaver::WorkerThread, this); } LOG_INFO("[DebugDataSaver] started %d storage threads\n", m_maxThreads); } DebugDataSaver::~DebugDataSaver() { Stop(); } void DebugDataSaver::Stop() { FinishAllStreams(); if (!m_running.exchange(false)) { return; } m_queueCondition.notify_all(); for (auto& worker : m_workers) { if (worker.joinable()) { worker.join(); } } m_workers.clear(); std::lock_guard lock(m_queueMutex); while (!m_taskQueue.empty()) { FreeData(m_taskQueue.front().data); m_taskQueue.pop(); } LOG_INFO("[DebugDataSaver] stopped\n"); } void DebugDataSaver::SetMaxQueueSize(int maxTasks) { if (maxTasks < 1) { maxTasks = 1; } m_maxQueueSize = maxTasks; } void DebugDataSaver::PrepareStorageCache(const std::string& basePath) { std::lock_guard lock(m_diskSpaceMutex); StorageCache& cache = m_storageCaches[NormalizeBasePath(basePath)]; if (!cache.initialized) { BuildStorageCacheLocked(basePath, cache); LOG_INFO("[DebugDataSaver] storage cache ready: %s, files=%zu, size=%lld\n", basePath.c_str(), cache.files.size(), static_cast(cache.totalSize)); } EnsureStorageLimitLocked(basePath, cache); } uint64_t DebugDataSaver::GetLineStorageSize(EVzResultDataType dataType, const SVzLaserLineData& line) { if (line.nPointCount < 0) return 0; const auto sizes = PointStorageSizes(dataType); if (sizes.first == 0) return 0; return sizeof(StreamLine) + static_cast(sizes.first + sizes.second) * line.nPointCount; } bool DebugDataSaver::StartStream(int cameraIndex, const std::string& basePath, const std::string& timestamp, const std::string& deviceName) { FinishStream(cameraIndex, false); if (!m_running.load()) return false; PrepareStorageCache(basePath); auto task = std::make_shared(); task->basePath = basePath; const QString requested = QString::fromStdString(GenerateSavePath(basePath, timestamp, deviceName)); const QFileInfo info(requested); if (!info.absoluteDir().mkpath(QStringLiteral("."))) return false; QString path = requested; int suffix = 0; while (QFileInfo::exists(path) || QFileInfo::exists(path + QStringLiteral(".part"))) { path = info.absoluteDir().filePath(info.completeBaseName() + QStringLiteral("_%1.txt").arg(++suffix)); } task->filePath = path.toStdString(); task->output.open(task->filePath + ".part", std::ios::binary | std::ios::trunc); task->output << "LineNum:" << std::left << std::setw(20) << 0 << "\nDataType:0\nScanSpeed:0\nPointAdjust: 1\nMaxTimeStamp:0_0\n"; task->output.flush(); if (!task->output.good()) { LOG_ERROR("[DebugDataSaver] stream open failed: %s\n", task->filePath.c_str()); return false; } { std::lock_guard lock(m_streamMutex); m_streams[cameraIndex] = task; try { task->worker = std::thread(&DebugDataSaver::StreamWorker, task); } catch (const std::exception& error) { m_streams.erase(cameraIndex); LOG_ERROR("[DebugDataSaver] stream worker failed: %s\n", error.what()); return false; } } LOG_INFO("[DebugDataSaver] streaming TXT started: %s\n", task->filePath.c_str()); return true; } bool DebugDataSaver::AppendStream(int cameraIndex, EVzResultDataType dataType, const SVzLaserLineData& line) { std::shared_ptr task; std::unique_lock appendLock; { std::lock_guard lock(m_streamMutex); const auto entry = m_streams.find(cameraIndex); if (entry == m_streams.end()) return true; task = entry->second; appendLock = std::unique_lock(task->appendMutex); } try { const uint64_t bytes = GetLineStorageSize(dataType, line); if (bytes == 0 || (line.nPointCount > 0 && !line.p3DPoint)) { throw std::runtime_error("Invalid point-cloud line"); } auto copy = std::make_unique(); copy->dataType = dataType; copy->line = line; copy->line.p3DPoint = nullptr; copy->line.p2DPoint = nullptr; copy->bytes = static_cast(bytes); const auto sizes = PointStorageSizes(dataType); if (line.nPointCount > 0) { const size_t bytes3D = sizes.first * line.nPointCount; const size_t bytes2D = sizes.second * line.nPointCount; copy->points3D = std::make_unique(bytes3D); std::memcpy(copy->points3D.get(), line.p3DPoint, bytes3D); copy->points2D = std::make_unique(bytes2D); if (line.p2DPoint) std::memcpy(copy->points2D.get(), line.p2DPoint, bytes2D); copy->line.p3DPoint = copy->points3D.get(); copy->line.p2DPoint = copy->points2D.get(); } std::lock_guard lock(task->queueMutex); if (task->closing || task->failed) return false; task->lines.push(std::move(copy)); task->queuedBytes += bytes; task->condition.notify_all(); return true; } catch (const std::exception& error) { std::lock_guard lock(task->queueMutex); task->failed = true; task->condition.notify_all(); LOG_ERROR("[DebugDataSaver] stream enqueue failed: %s\n", error.what()); return false; } } void DebugDataSaver::StreamWorker(const std::shared_ptr& task) { try { auto lastFlush = std::chrono::steady_clock::now(); while (true) { std::unique_ptr line; { std::unique_lock lock(task->queueMutex); task->condition.wait_for(lock, std::chrono::milliseconds(500), [&]() { return task->closing || task->failed || !task->lines.empty(); }); if (task->lines.empty() && (task->closing || task->failed)) break; if (!task->lines.empty()) { line = std::move(task->lines.front()); task->lines.pop(); } } if (line && !LaserDataLoader::WriteLaserScanLine(task->output, line->dataType, line->line)) { throw std::runtime_error("Point-cloud TXT write failed"); } const auto now = std::chrono::steady_clock::now(); if (now - lastFlush >= std::chrono::milliseconds(500)) { task->output.flush(); lastFlush = now; } std::lock_guard lock(task->queueMutex); if (line) task->queuedBytes -= line->bytes; if (!task->output.good()) { task->failed = true; task->condition.notify_all(); LOG_ERROR("[DebugDataSaver] stream write failed: %s\n", task->filePath.c_str()); break; } if (line) ++task->writtenLines; task->condition.notify_all(); } } catch (const std::exception& error) { std::lock_guard lock(task->queueMutex); task->failed = true; task->condition.notify_all(); LOG_ERROR("[DebugDataSaver] stream worker failed: %s\n", error.what()); } } bool DebugDataSaver::FinishStream(int cameraIndex, bool complete) { std::shared_ptr task; { std::lock_guard lock(m_streamMutex); const auto entry = m_streams.find(cameraIndex); if (entry == m_streams.end()) return true; task = entry->second; m_streams.erase(entry); } std::lock_guard appendLock(task->appendMutex); { std::lock_guard lock(task->queueMutex); task->closing = true; task->condition.notify_all(); } if (task->worker.joinable()) task->worker.join(); task->output.seekp(sizeof("LineNum:") - 1); task->output << std::left << std::setw(20) << task->writtenLines; task->output.flush(); const bool written = task->output.good() && !task->failed && task->writtenLines <= static_cast((std::numeric_limits::max)()); task->output.close(); bool success = written && task->output.good() && complete; std::string savedPath = task->filePath + ".part"; if (success) { success = QFile::rename(QString::fromStdString(savedPath), QString::fromStdString(task->filePath)); if (success) savedPath = task->filePath; } LOG_INFO("[DebugDataSaver] stream finished: complete=%d lines=%llu path=%s\n", success ? 1 : 0, static_cast(task->writtenLines), savedPath.c_str()); RecordSavedFile(task->basePath, savedPath, true); return success; } void DebugDataSaver::FinishAllStreams() { std::vector cameras; { std::lock_guard lock(m_streamMutex); for (const auto& entry : m_streams) cameras.push_back(entry.first); } for (int cameraIndex : cameras) FinishStream(cameraIndex, false); } void DebugDataSaver::SaveAsync(const std::string& basePath, const std::string& timestamp, const std::vector>& data, const std::string& deviceName, bool binary) { if (data.empty() || !m_running.load()) { return; } const int maxQueueSize = m_maxQueueSize.load(); { std::lock_guard lock(m_queueMutex); if (static_cast(m_taskQueue.size()) >= maxQueueSize) { LOG_WARNING("[DebugDataSaver] save queue full (%zu/%d), skip point cloud save: %s\n", m_taskQueue.size(), maxQueueSize, deviceName.c_str()); return; } } SaveTask task; task.basePath = basePath; task.deviceName = deviceName; task.binary = binary; task.filePath = GenerateSavePath(basePath, timestamp, deviceName, binary); if (!QFileInfo(QString::fromStdString(task.filePath)).absoluteDir().mkpath(QStringLiteral("."))) { LOG_ERROR("[DebugDataSaver] cannot create output directory: %s\n", task.filePath.c_str()); return; } task.data = DeepCopyData(data); { std::lock_guard lock(m_queueMutex); if (static_cast(m_taskQueue.size()) >= maxQueueSize) { LOG_WARNING("[DebugDataSaver] save queue full after copy (%zu/%d), skip point cloud save: %s\n", m_taskQueue.size(), maxQueueSize, deviceName.c_str()); FreeData(task.data); return; } m_taskQueue.push(std::move(task)); } m_queueCondition.notify_one(); } void DebugDataSaver::WorkerThread() { LaserDataLoader dataLoader; while (true) { SaveTask task; { std::unique_lock lock(m_queueMutex); m_queueCondition.wait(lock, [this] { return !m_taskQueue.empty() || !m_running.load(); }); if (!m_running.load() && m_taskQueue.empty()) { break; } if (m_taskQueue.empty()) { continue; } task = std::move(m_taskQueue.front()); m_taskQueue.pop(); } int lineNum = static_cast(task.data.size()); float scanSpeed = 0.0f; int maxTimeStamp = 0; int clockPerSecond = 0; int result = task.binary ? dataLoader.SaveLaserScanDataBinary(task.filePath, task.data, lineNum, scanSpeed, maxTimeStamp, clockPerSecond) : dataLoader.SaveLaserScanData(task.filePath, task.data, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); if (result == SUCCESS) { RecordSavedFile(task.basePath, task.filePath); LOG_INFO("[DebugDataSaver] saved: %s lines=%d firstFrame=%llu lastFrame=%llu\n", task.filePath.c_str(), lineNum, task.data.front().second.llFrameIdx, task.data.back().second.llFrameIdx); } else { LOG_ERROR("[DebugDataSaver] save failed: %s, error: %s\n", task.filePath.c_str(), dataLoader.GetLastError().c_str()); // 失败文件也计入空间管理,避免磁盘写满后残留文件长期绕过容量限制。 RecordSavedFile(task.basePath, task.filePath); } FreeData(task.data); } } std::vector> DebugDataSaver::DeepCopyData( const std::vector>& src) { std::vector> dst; dst.reserve(src.size()); for (const auto& item : src) { EVzResultDataType dataType = item.first; const SVzLaserLineData& srcLine = item.second; SVzLaserLineData dstLine; memset(&dstLine, 0, sizeof(SVzLaserLineData)); if (srcLine.nPointCount > 0) { if (dataType == keResultDataType_Position) { dstLine.p3DPoint = new SVzNL3DPosition[srcLine.nPointCount](); if (srcLine.p3DPoint) { memcpy(dstLine.p3DPoint, srcLine.p3DPoint, sizeof(SVzNL3DPosition) * srcLine.nPointCount); } dstLine.p2DPoint = new SVzNL2DPosition[srcLine.nPointCount](); if (srcLine.p2DPoint) { memcpy(dstLine.p2DPoint, srcLine.p2DPoint, sizeof(SVzNL2DPosition) * srcLine.nPointCount); } } else if (dataType == keResultDataType_PositionF) { dstLine.p3DPoint = new SVzNL3DPosition[srcLine.nPointCount](); if (srcLine.p3DPoint) { memcpy(dstLine.p3DPoint, srcLine.p3DPoint, sizeof(SVzNL3DPosition) * srcLine.nPointCount); } dstLine.p2DPoint = new SVzNL2DPositionF[srcLine.nPointCount](); if (srcLine.p2DPoint) { memcpy(dstLine.p2DPoint, srcLine.p2DPoint, sizeof(SVzNL2DPositionF) * srcLine.nPointCount); } } else if (dataType == keResultDataType_PointXYZRGBA) { dstLine.p3DPoint = new SVzNLPointXYZRGBA[srcLine.nPointCount](); if (srcLine.p3DPoint) { memcpy(dstLine.p3DPoint, srcLine.p3DPoint, sizeof(SVzNLPointXYZRGBA) * srcLine.nPointCount); } dstLine.p2DPoint = new SVzNL2DLRPoint[srcLine.nPointCount](); if (srcLine.p2DPoint) { memcpy(dstLine.p2DPoint, srcLine.p2DPoint, sizeof(SVzNL2DLRPoint) * srcLine.nPointCount); } } else if (dataType == keResultDataType_PointXYZI) { dstLine.p3DPoint = new SVzNLPointXYZI[srcLine.nPointCount](); if (srcLine.p3DPoint) { memcpy(dstLine.p3DPoint, srcLine.p3DPoint, sizeof(SVzNLPointXYZI) * srcLine.nPointCount); } dstLine.p2DPoint = new SVzNL2DLRPoint[srcLine.nPointCount](); if (srcLine.p2DPoint) { memcpy(dstLine.p2DPoint, srcLine.p2DPoint, sizeof(SVzNL2DLRPoint) * srcLine.nPointCount); } } } dstLine.nPointCount = srcLine.nPointCount; dstLine.dTotleOffset = srcLine.dTotleOffset; dstLine.dStep = srcLine.dStep; dstLine.llTimeStamp = srcLine.llTimeStamp; dstLine.llFrameIdx = srcLine.llFrameIdx; dstLine.nEncodeNo = srcLine.nEncodeNo; dstLine.fSwingAngle = srcLine.fSwingAngle; dstLine.bEndOnceScan = srcLine.bEndOnceScan; dst.push_back(std::make_pair(dataType, dstLine)); } return dst; } void DebugDataSaver::FreeData(std::vector>& data) { LaserDataLoader loader; loader.FreeLaserScanData(data); data.clear(); } std::string DebugDataSaver::GenerateSavePath(const std::string& basePath, const std::string& timestamp, const std::string& deviceName, bool binary) { const QString timestampText = QString::fromStdString(timestamp); const QString dateDirectory = SubDirPrefix() + timestampText.left(8); const QString fullPath = QDir(QString::fromStdString(basePath)) .filePath(dateDirectory); QString name = deviceName.empty() ? QStringLiteral("pointcloud") : QString::fromStdString(deviceName); QString fileName = QString("%1/%2_%3.%4").arg(fullPath) .arg(name) .arg(QString::fromStdString(timestamp)) .arg(binary ? QStringLiteral("dat") : QStringLiteral("txt")); return fileName.toStdString(); } void DebugDataSaver::EnsureStorageLimit(const std::string& basePath) { std::lock_guard lock(m_diskSpaceMutex); StorageCache& cache = m_storageCaches[NormalizeBasePath(basePath)]; if (!cache.initialized) { BuildStorageCacheLocked(basePath, cache); } EnsureStorageLimitLocked(basePath, cache); } void DebugDataSaver::EnsureStorageLimitLocked(const std::string& basePath, StorageCache& cache, const std::string& protectedPath) { Q_UNUSED(basePath); const int64_t limit = m_maxStorageSize.load(); if (limit <= 0) { return; } while (cache.totalSize > limit && !cache.files.empty()) { auto candidate = std::find_if(cache.files.begin(), cache.files.end(), [&](const StorageFileInfo& file) { return file.path != protectedPath; }); if (candidate == cache.files.end()) break; StorageFileInfo oldest = *candidate; cache.files.erase(candidate); QString filePath = QString::fromStdString(oldest.path); QFileInfo info(filePath); QString parentPath = info.absolutePath(); int64_t fileSize = oldest.size; if (QFileInfo::exists(filePath) && !QFile::remove(filePath)) { cache.files.insert(cache.files.begin(), oldest); LOG_ERROR("[DebugDataSaver] delete failed: %s\n", oldest.path.c_str()); break; } cache.totalSize -= fileSize; if (cache.totalSize < 0) { cache.totalSize = 0; } LOG_INFO("[DebugDataSaver] storage limit exceeded, delete oldest file: %s (%lld bytes)\n", oldest.path.c_str(), static_cast(fileSize)); QDir parentDir(parentPath); const QString parentName = parentDir.dirName(); if (MatchesDebugSubDir(parentName) && parentDir.isEmpty()) { parentDir.cdUp(); if (parentDir.rmdir(parentName)) { LOG_INFO("[DebugDataSaver] removed empty directory: %s\n", parentPath.toStdString().c_str()); } } } } void DebugDataSaver::BuildStorageCacheLocked(const std::string& basePath, StorageCache& cache) { cache.files.clear(); cache.totalSize = 0; QDir rootDir(QString::fromStdString(basePath)); if (rootDir.exists()) { const QStringList subDirs = rootDir.entryList(QDir::Dirs | QDir::NoDotAndDotDot); for (const QString& name : subDirs) { if (!MatchesDebugSubDir(name)) { continue; } QString subPath = rootDir.absoluteFilePath(name); QDirIterator it(subPath, QStringList() << "*.txt" << "*.dat", QDir::Files, QDirIterator::Subdirectories); while (it.hasNext()) { it.next(); QFileInfo info = it.fileInfo(); StorageFileInfo fileInfo; fileInfo.path = info.absoluteFilePath().toStdString(); fileInfo.size = info.size(); fileInfo.lastModifiedMSecs = info.lastModified().toMSecsSinceEpoch(); cache.totalSize += fileInfo.size; cache.files.push_back(fileInfo); } } } std::sort(cache.files.begin(), cache.files.end(), DebugDataSaver::IsOlderFile); cache.initialized = true; } void DebugDataSaver::AddFileToCacheLocked(StorageCache& cache, const StorageFileInfo& fileInfo) { RemoveFileFromCache(cache, fileInfo.path); auto it = std::lower_bound(cache.files.begin(), cache.files.end(), fileInfo, DebugDataSaver::IsOlderFile); cache.files.insert(it, fileInfo); cache.totalSize += fileInfo.size; } void DebugDataSaver::RecordSavedFile(const std::string& basePath, const std::string& filePath, bool protectFile) { std::lock_guard lock(m_diskSpaceMutex); StorageCache& cache = m_storageCaches[NormalizeBasePath(basePath)]; if (!cache.initialized) { BuildStorageCacheLocked(basePath, cache); } std::string normalizedFilePath = NormalizeFilePath(filePath); QFileInfo info(QString::fromStdString(normalizedFilePath)); if (info.exists() && info.isFile()) { StorageFileInfo fileInfo; fileInfo.path = normalizedFilePath; fileInfo.size = info.size(); fileInfo.lastModifiedMSecs = info.lastModified().toMSecsSinceEpoch(); AddFileToCacheLocked(cache, fileInfo); } EnsureStorageLimitLocked(basePath, cache, protectFile ? normalizedFilePath : std::string()); } bool DebugDataSaver::RemoveFileFromCache(StorageCache& cache, const std::string& filePath) { auto it = std::find_if(cache.files.begin(), cache.files.end(), [&filePath](const StorageFileInfo& item) { return item.path == filePath; }); if (it == cache.files.end()) { return false; } cache.totalSize -= it->size; if (cache.totalSize < 0) { cache.totalSize = 0; } cache.files.erase(it); return true; } bool DebugDataSaver::IsOlderFile(const StorageFileInfo& lhs, const StorageFileInfo& rhs) { if (lhs.lastModifiedMSecs == rhs.lastModifiedMSecs) { return lhs.path < rhs.path; } return lhs.lastModifiedMSecs < rhs.lastModifiedMSecs; } int64_t DebugDataSaver::ComputeDirSize(const std::string& rootPath) { QDir rootDir(QString::fromStdString(rootPath)); if (!rootDir.exists()) { return 0; } int64_t total = 0; const QStringList subDirs = rootDir.entryList(QDir::Dirs | QDir::NoDotAndDotDot); for (const QString& name : subDirs) { if (!MatchesDebugSubDir(name)) { continue; } QString subPath = rootDir.absoluteFilePath(name); QDirIterator it(subPath, QStringList() << "*.txt" << "*.dat", QDir::Files, QDirIterator::Subdirectories); while (it.hasNext()) { it.next(); total += it.fileInfo().size(); } } return total; } std::string DebugDataSaver::FindOldestFile(const std::string& rootPath) { QDir rootDir(QString::fromStdString(rootPath)); if (!rootDir.exists()) { return std::string(); } QString oldestPath; QDateTime oldestTime; bool first = true; const QStringList subDirs = rootDir.entryList(QDir::Dirs | QDir::NoDotAndDotDot); for (const QString& name : subDirs) { if (!MatchesDebugSubDir(name)) { continue; } QString subPath = rootDir.absoluteFilePath(name); QDirIterator it(subPath, QStringList() << "*.txt" << "*.dat", QDir::Files, QDirIterator::Subdirectories); while (it.hasNext()) { it.next(); QFileInfo info = it.fileInfo(); QDateTime mtime = info.lastModified(); if (first || mtime < oldestTime) { oldestTime = mtime; oldestPath = info.absoluteFilePath(); first = false; } } } return oldestPath.toStdString(); }