96 lines
3.0 KiB
C
96 lines
3.0 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>
|
|||
|
|
|
|||
|
|
using namespace robosense::lidar;
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
private:
|
|||
|
|
// SDK 内部类型别名
|
|||
|
|
using SdkCloudMsg = PointCloudT<PointXYZI>;
|
|||
|
|
using SdkCloudPtr = std::shared_ptr<SdkCloudMsg>;
|
|||
|
|
|
|||
|
|
void processCloudThread();
|
|||
|
|
|
|||
|
|
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};
|
|||
|
|
|
|||
|
|
PointCloudCallback m_pointCloudCallback;
|
|||
|
|
PacketCallback m_packetCallback;
|
|||
|
|
ExceptionCallback m_exceptionCallback;
|
|||
|
|
std::mutex m_callbackMutex;
|
|||
|
|
|
|||
|
|
std::unique_ptr<LidarDriver<SdkCloudMsg>> m_pDriver;
|
|||
|
|
std::thread m_processThread;
|
|||
|
|
|
|||
|
|
SyncQueue<SdkCloudPtr, 64> m_freeQueue;
|
|||
|
|
SyncQueue<SdkCloudPtr, 64> m_stuffedQueue;
|
|||
|
|
|
|||
|
|
RSDriverParam m_param;
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
#endif // CRSLIDARDEVICE_H
|