150 lines
5.3 KiB
C++
150 lines
5.3 KiB
C++
#ifndef CRSLIDARDEVICE_H
|
||
#define CRSLIDARDEVICE_H
|
||
|
||
#include "IRsLidarDevice.h"
|
||
|
||
// SDK 内部以 uint8_t* 调用 recvfrom()，但 C++ 中 char*/unsigned char*/signed char*
|
||
// 是三种独立类型，MSVC 拒绝 uint8_t*(=unsigned char*) → char* 的隐式转换。
|
||
// 此重载在 SDK 头文件之前定义，利用重载决议的精确匹配规则拦截调用并内部转型。
|
||
#ifdef _WIN32
|
||
#include <winsock2.h>
|
||
#pragma comment(lib, "ws2_32.lib")
|
||
inline int recvfrom(SOCKET s, unsigned char* buf, int len, int flags, sockaddr* from, int* fromlen)
|
||
{
|
||
return ::recvfrom(s, reinterpret_cast<char*>(buf), len, flags, from, fromlen);
|
||
}
|
||
#endif
|
||
|
||
// SDK 依赖 std::array / int16_t / uint16_t，但其内部缺少对应的 #include，
|
||
// 在 Linux 上通过 libstdc++ 间接包含可用，MSVC STL 不会间接拉入，需在此处显式引入。
|
||
#include <array>
|
||
#include <cstdint>
|
||
|
||
// SDK 头文件全部放在私有层，不污染公共接口
|
||
#include <rs_driver/api/lidar_driver.hpp>
|
||
#include <rs_driver/msg/point_cloud_msg.hpp>
|
||
#include <rs_driver/msg/packet.hpp>
|
||
#include <rs_driver/driver/driver_param.hpp>
|
||
#include <rs_driver/common/error_code.hpp>
|
||
#include <rs_driver/utility/sync_queue.hpp>
|
||
|
||
#include <thread>
|
||
#include <atomic>
|
||
#include <mutex>
|
||
#include <array>
|
||
#include <chrono>
|
||
|
||
using namespace robosense::lidar;
|
||
|
||
// 交付帧：封装一次完整扫描的数据 + 行缓冲池（p3DPoint 指向此处）
|
||
// processCloudThread 填充 → deliveryThread 消费（调用回调）→ 归还 m_frameFreeQueue
|
||
struct DeliveryFrame
|
||
{
|
||
std::vector<std::vector<SVzNLPointXYZI>> linePool;
|
||
RsCloudData cloudData;
|
||
RsFrameInfo info;
|
||
|
||
void reserveLines(size_t lines, size_t pointsPerLine)
|
||
{
|
||
linePool.resize(lines);
|
||
for (auto& v : linePool)
|
||
v.reserve(pointsPerLine);
|
||
}
|
||
};
|
||
using DeliveryFramePtr = std::shared_ptr<DeliveryFrame>;
|
||
|
||
class CRsLidarDevice : public IRsLidarDevice
|
||
{
|
||
public:
|
||
CRsLidarDevice();
|
||
~CRsLidarDevice();
|
||
|
||
int InitDevice() override;
|
||
int OpenDevice(const RsLidarConfig& config) override;
|
||
int CloseDevice() override;
|
||
bool IsOpened() const override;
|
||
|
||
int Start() override;
|
||
int Stop() override;
|
||
bool IsRunning() const override;
|
||
|
||
int SetPointCloudCallback(PointCloudCallback 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>;
|
||
using SdkCloudPtr = std::shared_ptr<SdkCloudMsg>;
|
||
|
||
/// 解析线程：pop SDK帧 → NaN→0+变换+拆线 → 推入交付队列 → 归还SDK缓冲
|
||
void processCloudThread();
|
||
|
||
/// 交付线程：从交付队列取帧 → 调用用户回调（PushFrame memcpy在此发生）→ 归还交付帧
|
||
void deliveryThread();
|
||
|
||
/// 输出 5s 窗口诊断日志并重置窗口字段；仅当距 lastLogTime ≥5s 时实际输出
|
||
void emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime);
|
||
|
||
SdkCloudPtr getFreeCloud();
|
||
void putStuffedCloud(SdkCloudPtr msg);
|
||
|
||
/// 将 SDK Packet 转为 RsPacketInfo 后转发给用户
|
||
void onPacket(const Packet& pkt);
|
||
|
||
/// 将 SDK Error 转为 RsExceptionInfo 后转发给用户
|
||
void onException(const Error& code);
|
||
|
||
/// RsLidarConfig → RSDriverParam
|
||
static RSDriverParam toDriverParam(const RsLidarConfig& config);
|
||
|
||
bool m_bOpened = false;
|
||
std::atomic<bool> m_bRunning{false};
|
||
std::atomic<bool> m_bStopProcess{false};
|
||
std::atomic<bool> m_bStopDelivery{false};
|
||
|
||
PointCloudCallback m_pointCloudCallback;
|
||
PacketCallback m_packetCallback;
|
||
ExceptionCallback m_exceptionCallback;
|
||
std::mutex m_callbackMutex;
|
||
|
||
std::unique_ptr<LidarDriver<SdkCloudMsg>> m_pDriver;
|
||
std::thread m_processThread;
|
||
std::thread m_deliveryThread;
|
||
|
||
SyncQueue<SdkCloudPtr, 4096> m_freeQueue;
|
||
SyncQueue<SdkCloudPtr, 4096> m_stuffedQueue;
|
||
|
||
// 交付帧流水线（深度2=解耦一帧）：processCloud 推入 → deliveryThread 消费
|
||
SyncQueue<DeliveryFramePtr, 2> m_deliveryQueue;
|
||
SyncQueue<DeliveryFramePtr, 4> m_frameFreeQueue; // 空闲帧池（3帧预分配 + 1余量）
|
||
|
||
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
|