GrabBag/Device/RsLidarDevice/Src/RsLidarDevice.cpp

#include "RsLidarDevice.h"
#include <cmath>

// WSAStartup 引用计数
static std::atomic<int> g_sockRefCount{0};

// ============================================================
// 工厂方法
// ============================================================
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;
    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;
    }

    for (int i = 0; i < 8; ++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::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();
}

// ============================================================
// 内部：点云处理线程
// ============================================================
void CRsLidarDevice::processCloudThread()
{
    while (!m_bStopProcess)
    {
        SdkCloudPtr sdkCloud = m_stuffedQueue.popWait(500000);
        if (!sdkCloud || sdkCloud->points.empty())
        {
            continue;
        }

        PointCloudCallback cb;
        {
            std::lock_guard<std::mutex> lock(m_callbackMutex);
            cb = m_pointCloudCallback;
        }

        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);
            }

            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);
        }

        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_stuffedQueue.push(msg);
}

// ============================================================
// 内部：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);
    }
}

// ============================================================
// 内部：Error → RsExceptionInfo
// ============================================================
void CRsLidarDevice::onException(const Error& code)
{
    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);
    }
}