From 3eea68772b7c58c2f5b8a8a2ab6dfe43ce652e82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=B0=E4=BB=94?= Date: Sun, 17 May 2026 18:59:44 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E9=9B=B7=E8=BE=BE?= =?UTF-8?q?=E7=BD=91=E7=BB=9C=E6=8E=A5=E5=8F=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Device/RsLidarDevice/Inc/IRsLidarDevice.h | 46 +++- Device/RsLidarDevice/RsLidarDevice.pro | 3 + Device/RsLidarDevice/Src/RsLidarDevice.cpp | 240 ++++++++++++++++-- Device/RsLidarDevice/_Inc/RsLidarDevice.h | 33 ++- .../driver/input/win/input_sock_select.hpp | 57 +++-- Test/RsLidarTest/main.cpp | 197 +++++++++++--- 6 files changed, 486 insertions(+), 90 deletions(-) diff --git a/Device/RsLidarDevice/Inc/IRsLidarDevice.h b/Device/RsLidarDevice/Inc/IRsLidarDevice.h index 98550ead..36b86343 100644 --- a/Device/RsLidarDevice/Inc/IRsLidarDevice.h +++ b/Device/RsLidarDevice/Inc/IRsLidarDevice.h @@ -73,11 +73,27 @@ struct RsLidarConfig float endAngle = 360.0f; ///< 有效角度结束(度) float splitAngle = 0.0f; ///< 帧分割角度(度) std::string frameId = "rslidar"; ///< 帧标识字符串 + unsigned int socketRecvBufBytes = 33554432; ///< socket 接收缓冲，默认 32MB；低配机推荐 ≥16MB }; -// ============================================================ -// 抽象接口 -// ============================================================ +/// 帧元数据（不含点数据，用于原始点云回调） +struct RsFrameMeta +{ + unsigned int seq = 0; + double timestamp = 0.0; + unsigned int height = 0; + unsigned int width = 0; + bool isDense = false; + std::string frameId; +}; + +/// 用户回调耗时统计（来自最近一个 5s 滚动窗口的快照） +struct RsCallbackStats +{ + unsigned long long maxUs = 0; ///< 窗口内单次回调最大耗时(μs) + unsigned long long avgUs = 0; ///< 窗口内平均耗时(μs) + unsigned long long count = 0; ///< 窗口内回调次数 +}; class RSLIDARDEVICE_EXPORT IRsLidarDevice { @@ -86,6 +102,11 @@ public: using PacketCallback = std::function; using ExceptionCallback = std::function; + /// 原始点云回调：提供未经转换的 SDK 原始点（含 NaN，单位米）， + /// 用户可在一次遍历中完成 NaN 清洗 + 单位转换 + 格式复制， + /// 避免 RsLidarDevice 内部和用户侧各做一次点级循环 + using RawCloudCallback = std::function; + virtual ~IRsLidarDevice() = default; /// 工厂方法 @@ -118,11 +139,30 @@ public: /// 注册数据包回调（is_frame_begin 标记帧起始） virtual int SetPacketCallback(PacketCallback callback) = 0; + /// 注册原始点云回调（注册后优先使用，跳过内部 Transform 循环） + virtual int SetRawCloudCallback(RawCloudCallback callback) = 0; + /// 注册异常回调 virtual int SetExceptionCallback(ExceptionCallback callback) = 0; /// 获取驱动版本 virtual std::string GetVersion() = 0; + + /// 获取丢帧计数（m_stuffedQueue 满时被丢弃的帧数） + virtual uint64_t GetDroppedFrameCount() const = 0; + + /// 获取当前就绪队列深度（待消费的帧数） + virtual size_t GetStuffedQueueDepth() const = 0; + + /// 获取指定 SDK ErrCode 的累计触发次数（接受原始 SDK error_code，如 0x40/0x48/0x49/0x42） + virtual uint64_t GetExceptionCount(int errCode) const = 0; + + /// 获取并清零最近一个 5s 滚动窗口内用户回调耗时统计 + /// 注意：内部统计字段每 5s 由 processCloudThread 重置，此接口仅返回当前累计快照 + virtual RsCallbackStats GetCallbackLatencyStats() const = 0; + + /// 获取就绪队列历史峰值深度（Open 以来） + virtual size_t GetStuffedQueuePeak() const = 0; }; #endif // IRSLIDARDEVICE_H diff --git a/Device/RsLidarDevice/RsLidarDevice.pro b/Device/RsLidarDevice/RsLidarDevice.pro index 6c13d5db..a61d694a 100644 --- a/Device/RsLidarDevice/RsLidarDevice.pro +++ b/Device/RsLidarDevice/RsLidarDevice.pro @@ -12,6 +12,9 @@ DEFINES += RSLIDARDEVICE_LIBRARY # 禁用 PCAP 文件回放（不需要 WinPcap/Npcap 依赖），如需 PCAP 功能请移除此行 DEFINES += DISABLE_PCAP_PARSE +# 启用 socket 接收缓冲区调整（setsockopt SO_RCVBUF） +DEFINES += ENABLE_MODIFY_RECVBUF + TARGET = RsLidarDevice # 支持 UTF-8 编码 diff --git a/Device/RsLidarDevice/Src/RsLidarDevice.cpp b/Device/RsLidarDevice/Src/RsLidarDevice.cpp index 6c5d80c3..747820bd 100644 --- a/Device/RsLidarDevice/Src/RsLidarDevice.cpp +++ b/Device/RsLidarDevice/Src/RsLidarDevice.cpp @@ -1,9 +1,28 @@ #include "RsLidarDevice.h" #include +#include +#include // WSAStartup 引用计数 static std::atomic g_sockRefCount{0}; +// ============================================================ +// SDK ErrCode → 统计桶映射 +// SDK error_code.hpp: MSOPTIMEOUT=0x40 WRONGMSOPLEN=0x42 PKTBUFOVERFLOW=0x48 CLOUDOVERFLOW=0x49 +// 桶顺序与日志列对齐：0=msopto 1=pktof 2=cldof 3=wlen 4=other +// ============================================================ +size_t CRsLidarDevice::errCodeToBucket(int errCode) +{ + switch (errCode) + { + case 0x40: return 0; // MSOPTIMEOUT + case 0x48: return 1; // PKTBUFOVERFLOW + case 0x49: return 2; // CLOUDOVERFLOW + case 0x42: return 3; // WRONGMSOPLEN + default: return 4; // other + } +} + // ============================================================ // 工厂方法 // ============================================================ @@ -70,6 +89,11 @@ RSDriverParam CRsLidarDevice::toDriverParam(const RsLidarConfig& config) param.decoder_param.start_angle = config.startAngle; param.decoder_param.end_angle = config.endAngle; param.decoder_param.split_angle = config.splitAngle; + + param.input_param.socket_recv_buf = (config.socketRecvBufBytes > 0) + ? config.socketRecvBufBytes + : 33554432; // 32MB，低配电脑防内核缓冲溢出 + return param; } @@ -154,7 +178,20 @@ int CRsLidarDevice::Start() return 0; } - for (int i = 0; i < 8; ++i) + // 重置诊断计数（每次 Start 开始新会话统计） + m_droppedFrameCount.store(0, std::memory_order_relaxed); + m_totalPushCount.store(0, std::memory_order_relaxed); + for (auto& c : m_exceptionCounts) c.store(0, std::memory_order_relaxed); + m_cbMaxUs.store(0, std::memory_order_relaxed); + m_cbAccumUs.store(0, std::memory_order_relaxed); + m_cbCount.store(0, std::memory_order_relaxed); + m_qPeak.store(0, std::memory_order_relaxed); + + // 清空可能残留的预热缓冲（防止 Stop→Start 循环累积膨胀） + m_freeQueue.clear(); + m_stuffedQueue.clear(); + + for (int i = 0; i < 32; ++i) { m_freeQueue.push(std::make_shared()); } @@ -206,6 +243,13 @@ int CRsLidarDevice::SetPointCloudCallback(PointCloudCallback callback) return 0; } +int CRsLidarDevice::SetRawCloudCallback(RawCloudCallback callback) +{ + std::lock_guard lock(m_callbackMutex); + m_rawCloudCallback = std::move(callback); + return 0; +} + int CRsLidarDevice::SetPacketCallback(PacketCallback callback) { std::lock_guard lock(m_callbackMutex); @@ -225,52 +269,140 @@ std::string CRsLidarDevice::GetVersion() return getDriverVersion(); } +// ============================================================ +// 内部：5s 窗口诊断日志输出 + 字段重置（仅当距上次输出 ≥5s 时实际输出） +// 在 processCloudThread 内部调用，因此对窗口字段的 exchange 是单写者操作 +// ============================================================ +void CRsLidarDevice::emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime) +{ + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast(now - lastLogTime).count() < 5) + { + return; + } + + uint64_t cbCnt = m_cbCount.exchange(0); + uint64_t cbMax = m_cbMaxUs.exchange(0); + uint64_t cbAcc = m_cbAccumUs.exchange(0); + uint64_t cbAvg = (cbCnt > 0) ? (cbAcc / cbCnt) : 0; + uint64_t e0 = m_exceptionCounts[0].exchange(0); + uint64_t e1 = m_exceptionCounts[1].exchange(0); + uint64_t e2 = m_exceptionCounts[2].exchange(0); + uint64_t e3 = m_exceptionCounts[3].exchange(0); + uint64_t e4 = m_exceptionCounts[4].exchange(0); + size_t qPeakSnap = m_qPeak.load(); + + fprintf(stderr, + "[RsLidarDevice] qd=%zu qpeak=%zu drop=%llu push=%llu | " + "cb_us max=%llu avg=%llu n=%llu | " + "exc msopto=%llu pktof=%llu cldof=%llu wlen=%llu other=%llu\n", + static_cast(m_stuffedQueue.size()), + qPeakSnap, + static_cast(m_droppedFrameCount.load()), + static_cast(m_totalPushCount.load()), + static_cast(cbMax), + static_cast(cbAvg), + static_cast(cbCnt), + static_cast(e0), + static_cast(e1), + static_cast(e2), + static_cast(e3), + static_cast(e4)); + lastLogTime = now; +} + // ============================================================ // 内部：点云处理线程 // ============================================================ void CRsLidarDevice::processCloudThread() { + auto lastLogTime = std::chrono::steady_clock::now(); while (!m_bStopProcess) { SdkCloudPtr sdkCloud = m_stuffedQueue.popWait(500000); if (!sdkCloud || sdkCloud->points.empty()) { + // 即使无帧也要按周期打印诊断（确认线程活着 + 异常计数继续流动） + emitDiagnosticLogIfDue(lastLogTime); continue; } - PointCloudCallback cb; + // 每 5 秒打印队列深度、丢帧、回调耗时、异常分类（窗口式重置） + emitDiagnosticLogIfDue(lastLogTime); + + // 优先使用原始回调（零拷贝路径，用户自行在一次遍历中完成转换） + RawCloudCallback rawCb; { std::lock_guard lock(m_callbackMutex); - cb = m_pointCloudCallback; + rawCb = m_rawCloudCallback; } - if (cb) + auto cbStart = std::chrono::steady_clock::now(); + + if (rawCb) { - // 转换并清洗数据: NaN→0, m→mm + RsFrameMeta meta; + meta.seq = sdkCloud->seq; + meta.timestamp = sdkCloud->timestamp; + meta.height = sdkCloud->height; + meta.width = sdkCloud->width; + meta.isDense = sdkCloud->is_dense; + meta.frameId = sdkCloud->frame_id; + const auto& src = sdkCloud->points; - std::vector transformed; - transformed.reserve(src.size()); - for (const auto& s : src) + rawCb(reinterpret_cast(src.data()), + static_cast(src.size()), meta); + } + else + { + PointCloudCallback cb; { - RsPointXYZI p; - p.x = (std::isnan(s.x) ? 0.0f : s.x) * 1000.0f; - p.y = (std::isnan(s.y) ? 0.0f : s.y) * 1000.0f; - p.z = (std::isnan(s.z) ? 0.0f : s.z) * 1000.0f; - p.intensity = s.intensity; - transformed.push_back(p); + std::lock_guard lock(m_callbackMutex); + cb = m_pointCloudCallback; } - RsPointCloudData data; - data.seq = sdkCloud->seq; - data.timestamp = sdkCloud->timestamp; - data.height = sdkCloud->height; - data.width = sdkCloud->width; - data.isDense = sdkCloud->is_dense; - data.frameId = sdkCloud->frame_id; - data.pointCount = static_cast(transformed.size()); - data.points = transformed.data(); + if (cb) + { + // 转换并清洗数据: NaN→0, m→mm + const auto& src = sdkCloud->points; + std::vector transformed; + transformed.reserve(src.size()); + for (const auto& s : src) + { + RsPointXYZI p; + p.x = (std::isnan(s.x) ? 0.0f : s.x) * 1000.0f; + p.y = (std::isnan(s.y) ? 0.0f : s.y) * 1000.0f; + p.z = (std::isnan(s.z) ? 0.0f : s.z) * 1000.0f; + p.intensity = s.intensity; + transformed.push_back(p); + } - cb(data); + RsPointCloudData data; + data.seq = sdkCloud->seq; + data.timestamp = sdkCloud->timestamp; + data.height = sdkCloud->height; + data.width = sdkCloud->width; + data.isDense = sdkCloud->is_dense; + data.frameId = sdkCloud->frame_id; + data.pointCount = static_cast(transformed.size()); + data.points = transformed.data(); + + cb(data); + } + } + + // 累计回调耗时 + auto cbEnd = std::chrono::steady_clock::now(); + uint64_t us = static_cast( + std::chrono::duration_cast(cbEnd - cbStart).count()); + m_cbAccumUs.fetch_add(us, std::memory_order_relaxed); + m_cbCount.fetch_add(1, std::memory_order_relaxed); + // CAS 更新 max + uint64_t prevMax = m_cbMaxUs.load(std::memory_order_relaxed); + while (us > prevMax && + !m_cbMaxUs.compare_exchange_weak(prevMax, us, std::memory_order_relaxed)) + { + // prevMax 已被 compare_exchange_weak 自动更新，循环再试 } m_freeQueue.push(sdkCloud); @@ -289,7 +421,25 @@ CRsLidarDevice::SdkCloudPtr CRsLidarDevice::getFreeCloud() void CRsLidarDevice::putStuffedCloud(SdkCloudPtr msg) { - m_stuffedQueue.push(msg); + m_totalPushCount++; + size_t depth = m_stuffedQueue.push(msg); + if (depth == 0) + { + m_droppedFrameCount++; + // 丢帧时把 SdkCloudMsg 回收到 freeQueue，复用其 points vector 容量 + // (单帧最多 reserve MAX_POINT_CLOUD_SIZE × 13B ≈ 22MB，避免反复分配) + m_freeQueue.push(msg); + } + else + { + // CAS 采样队列峰值（Open 以来） + size_t prevPeak = m_qPeak.load(std::memory_order_relaxed); + while (depth > prevPeak && + !m_qPeak.compare_exchange_weak(prevPeak, depth, std::memory_order_relaxed)) + { + // prevPeak 已被自动更新，循环重试 + } + } } // ============================================================ @@ -317,10 +467,48 @@ void CRsLidarDevice::onPacket(const Packet& pkt) } // ============================================================ -// 内部：Error → RsExceptionInfo +// 监控接口 +// ============================================================ +uint64_t CRsLidarDevice::GetDroppedFrameCount() const +{ + return m_droppedFrameCount.load(); +} + +size_t CRsLidarDevice::GetStuffedQueueDepth() const +{ + return m_stuffedQueue.size(); +} + +uint64_t CRsLidarDevice::GetExceptionCount(int errCode) const +{ + size_t bucket = errCodeToBucket(errCode); + return m_exceptionCounts[bucket].load(std::memory_order_relaxed); +} + +RsCallbackStats CRsLidarDevice::GetCallbackLatencyStats() const +{ + RsCallbackStats stats; + stats.count = m_cbCount.load(std::memory_order_relaxed); + stats.maxUs = m_cbMaxUs.load(std::memory_order_relaxed); + uint64_t acc = m_cbAccumUs.load(std::memory_order_relaxed); + stats.avgUs = (stats.count > 0) ? (acc / stats.count) : 0; + return stats; +} + +size_t CRsLidarDevice::GetStuffedQueuePeak() const +{ + return m_qPeak.load(std::memory_order_relaxed); +} + +// ============================================================ +// 内部：Error → RsExceptionInfo（按 ErrCode 分桶计数后转发） // ============================================================ void CRsLidarDevice::onException(const Error& code) { + // 分桶累加（用于诊断日志和 GetExceptionCount 查询） + size_t bucket = errCodeToBucket(static_cast(code.error_code)); + m_exceptionCounts[bucket].fetch_add(1, std::memory_order_relaxed); + ExceptionCallback cb; { std::lock_guard lock(m_callbackMutex); diff --git a/Device/RsLidarDevice/_Inc/RsLidarDevice.h b/Device/RsLidarDevice/_Inc/RsLidarDevice.h index ba990905..0acea4e7 100644 --- a/Device/RsLidarDevice/_Inc/RsLidarDevice.h +++ b/Device/RsLidarDevice/_Inc/RsLidarDevice.h @@ -31,6 +31,8 @@ inline int recvfrom(SOCKET s, unsigned char* buf, int len, int flags, sockaddr* #include #include #include +#include +#include using namespace robosense::lidar; @@ -50,11 +52,18 @@ public: bool IsRunning() const override; int SetPointCloudCallback(PointCloudCallback callback) override; + int SetRawCloudCallback(RawCloudCallback callback) override; int SetPacketCallback(PacketCallback callback) override; int SetExceptionCallback(ExceptionCallback callback) override; std::string GetVersion() override; + uint64_t GetDroppedFrameCount() const override; + size_t GetStuffedQueueDepth() const override; + uint64_t GetExceptionCount(int errCode) const override; + RsCallbackStats GetCallbackLatencyStats() const override; + size_t GetStuffedQueuePeak() const override; + private: // SDK 内部类型别名 using SdkCloudMsg = PointCloudT; @@ -62,6 +71,9 @@ private: void processCloudThread(); + /// 输出 5s 窗口诊断日志并重置窗口字段；仅当距 lastLogTime ≥5s 时实际输出 + void emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime); + SdkCloudPtr getFreeCloud(); void putStuffedCloud(SdkCloudPtr msg); @@ -79,6 +91,7 @@ private: std::atomic m_bStopProcess{false}; PointCloudCallback m_pointCloudCallback; + RawCloudCallback m_rawCloudCallback; PacketCallback m_packetCallback; ExceptionCallback m_exceptionCallback; std::mutex m_callbackMutex; @@ -86,10 +99,26 @@ private: std::unique_ptr> m_pDriver; std::thread m_processThread; - SyncQueue m_freeQueue; - SyncQueue m_stuffedQueue; + SyncQueue m_freeQueue; + SyncQueue m_stuffedQueue; RSDriverParam m_param; + + // 丢帧监控 + std::atomic m_droppedFrameCount{0}; + std::atomic m_totalPushCount{0}; + + // 诊断统计（每 5s 滚动窗口） + // bucket: 0=MSOPTIMEOUT(0x40) 1=PKTBUFOVERFLOW(0x48) 2=CLOUDOVERFLOW(0x49) 3=WRONGMSOPLEN(0x42) 4=other + static constexpr size_t EXC_BUCKET_COUNT = 5; + std::array, EXC_BUCKET_COUNT> m_exceptionCounts{}; + std::atomic m_cbMaxUs{0}; + std::atomic m_cbAccumUs{0}; + std::atomic m_cbCount{0}; + std::atomic m_qPeak{0}; + + /// 将 SDK ErrCode 映射到统计桶下标，未知返回 4 (other) + static size_t errCodeToBucket(int errCode); }; #endif // CRSLIDARDEVICE_H diff --git a/SDK/Lidar/rslidar/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp b/SDK/Lidar/rslidar/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp index 524ba38d..5001de37 100644 --- a/SDK/Lidar/rslidar/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp +++ b/SDK/Lidar/rslidar/rs_driver/src/rs_driver/driver/input/win/input_sock_select.hpp @@ -214,7 +214,7 @@ inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, con } socklen_t opt_len = sizeof(uint32_t); // get original value - if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &before_set_val, &opt_len) == -1) + if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, reinterpret_cast(&before_set_val), &opt_len) == -1) { perror("getsockopt before"); return -1; @@ -222,14 +222,14 @@ inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, con RS_INFO << "Original receive buffer size: " << before_set_val << " bytes" << RS_REND; // set new value - if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &opt_val, opt_len) == -1) + if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, reinterpret_cast(&opt_val), opt_len) == -1) { perror("setsockopt"); return -1; } // get new value - if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &after_set_val, &opt_len) == -1) + if (getsockopt(fd, SOL_SOCKET, SO_RCVBUF, reinterpret_cast(&after_set_val), &opt_len) == -1) { perror("getsockopt after"); return -1; @@ -291,34 +291,47 @@ inline void InputSock::recvPacket() perror("select: "); break; } - int ret = 0; for (int i = 0; i < 3; i++) { if ((fds_[i] >= 0) && FD_ISSET(fds_[i], &rfds)) { - std::shared_ptr pkt = cb_get_pkt_(pkt_buf_len_); - try + // 批量读取：socket 已设为非阻塞，循环 recvfrom 直到内核缓冲区清空， + // 避免每读一个包就重新 select() 一次，大幅减少系统调用开销 + while (true) { - ret = recvfrom(fds_[i], pkt->buf(), pkt->bufSize(), 0, NULL, NULL); - } - catch (const std::exception& e) - { - RS_ERROR << "fd[" << i << "] , recvfrom exception:" << e.what() << RS_REND; - continue; - } + std::shared_ptr pkt = cb_get_pkt_(pkt_buf_len_); + int ret = 0; + try + { + ret = recvfrom(fds_[i], pkt->buf(), pkt->bufSize(), 0, NULL, NULL); + } + catch (const std::exception& e) + { + RS_ERROR << "fd[" << i << "] , recvfrom exception:" << e.what() << RS_REND; + break; + } - if (ret < 0) - { - perror("recvfrom: "); - break; - } - else if (ret > 0) - { - pkt->setData(sock_offset_, ret - sock_offset_ - sock_tail_); - pushPacket(pkt); + if (ret < 0) + { +#ifdef _WIN32 + if (WSAGetLastError() == WSAEWOULDBLOCK) + break; // 内核缓冲区已清空，回到 select 等待下一批 +#else + if (errno == EAGAIN || errno == EWOULDBLOCK) + break; +#endif + perror("recvfrom: "); + goto nextSelect; + } + else if (ret > 0) + { + pkt->setData(sock_offset_, ret - sock_offset_ - sock_tail_); + pushPacket(pkt); + } } } } + nextSelect: ; } } diff --git a/Test/RsLidarTest/main.cpp b/Test/RsLidarTest/main.cpp index cd0ac15a..dcc3e3bd 100644 --- a/Test/RsLidarTest/main.cpp +++ b/Test/RsLidarTest/main.cpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include #include @@ -38,6 +40,31 @@ static bool g_bDisplay = true; static std::string g_saveDir; static std::atomic g_nSaveSeq{0}; static std::unique_ptr g_pCloudShow; +static IRsLidarDevice* g_pDevice = nullptr; + +// ============================================================ +// 异步保存：OnRawCloud 仅入队，独立线程消费 SaveToTxt +// 队列上限 8，满则丢最老一帧并计数（离线场景丢老优于阻塞） +// ============================================================ +struct SaveTask +{ + int seq = 0; + pcl::PointCloud::Ptr cloud; +}; + +static std::mutex g_saveMutex; +static std::condition_variable g_saveCv; +static std::deque g_saveQueue; +static std::atomic g_saveExit{false}; +static std::atomic g_saveDropped{0}; +static constexpr size_t SAVE_QUEUE_MAX = 8; + +// ============================================================ +// 显示限速：UpdateCloud 间隔 ≥200ms (~5fps)，保存路径不限速 +// ============================================================ +static constexpr int DISPLAY_INTERVAL_MS = 200; +static std::chrono::steady_clock::time_point g_lastDisplayTime; +static std::mutex g_displayTimeMutex; static void PrintLog(const std::string& message) { @@ -66,41 +93,48 @@ static void OnPacket(const RsPacketInfo& pkt) } } -/// 每收到一帧完整点云触发 — 可在此做后处理 -static void OnPointCloud(const RsPointCloudData& data) +/// 每收到一帧原始点云触发 — 在单次遍历中完成 NaN 清洗 + m→mm 转换 + PCL 格式拷贝 +static void OnRawCloud(const RsPointXYZI* points, uint32_t pointCount, const RsFrameMeta& meta) { - if (data.pointCount == 0) return; + if (pointCount == 0) return; g_nFrameCount++; - g_nTotalPoints += data.pointCount; + g_nTotalPoints += pointCount; if (g_bVerbose) { - const auto& first = data.points[0]; - const auto& last = data.points[data.pointCount - 1]; + const auto& first = points[0]; + const auto& last = points[pointCount - 1]; + + float fx = (std::isnan(first.x) ? 0.0f : first.x) * 1000.0f; + float fy = (std::isnan(first.y) ? 0.0f : first.y) * 1000.0f; + float fz = (std::isnan(first.z) ? 0.0f : first.z) * 1000.0f; + float lx = (std::isnan(last.x) ? 0.0f : last.x) * 1000.0f; + float ly = (std::isnan(last.y) ? 0.0f : last.y) * 1000.0f; + float lz = (std::isnan(last.z) ? 0.0f : last.z) * 1000.0f; std::ostringstream oss; - oss << " └─ 完整帧 #" << data.seq - << " | " << data.pointCount << "点" - << " | ts=" << std::fixed << std::setprecision(3) << data.timestamp - << " | (" << first.x << "," << first.y << "," << first.z << ")" - << "→(" << last.x << "," << last.y << "," << last.z << ")"; + oss << " └─ 完整帧 #" << meta.seq + << " | " << pointCount << "点" + << " | ts=" << std::fixed << std::setprecision(3) << meta.timestamp + << " | (" << fx << "," << fy << "," << fz << ")" + << "→(" << lx << "," << ly << "," << lz << ")"; PrintLog(oss.str()); } - // 转换为 PCL 格式 — RsPointXYZI(packed 13B) 与 pcl::PointXYZI(20B) 布局不同，需逐字段拷贝 - // NaN→0 和 m→mm 已在 RsLidarDevice 层处理 + // 单次遍历：NaN→0 + m→mm + 近零点→NaN + PCL 格式填充 auto pclCloud = std::make_shared>(); - pclCloud->height = data.height; - pclCloud->width = data.width; - pclCloud->is_dense = data.isDense; - pclCloud->points.resize(data.pointCount); + pclCloud->height = meta.height; + pclCloud->width = meta.width; + pclCloud->is_dense = meta.isDense; + pclCloud->points.resize(pointCount); const float nan = std::numeric_limits::quiet_NaN(); - for (size_t i = 0; i < data.pointCount; ++i) + for (uint32_t i = 0; i < pointCount; ++i) { - float x = data.points[i].x; - float y = data.points[i].y; - float z = data.points[i].z; + float x = (std::isnan(points[i].x) ? 0.0f : points[i].x) * 1000.0f; + float y = (std::isnan(points[i].y) ? 0.0f : points[i].y) * 1000.0f; + float z = (std::isnan(points[i].z) ? 0.0f : points[i].z) * 1000.0f; + if (std::abs(x) < 0.005f && std::abs(y) < 0.005f && std::abs(z) < 0.005f) { pclCloud->points[i].x = nan; @@ -114,21 +148,29 @@ static void OnPointCloud(const RsPointCloudData& data) pclCloud->points[i].y = y; pclCloud->points[i].z = z; } - pclCloud->points[i].intensity = static_cast(data.points[i].intensity); + pclCloud->points[i].intensity = static_cast(points[i].intensity); } - // 持续保存到目录（--save dir） + // 持续保存到目录（--save dir）：仅入队，由 SaveLoop 异步写 if (!g_saveDir.empty() && g_pCloudShow) { - int seq = ++g_nSaveSeq; - std::ostringstream oss; - oss << g_saveDir << "/LaserData_" << seq << ".txt"; - PrintLog("开始存储: " + oss.str()); - g_pCloudShow->SaveToTxt(oss.str(), pclCloud); - PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast(data.pointCount)) + "点"); + SaveTask task; + task.seq = ++g_nSaveSeq; + task.cloud = pclCloud; + + { + std::lock_guard lk(g_saveMutex); + if (g_saveQueue.size() >= SAVE_QUEUE_MAX) + { + g_saveQueue.pop_front(); // 丢最老 + g_saveDropped.fetch_add(1, std::memory_order_relaxed); + } + g_saveQueue.emplace_back(std::move(task)); + } + g_saveCv.notify_one(); } - // 单次保存（PCL 窗口按键 S/s 触发） + // 单次保存（PCL 窗口按键 S/s 触发）— 同步直接写，单次操作不影响吞吐 if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsSaveRequested()) { g_pCloudShow->ClearSaveRequest(); @@ -138,13 +180,26 @@ static void OnPointCloud(const RsPointCloudData& data) oss << dir << "/LaserData_" << seq << ".txt"; PrintLog("开始存储: " + oss.str()); g_pCloudShow->SaveToTxt(oss.str(), pclCloud); - PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast(data.pointCount)) + "点"); + PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast(pointCount)) + "点"); } - // 推送点云到显示窗口 + // 推送点云到显示窗口（限速 ~5fps，避免 PCL/VTK 渲染压力反传） if (g_bDisplay && g_pCloudShow) { - g_pCloudShow->UpdateCloud(pclCloud); + auto now = std::chrono::steady_clock::now(); + bool shouldShow = false; + { + std::lock_guard lk(g_displayTimeMutex); + if (std::chrono::duration_cast(now - g_lastDisplayTime).count() >= DISPLAY_INTERVAL_MS) + { + g_lastDisplayTime = now; + shouldShow = true; + } + } + if (shouldShow) + { + g_pCloudShow->UpdateCloud(pclCloud); + } } } @@ -153,6 +208,35 @@ static void OnException(const RsExceptionInfo& info) PrintLog("[异常 " + std::to_string(info.code) + "] " + info.message); } +// 异步保存线程：从 g_saveQueue 取任务调 SaveToTxt +// 队列空时 wait，g_saveExit 标记后 flush 剩余帧再退出 +static void SaveLoop() +{ + while (true) + { + SaveTask task; + { + std::unique_lock lk(g_saveMutex); + g_saveCv.wait(lk, [] { return g_saveExit.load() || !g_saveQueue.empty(); }); + if (g_saveQueue.empty()) + { + if (g_saveExit.load()) return; + continue; + } + task = std::move(g_saveQueue.front()); + g_saveQueue.pop_front(); + } + + std::ostringstream oss; + oss << g_saveDir << "/LaserData_" << task.seq << ".txt"; + g_pCloudShow->SaveToTxt(oss.str(), task.cloud); + if (g_bVerbose) + { + PrintLog("存储完成: " + oss.str()); + } + } +} + static void StatsLoop() { while (!g_bExit) @@ -171,6 +255,14 @@ static void StatsLoop() << (static_cast(g_nFrameCount) / elapsed) << " | pps=" << std::fixed << std::setprecision(0) << (static_cast(g_nTotalPoints) / elapsed); + + // 应用层累计指标（窗口型字段由 Device 自身 5s stderr 日志输出，避免重复+竞态） + if (g_pDevice) + { + oss << " | drop=" << g_pDevice->GetDroppedFrameCount() + << " save_drop=" << g_saveDropped.load(); + } + PrintLog(oss.str()); } } @@ -187,7 +279,8 @@ static void PrintUsage(const char* prog) std::cout << " --time 运行时长 (默认: 30, 0=持续)" << std::endl; std::cout << " --verbose 逐帧打印扫描生命周期" << std::endl; std::cout << " --display 实时点云显示（非阻塞，PCL窗口 S=保存 Q=退出）" << std::endl; - std::cout << " --save 保存点云到指定目录（frame_.txt）" << std::endl; + std::cout << " --save 保存点云到指定目录（frame_.txt，异步写）" << std::endl; + std::cout << " --recvbuf socket 接收缓冲字节数 (默认 33554432 = 32MB)" << std::endl; std::cout << std::endl; std::cout << "雷达型号: RS16 RS32 RSBP RSAIRY RSHELIOS RS128 RS80 RS48 RSM1 RSM2 RSM3 等" << std::endl; std::cout << std::endl; @@ -227,6 +320,8 @@ int main(int argc, char* argv[]) g_bDisplay = true; else if (arg == "--save" && i + 1 < argc) g_saveDir = argv[++i]; + else if (arg == "--recvbuf" && i + 1 < argc) + config.socketRecvBufBytes = static_cast(std::stoul(argv[++i])); } std::cout << "============================================" << std::endl; @@ -237,6 +332,7 @@ int main(int argc, char* argv[]) std::cout << "端口: MSOP=" << config.msopPort << " DIFOP=" << config.difopPort << std::endl; std::cout << "时长: " << (runSeconds > 0 ? std::to_string(runSeconds) + "s" : "持续") << std::endl; std::cout << "详细: " << (g_bVerbose ? "ON" : "OFF") << std::endl; + std::cout << "RecvBuf: " << (config.socketRecvBufBytes / (1024 * 1024)) << "MB" << std::endl; std::cout << "============================================" << std::endl; // 1. 创建 @@ -246,6 +342,7 @@ int main(int argc, char* argv[]) std::cerr << "创建设备失败" << std::endl; return -1; } + g_pDevice = pDevice; // 供 StatsLoop 查询诊断接口 PrintLog("设备实例创建完成"); PrintLog("驱动版本: " + pDevice->GetVersion()); @@ -271,9 +368,9 @@ int main(int argc, char* argv[]) } PrintLog("设备打开完成"); - // 4. 注册回调 + // 4. 注册回调（原始回调—单次遍历完成全部处理） pDevice->SetPacketCallback(OnPacket); - pDevice->SetPointCloudCallback(OnPointCloud); + pDevice->SetRawCloudCallback(OnRawCloud); pDevice->SetExceptionCallback(OnException); // 5. 启动 @@ -287,8 +384,14 @@ int main(int argc, char* argv[]) g_startTime = std::chrono::steady_clock::now(); PrintLog("数据采集已启动"); - // 6. 统计线程 + // 6. 统计线程 + 异步保存线程 std::thread statsThread(StatsLoop); + std::thread saveThread; + if (!g_saveDir.empty()) + { + saveThread = std::thread(SaveLoop); + PrintLog("异步保存线程已启动 -> " + g_saveDir); + } // 7. 启动显示窗口（主线程创建 viewer + 驱动渲染） if (g_bDisplay && g_pCloudShow) @@ -339,6 +442,16 @@ int main(int argc, char* argv[]) pDevice->Stop(); if (statsThread.joinable()) statsThread.join(); + // 通知保存线程退出（flush 剩余 ≤8 帧） + if (saveThread.joinable()) + { + g_saveExit = true; + g_saveCv.notify_all(); + PrintLog("等待保存线程 flush 剩余帧..."); + saveThread.join(); + PrintLog("保存线程退出"); + } + auto endTime = std::chrono::steady_clock::now(); auto totalElapsed = std::chrono::duration_cast(endTime - g_startTime).count(); if (totalElapsed == 0) totalElapsed = 1; @@ -355,8 +468,18 @@ int main(int argc, char* argv[]) << (static_cast(g_nFrameCount) / totalElapsed) << " fps" << std::endl; std::cout << "点率: " << std::fixed << std::setprecision(0) << (static_cast(g_nTotalPoints) / totalElapsed) << " pts/s" << std::endl; + std::cout << "预期帧数: " << (totalElapsed * 10) << std::endl; + std::cout << "丢失率: " << std::fixed << std::setprecision(1) + << (100.0 * (totalElapsed * 10 - g_nFrameCount) / (totalElapsed * 10)) << "%" << std::endl; + std::cout << "队列丢帧: " << pDevice->GetDroppedFrameCount() << std::endl; + std::cout << "队列峰值: " << pDevice->GetStuffedQueuePeak() << std::endl; + std::cout << "保存丢帧: " << g_saveDropped.load() << std::endl; + std::cout << "异常 msopto(0x40): " << pDevice->GetExceptionCount(0x40) << std::endl; + std::cout << "异常 pktof(0x48): " << pDevice->GetExceptionCount(0x48) << std::endl; + std::cout << "异常 cldof(0x49): " << pDevice->GetExceptionCount(0x49) << std::endl; std::cout << "============================================" << std::endl; + g_pDevice = nullptr; pDevice->CloseDevice(); delete pDevice;