542 lines
17 KiB
C++
542 lines
17 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();
|
||
|
||
#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)
|
||
{
|
||
m_freeQueue.push(std::make_shared<SdkCloudMsg>());
|
||
}
|
||
|
||
m_bStopProcess = false;
|
||
m_pDriver->start();
|
||
|
||
m_bRunning = true;
|
||
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();
|
||
}
|
||
|
||
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::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);
|
||
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();
|
||
|
||
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;
|
||
}
|
||
|
||
// 每 5 秒打印队列深度、丢帧、回调耗时、异常分类（窗口式重置）
|
||
emitDiagnosticLogIfDue(lastLogTime);
|
||
|
||
// 优先使用原始回调（零拷贝路径，用户自行在一次遍历中完成转换）
|
||
RawCloudCallback rawCb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
rawCb = m_rawCloudCallback;
|
||
}
|
||
|
||
auto cbStart = std::chrono::steady_clock::now();
|
||
|
||
if (rawCb)
|
||
{
|
||
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;
|
||
|
||
// 坐标系修正：原始 SDK (X前 Y左 Z上) → 输出 (X右 Y下 Z前)
|
||
// newX = -oldY, newY = -oldZ, newZ = oldX
|
||
// 直接在 SDK 缓冲区上 in-place 改写：缓冲区回收后会被 SDK 重新填充，修改安全
|
||
auto& pts = sdkCloud->points;
|
||
for (auto& p : pts)
|
||
{
|
||
float ox = p.x, oy = p.y, oz = p.z;
|
||
p.x = -oy;
|
||
p.y = -oz;
|
||
p.z = ox;
|
||
}
|
||
|
||
rawCb(reinterpret_cast<const RsPointXYZI*>(pts.data()),
|
||
static_cast<uint32_t>(pts.size()), meta);
|
||
}
|
||
else
|
||
{
|
||
PointCloudCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
cb = m_pointCloudCallback;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
// 转换并清洗数据: NaN→0, m→mm, 同时做坐标系修正
|
||
// 原始 SDK (X前 Y左 Z上) → 输出 (X右 Y下 Z前)
|
||
// newX = -oldY, newY = -oldZ, newZ = oldX
|
||
const auto& src = sdkCloud->points;
|
||
std::vector<RsPointXYZI> transformed;
|
||
transformed.reserve(src.size());
|
||
for (const auto& s : src)
|
||
{
|
||
float sx = std::isnan(s.x) ? 0.0f : s.x;
|
||
float sy = std::isnan(s.y) ? 0.0f : s.y;
|
||
float sz = std::isnan(s.z) ? 0.0f : s.z;
|
||
RsPointXYZI p;
|
||
p.x = -sy * 1000.0f;
|
||
p.y = -sz * 1000.0f;
|
||
p.z = sx * 1000.0f;
|
||
p.intensity = s.intensity;
|
||
transformed.push_back(p);
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：空闲/就绪队列回调（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);
|
||
}
|
||
}
|