584 lines
18 KiB
C++
584 lines
18 KiB
C++
#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
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 工厂方法
|
||
// ============================================================
|
||
int IRsLidarDevice::CreateObject(IRsLidarDevice** ppDevice)
|
||
{
|
||
if (!ppDevice) return -1;
|
||
CRsLidarDevice* p = new CRsLidarDevice();
|
||
*ppDevice = p;
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// 构造 / 析构
|
||
// ============================================================
|
||
CRsLidarDevice::CRsLidarDevice()
|
||
: m_pDriver(std::make_unique<LidarDriver<SdkCloudMsg>>())
|
||
{
|
||
}
|
||
|
||
CRsLidarDevice::~CRsLidarDevice()
|
||
{
|
||
Stop();
|
||
CloseDevice();
|
||
}
|
||
|
||
// ============================================================
|
||
// InitDevice
|
||
// ============================================================
|
||
int CRsLidarDevice::InitDevice()
|
||
{
|
||
#ifdef _WIN32
|
||
if (g_sockRefCount == 0)
|
||
{
|
||
WSADATA wsaData;
|
||
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
|
||
{
|
||
return -1;
|
||
}
|
||
}
|
||
g_sockRefCount++;
|
||
#endif
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// 类型转换: RsLidarConfig → RSDriverParam
|
||
// ============================================================
|
||
RSDriverParam CRsLidarDevice::toDriverParam(const RsLidarConfig& config)
|
||
{
|
||
RSDriverParam param;
|
||
param.lidar_type = strToLidarType(config.lidarType);
|
||
param.input_type = InputType::ONLINE_LIDAR;
|
||
param.frame_id = config.frameId;
|
||
param.input_param.host_address = config.hostAddress;
|
||
param.input_param.msop_port = config.msopPort;
|
||
param.input_param.difop_port = config.difopPort;
|
||
param.input_param.imu_port = config.imuPort;
|
||
param.decoder_param.min_distance = config.minDistance;
|
||
param.decoder_param.max_distance = config.maxDistance;
|
||
param.decoder_param.dense_points = config.densePoints;
|
||
param.decoder_param.use_lidar_clock = config.useLidarClock;
|
||
param.decoder_param.ts_first_point = config.tsFirstPoint;
|
||
param.decoder_param.wait_for_difop = config.waitForDifop;
|
||
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;
|
||
}
|
||
|
||
// ============================================================
|
||
// OpenDevice
|
||
// ============================================================
|
||
int CRsLidarDevice::OpenDevice(const RsLidarConfig& config)
|
||
{
|
||
if (!m_pDriver)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
CloseDevice();
|
||
|
||
m_param = toDriverParam(config);
|
||
|
||
m_pDriver->regPointCloudCallback(
|
||
[this]() -> SdkCloudPtr { return this->getFreeCloud(); },
|
||
[this](SdkCloudPtr msg) { this->putStuffedCloud(msg); }
|
||
);
|
||
|
||
m_pDriver->regPacketCallback(
|
||
[this](const Packet& pkt) { this->onPacket(pkt); }
|
||
);
|
||
|
||
m_pDriver->regExceptionCallback(
|
||
[this](const Error& code) { this->onException(code); }
|
||
);
|
||
|
||
if (!m_pDriver->init(m_param))
|
||
{
|
||
return -2;
|
||
}
|
||
|
||
m_bOpened = true;
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// CloseDevice
|
||
// ============================================================
|
||
int CRsLidarDevice::CloseDevice()
|
||
{
|
||
if (m_bRunning)
|
||
{
|
||
Stop();
|
||
}
|
||
|
||
m_bOpened = false;
|
||
|
||
m_freeQueue.clear();
|
||
m_stuffedQueue.clear();
|
||
m_frameFreeQueue.clear();
|
||
m_deliveryQueue.clear();
|
||
|
||
#ifdef _WIN32
|
||
if (--g_sockRefCount == 0)
|
||
{
|
||
WSACleanup();
|
||
}
|
||
#endif
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool CRsLidarDevice::IsOpened() const
|
||
{
|
||
return m_bOpened;
|
||
}
|
||
|
||
// ============================================================
|
||
// Start / Stop
|
||
// ============================================================
|
||
int CRsLidarDevice::Start()
|
||
{
|
||
if (!m_bOpened || !m_pDriver)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
if (m_bRunning)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
// 重置诊断计数（每次 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)
|
||
{
|
||
auto msg = std::make_shared<SdkCloudMsg>();
|
||
msg->points.reserve(500000); // 预分配 SDK 内部缓冲，避免 SDK push_back 反复 realloc
|
||
m_freeQueue.push(std::move(msg));
|
||
}
|
||
|
||
m_bStopProcess = false;
|
||
|
||
// 预分配交付帧：3 帧 × 600 线 × 1500 点 × 32B ≈ 86MB
|
||
// 帧 0=解析线程填充中 帧 1=交付队列中 帧 2=空闲备用
|
||
m_deliveryQueue.clear();
|
||
m_frameFreeQueue.clear();
|
||
for (int i = 0; i < 3; ++i)
|
||
{
|
||
auto frame = std::make_shared<DeliveryFrame>();
|
||
frame->reserveLines(600, 1500);
|
||
m_frameFreeQueue.push(frame);
|
||
}
|
||
|
||
m_pDriver->start();
|
||
|
||
m_bRunning = true;
|
||
m_bStopDelivery = false;
|
||
m_deliveryThread = std::thread(&CRsLidarDevice::deliveryThread, this);
|
||
m_processThread = std::thread(&CRsLidarDevice::processCloudThread, this);
|
||
|
||
return 0;
|
||
}
|
||
|
||
int CRsLidarDevice::Stop()
|
||
{
|
||
if (!m_bRunning)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
// ① 停止解析线程
|
||
m_bStopProcess = true;
|
||
|
||
if (m_pDriver)
|
||
{
|
||
m_pDriver->stop();
|
||
}
|
||
|
||
if (m_processThread.joinable())
|
||
{
|
||
m_processThread.join();
|
||
}
|
||
|
||
// ② 停止交付线程（推入空帧唤醒 popWait，避免等超时）
|
||
m_bStopDelivery = true;
|
||
m_deliveryQueue.push(std::make_shared<DeliveryFrame>());
|
||
if (m_deliveryThread.joinable())
|
||
{
|
||
m_deliveryThread.join();
|
||
}
|
||
|
||
// ③ 释放所有队列内存
|
||
m_deliveryQueue.clear();
|
||
m_frameFreeQueue.clear();
|
||
|
||
m_bRunning = false;
|
||
return 0;
|
||
}
|
||
|
||
bool CRsLidarDevice::IsRunning() const
|
||
{
|
||
return m_bRunning;
|
||
}
|
||
|
||
// ============================================================
|
||
// 回调注册
|
||
// ============================================================
|
||
int CRsLidarDevice::SetPointCloudCallback(PointCloudCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_pointCloudCallback = std::move(callback);
|
||
return 0;
|
||
}
|
||
|
||
int CRsLidarDevice::SetPacketCallback(PacketCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_packetCallback = std::move(callback);
|
||
return 0;
|
||
}
|
||
|
||
int CRsLidarDevice::SetExceptionCallback(ExceptionCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_exceptionCallback = std::move(callback);
|
||
return 0;
|
||
}
|
||
|
||
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();
|
||
#if 0
|
||
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));
|
||
#endif
|
||
lastLogTime = now;
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：点云解析线程（只做数据处理，不调回调）
|
||
// 处理完一帧 → 推入 m_deliveryQueue → 立刻取下一帧解析
|
||
// 回调（PushFrame memcpy）在 deliveryThread 中并行执行
|
||
// ============================================================
|
||
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;
|
||
}
|
||
|
||
emitDiagnosticLogIfDue(lastLogTime);
|
||
|
||
// 获取空闲交付帧（超时 100ms 允许检查 m_bStopProcess）
|
||
auto frame = m_frameFreeQueue.popWait(100000);
|
||
if (!frame)
|
||
{
|
||
if (m_bStopProcess) break;
|
||
continue;
|
||
}
|
||
|
||
// ① NaN→0 + ② 坐标系修正(X=-SDK_Y,Y=-SDK_Z,Z=SDK_X) + ③ 米→毫米(×1000) + ④ 拆线
|
||
const auto& src = sdkCloud->points;
|
||
const uint32_t N = static_cast<uint32_t>(src.size());
|
||
const uint32_t H = sdkCloud->height > 0 ? sdkCloud->height : 1;
|
||
const uint32_t W = (H > 1 && sdkCloud->width > 0) ? sdkCloud->width : N;
|
||
|
||
if (frame->linePool.size() < H)
|
||
frame->linePool.resize(H);
|
||
|
||
auto& cloudData = frame->cloudData;
|
||
cloudData.clear();
|
||
cloudData.reserve(H);
|
||
bool allValid = sdkCloud->is_dense;
|
||
|
||
for (uint32_t line = 0; line < H; ++line)
|
||
{
|
||
uint32_t start = line * W;
|
||
uint32_t end = (line + 1 == H) ? N : (start + W);
|
||
if (end <= start) continue;
|
||
|
||
uint32_t cnt = end - start;
|
||
auto& linePts = frame->linePool[line];
|
||
linePts.resize(cnt);
|
||
|
||
for (uint32_t j = 0; j < cnt; ++j)
|
||
{
|
||
uint32_t i = start + j;
|
||
float sx = std::isnan(src[i].x) ? 0.0f : src[i].x;
|
||
float sy = std::isnan(src[i].y) ? 0.0f : src[i].y;
|
||
float sz = std::isnan(src[i].z) ? 0.0f : src[i].z;
|
||
linePts[j].fData[0] = -sy * 1000.0f;
|
||
linePts[j].fData[1] = -sz * 1000.0f;
|
||
linePts[j].fData[2] = sx * 1000.0f;
|
||
linePts[j].fData_c[0] = static_cast<float>(src[i].intensity);
|
||
if (std::isnan(src[i].x) || std::isnan(src[i].y) || std::isnan(src[i].z))
|
||
allValid = false;
|
||
}
|
||
|
||
SVzLaserLineData ld{};
|
||
ld.p3DPoint = linePts.data();
|
||
ld.nPointCount = static_cast<int>(cnt);
|
||
ld.llFrameIdx = sdkCloud->seq;
|
||
ld.llTimeStamp = static_cast<unsigned long long>(sdkCloud->timestamp * 1e6);
|
||
ld.nEncodeNo = static_cast<int>(line);
|
||
ld.bEndOnceScan = (line + 1 == H) ? VzTrue : VzFalse;
|
||
|
||
cloudData.emplace_back(keResultDataType_PointXYZI, ld);
|
||
}
|
||
|
||
frame->info.height = H;
|
||
frame->info.width = W;
|
||
frame->info.isDense = allValid;
|
||
|
||
// 送入交付队列（可能短暂阻塞等 deliveryThread 消费，提供反压）
|
||
m_deliveryQueue.push(frame);
|
||
|
||
// 归还 SDK 缓冲到 freeQueue（复用其预留 capacity）
|
||
m_freeQueue.push(sdkCloud);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：交付线程（只调回调，不做数据处理）
|
||
// 从 m_deliveryQueue 取帧 → 调 PointCloudCallback（PushFrame memcpy 在此发生）
|
||
// 与 processCloudThread 并行：交付帧 N 的同时，解析帧 N+1
|
||
// ============================================================
|
||
void CRsLidarDevice::deliveryThread()
|
||
{
|
||
while (!m_bStopDelivery)
|
||
{
|
||
auto frame = m_deliveryQueue.popWait(500000);
|
||
if (!frame) continue; // 超时 → 检查 m_bStopDelivery
|
||
|
||
// 取回调（持锁时间极短）
|
||
PointCloudCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
cb = m_pointCloudCallback;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
auto t0 = std::chrono::steady_clock::now();
|
||
cb(frame->cloudData, frame->info);
|
||
auto t1 = std::chrono::steady_clock::now();
|
||
|
||
// 累计回调耗时统计
|
||
uint64_t us = static_cast<uint64_t>(
|
||
std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count());
|
||
m_cbAccumUs.fetch_add(us, std::memory_order_relaxed);
|
||
m_cbCount.fetch_add(1, std::memory_order_relaxed);
|
||
|
||
uint64_t prevMax = m_cbMaxUs.load(std::memory_order_relaxed);
|
||
while (us > prevMax &&
|
||
!m_cbMaxUs.compare_exchange_weak(prevMax, us, std::memory_order_relaxed))
|
||
{}
|
||
}
|
||
|
||
// 归还交付帧到空闲池（清空 cloudData 元数据，保留 linePool capacity）
|
||
frame->cloudData.clear();
|
||
m_frameFreeQueue.push(frame);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：空闲/就绪队列回调（rs_driver 内部线程）
|
||
// ============================================================
|
||
CRsLidarDevice::SdkCloudPtr CRsLidarDevice::getFreeCloud()
|
||
{
|
||
auto msg = m_freeQueue.pop();
|
||
if (msg) return msg;
|
||
return std::make_shared<SdkCloudMsg>();
|
||
}
|
||
|
||
void CRsLidarDevice::putStuffedCloud(SdkCloudPtr 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 已被自动更新，循环重试
|
||
}
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：Packet → RsPacketInfo
|
||
// ============================================================
|
||
void CRsLidarDevice::onPacket(const Packet& pkt)
|
||
{
|
||
PacketCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
cb = m_packetCallback;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
RsPacketInfo info;
|
||
info.timestamp = pkt.timestamp;
|
||
info.seq = pkt.seq;
|
||
info.is_difop = pkt.is_difop;
|
||
info.is_frame_begin = pkt.is_frame_begin;
|
||
info.frameId = pkt.frame_id;
|
||
info.dataSize = static_cast<uint32_t>(pkt.buf_.size());
|
||
cb(info);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 监控接口
|
||
// ============================================================
|
||
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);
|
||
cb = m_exceptionCallback;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
RsExceptionInfo info;
|
||
info.code = static_cast<int>(code.error_code);
|
||
info.message = code.toString();
|
||
cb(info);
|
||
}
|
||
}
|