优化了雷达网络接受
This commit is contained in:
杰仔
parent
ab9d65ec3c
commit
3eea68772b
@ -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<void(const RsPacketInfo&)>;
|
||||
using ExceptionCallback = std::function<void(const RsExceptionInfo&)>;
|
||||
|
||||
/// 原始点云回调：提供未经转换的 SDK 原始点（含 NaN，单位米），
|
||||
/// 用户可在一次遍历中完成 NaN 清洗 + 单位转换 + 格式复制，
|
||||
/// 避免 RsLidarDevice 内部和用户侧各做一次点级循环
|
||||
using RawCloudCallback = std::function<void(const RsPointXYZI* points, uint32_t pointCount, const RsFrameMeta& meta)>;
|
||||
|
||||
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
|
||||
|
||||
@ -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 编码
|
||||
|
||||
@ -1,9 +1,28 @@
|
||||
#include "RsLidarDevice.h"
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <chrono>
|
||||
|
||||
// WSAStartup 引用计数
|
||||
static std::atomic<int> 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<SdkCloudMsg>());
|
||||
}
|
||||
@ -206,6 +243,13 @@ int CRsLidarDevice::SetPointCloudCallback(PointCloudCallback callback)
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CRsLidarDevice::SetRawCloudCallback(RawCloudCallback callback)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||||
m_rawCloudCallback = std::move(callback);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CRsLidarDevice::SetPacketCallback(PacketCallback callback)
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<std::chrono::seconds>(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<size_t>(m_stuffedQueue.size()),
|
||||
qPeakSnap,
|
||||
static_cast<unsigned long long>(m_droppedFrameCount.load()),
|
||||
static_cast<unsigned long long>(m_totalPushCount.load()),
|
||||
static_cast<unsigned long long>(cbMax),
|
||||
static_cast<unsigned long long>(cbAvg),
|
||||
static_cast<unsigned long long>(cbCnt),
|
||||
static_cast<unsigned long long>(e0),
|
||||
static_cast<unsigned long long>(e1),
|
||||
static_cast<unsigned long long>(e2),
|
||||
static_cast<unsigned long long>(e3),
|
||||
static_cast<unsigned long long>(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<std::mutex> 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<RsPointXYZI> transformed;
|
||||
transformed.reserve(src.size());
|
||||
for (const auto& s : src)
|
||||
rawCb(reinterpret_cast<const RsPointXYZI*>(src.data()),
|
||||
static_cast<uint32_t>(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<std::mutex> 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<uint32_t>(transformed.size());
|
||||
data.points = transformed.data();
|
||||
if (cb)
|
||||
{
|
||||
// 转换并清洗数据: NaN→0, m→mm
|
||||
const auto& src = sdkCloud->points;
|
||||
std::vector<RsPointXYZI> 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<uint32_t>(transformed.size());
|
||||
data.points = transformed.data();
|
||||
|
||||
cb(data);
|
||||
}
|
||||
}
|
||||
|
||||
// 累计回调耗时
|
||||
auto cbEnd = std::chrono::steady_clock::now();
|
||||
uint64_t us = static_cast<uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::microseconds>(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<int>(code.error_code));
|
||||
m_exceptionCounts[bucket].fetch_add(1, std::memory_order_relaxed);
|
||||
|
||||
ExceptionCallback cb;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||||
|
||||
@ -31,6 +31,8 @@ inline int recvfrom(SOCKET s, unsigned char* buf, int len, int flags, sockaddr*
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
|
||||
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<PointXYZI>;
|
||||
@ -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<bool> 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<LidarDriver<SdkCloudMsg>> m_pDriver;
|
||||
std::thread m_processThread;
|
||||
|
||||
SyncQueue<SdkCloudPtr, 64> m_freeQueue;
|
||||
SyncQueue<SdkCloudPtr, 64> m_stuffedQueue;
|
||||
SyncQueue<SdkCloudPtr, 4096> m_freeQueue;
|
||||
SyncQueue<SdkCloudPtr, 4096> m_stuffedQueue;
|
||||
|
||||
RSDriverParam m_param;
|
||||
|
||||
// 丢帧监控
|
||||
std::atomic<uint64_t> m_droppedFrameCount{0};
|
||||
std::atomic<uint64_t> 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<std::atomic<uint64_t>, EXC_BUCKET_COUNT> m_exceptionCounts{};
|
||||
std::atomic<uint64_t> m_cbMaxUs{0};
|
||||
std::atomic<uint64_t> m_cbAccumUs{0};
|
||||
std::atomic<uint64_t> m_cbCount{0};
|
||||
std::atomic<size_t> m_qPeak{0};
|
||||
|
||||
/// 将 SDK ErrCode 映射到统计桶下标，未知返回 4 (other)
|
||||
static size_t errCodeToBucket(int errCode);
|
||||
};
|
||||
|
||||
#endif // CRSLIDARDEVICE_H
|
||||
|
||||
@ -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<char*>(&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<const char*>(&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<char*>(&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<Buffer> 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<Buffer> 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: ;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -4,6 +4,8 @@
|
||||
#include <chrono>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <deque>
|
||||
#include <string>
|
||||
#include <ctime>
|
||||
#include <sstream>
|
||||
@ -38,6 +40,31 @@ static bool g_bDisplay = true;
|
||||
static std::string g_saveDir;
|
||||
static std::atomic<int> g_nSaveSeq{0};
|
||||
static std::unique_ptr<ICloudShow> g_pCloudShow;
|
||||
static IRsLidarDevice* g_pDevice = nullptr;
|
||||
|
||||
// ============================================================
|
||||
// 异步保存：OnRawCloud 仅入队，独立线程消费 SaveToTxt
|
||||
// 队列上限 8，满则丢最老一帧并计数（离线场景丢老优于阻塞）
|
||||
// ============================================================
|
||||
struct SaveTask
|
||||
{
|
||||
int seq = 0;
|
||||
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
|
||||
};
|
||||
|
||||
static std::mutex g_saveMutex;
|
||||
static std::condition_variable g_saveCv;
|
||||
static std::deque<SaveTask> g_saveQueue;
|
||||
static std::atomic<bool> g_saveExit{false};
|
||||
static std::atomic<uint64_t> 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<pcl::PointCloud<pcl::PointXYZI>>();
|
||||
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<float>::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<float>(data.points[i].intensity);
|
||||
pclCloud->points[i].intensity = static_cast<float>(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<int>(data.pointCount)) + "点");
|
||||
SaveTask task;
|
||||
task.seq = ++g_nSaveSeq;
|
||||
task.cloud = pclCloud;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<int>(data.pointCount)) + "点");
|
||||
PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast<int>(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<std::mutex> lk(g_displayTimeMutex);
|
||||
if (std::chrono::duration_cast<std::chrono::milliseconds>(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<std::mutex> 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<double>(g_nFrameCount) / elapsed)
|
||||
<< " | pps=" << std::fixed << std::setprecision(0)
|
||||
<< (static_cast<double>(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 <seconds> 运行时长 (默认: 30, 0=持续)" << std::endl;
|
||||
std::cout << " --verbose 逐帧打印扫描生命周期" << std::endl;
|
||||
std::cout << " --display 实时点云显示（非阻塞，PCL窗口 S=保存 Q=退出）" << std::endl;
|
||||
std::cout << " --save <dir> 保存点云到指定目录（frame_<seq>.txt）" << std::endl;
|
||||
std::cout << " --save <dir> 保存点云到指定目录（frame_<seq>.txt，异步写）" << std::endl;
|
||||
std::cout << " --recvbuf <bytes> 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<unsigned int>(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<std::chrono::seconds>(endTime - g_startTime).count();
|
||||
if (totalElapsed == 0) totalElapsed = 1;
|
||||
@ -355,8 +468,18 @@ int main(int argc, char* argv[])
|
||||
<< (static_cast<double>(g_nFrameCount) / totalElapsed) << " fps" << std::endl;
|
||||
std::cout << "点率: " << std::fixed << std::setprecision(0)
|
||||
<< (static_cast<double>(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;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user