GrabBag/Test/RsLidarTest/main.cpp

#include <iostream>
#include <iomanip>
#include <thread>
#include <chrono>
#include <atomic>
#include <mutex>
#include <condition_variable>
#include <deque>
#include <string>
#include <ctime>
#include <sstream>
#include <vector>
#include <memory>
#include <cstring>
#include <cmath>
#include <limits>
#define NOMINMAX
#include <windows.h>
#include "IRsLidarDevice.h"
#include "ICloudShow.h"

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

/// 全局控制标志
static std::atomic<bool> g_bExit{false};

/// 统计信息
static std::atomic<uint64_t> g_nFrameCount{0};
static std::atomic<uint64_t> g_nTotalPoints{0};
static std::chrono::steady_clock::time_point g_startTime;

/// 当前帧追踪
static std::atomic<int> g_nFrameBeginCount{0};

/// 控制台输出锁
static std::mutex g_consoleMutex;
static bool g_bVerbose = false;
static bool g_bDisplay = true;
static std::string g_saveDir;
static std::atomic<int> g_nSaveSeq{0};
static std::unique_ptr<ICloudShow> g_pCloudShow;
static IRsLidarDevice* g_pDevice = nullptr;

// ============================================================
// 异步保存：OnRawCloud 仅入队，独立线程消费 SaveToTxt
// 队列上限 8，满则丢最老一帧并计数（离线场景丢老优于阻塞）
// ============================================================
struct SaveTask
{
    int seq = 0;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
};

static std::mutex                       g_saveMutex;
static std::condition_variable          g_saveCv;
static std::deque<SaveTask>             g_saveQueue;
static std::atomic<bool>                g_saveExit{false};
static std::atomic<uint64_t>            g_saveDropped{0};
static constexpr size_t                 SAVE_QUEUE_MAX = 8;

// ============================================================
// 显示限速：UpdateCloud 间隔 ≥200ms (~5fps)，保存路径不限速
// ============================================================
static constexpr int                    DISPLAY_INTERVAL_MS = 200;
static std::chrono::steady_clock::time_point g_lastDisplayTime;
static std::mutex                       g_displayTimeMutex;

static void PrintLog(const std::string& message)
{
    std::lock_guard<std::mutex> lock(g_consoleMutex);
    auto now = std::time(nullptr);
    char timeStr[20];
    std::strftime(timeStr, sizeof(timeStr), "%H:%M:%S", std::localtime(&now));
    std::cout << "[" << timeStr << "] " << message << std::endl;
}

/// 每收到一个原始包触发 — 通过 is_frame_begin 感知扫描起始
static void OnPacket(const RsPacketInfo& pkt)
{
    if (pkt.is_frame_begin)
    {
        g_nFrameBeginCount++;

        if (g_bVerbose)
        {
            std::ostringstream oss;
            oss << ">>> is_frame_begin=1"
                << " | pkt_seq=" << pkt.seq
                << " | 第 " << g_nFrameBeginCount << " 圈";
            PrintLog(oss.str());
        }
    }
}

/// 每收到一帧原始点云触发 — 在单次遍历中完成 NaN 清洗 + m→mm 转换 + PCL 格式拷贝
static void OnRawCloud(const RsPointXYZI* points, uint32_t pointCount, const RsFrameMeta& meta)
{
    if (pointCount == 0) return;

    g_nFrameCount++;
    g_nTotalPoints += pointCount;

    if (g_bVerbose)
    {
        const auto& first = points[0];
        const auto& last  = points[pointCount - 1];

        float fx = (std::isnan(first.x) ? 0.0f : first.x) * 1000.0f;
        float fy = (std::isnan(first.y) ? 0.0f : first.y) * 1000.0f;
        float fz = (std::isnan(first.z) ? 0.0f : first.z) * 1000.0f;
        float lx = (std::isnan(last.x) ? 0.0f : last.x) * 1000.0f;
        float ly = (std::isnan(last.y) ? 0.0f : last.y) * 1000.0f;
        float lz = (std::isnan(last.z) ? 0.0f : last.z) * 1000.0f;

        std::ostringstream oss;
        oss << "  └─ 完整帧 #" << meta.seq
            << " | " << pointCount << "点"
            << " | ts=" << std::fixed << std::setprecision(3) << meta.timestamp
            << " | (" << fx << "," << fy << "," << fz << ")"
            << "→(" << lx << "," << ly << "," << lz << ")";
        PrintLog(oss.str());
    }

    // 单次遍历：NaN→0 + m→mm + 近零点→NaN + PCL 格式填充
    auto pclCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
    pclCloud->height   = meta.height;
    pclCloud->width    = meta.width;
    pclCloud->is_dense = meta.isDense;
    pclCloud->points.resize(pointCount);
    const float nan = std::numeric_limits<float>::quiet_NaN();
    for (uint32_t i = 0; i < pointCount; ++i)
    {
        float x = (std::isnan(points[i].x) ? 0.0f : points[i].x) * 1000.0f;
        float y = (std::isnan(points[i].y) ? 0.0f : points[i].y) * 1000.0f;
        float z = (std::isnan(points[i].z) ? 0.0f : points[i].z) * 1000.0f;

        if (std::abs(x) < 0.005f && std::abs(y) < 0.005f && std::abs(z) < 0.005f)
        {
            pclCloud->points[i].x = nan;
            pclCloud->points[i].y = nan;
            pclCloud->points[i].z = nan;
            pclCloud->is_dense = false;
        }
        else
        {
            pclCloud->points[i].x = x;
            pclCloud->points[i].y = y;
            pclCloud->points[i].z = z;
        }
        pclCloud->points[i].intensity = static_cast<float>(points[i].intensity);
    }

    // 持续保存到目录（--save dir）：仅入队，由 SaveLoop 异步写
    if (!g_saveDir.empty() && g_pCloudShow)
    {
        SaveTask task;
        task.seq   = ++g_nSaveSeq;
        task.cloud = pclCloud;

        {
            std::lock_guard<std::mutex> lk(g_saveMutex);
            if (g_saveQueue.size() >= SAVE_QUEUE_MAX)
            {
                g_saveQueue.pop_front();             // 丢最老
                g_saveDropped.fetch_add(1, std::memory_order_relaxed);
            }
            g_saveQueue.emplace_back(std::move(task));
        }
        g_saveCv.notify_one();
    }

    // 单次保存（PCL 窗口按键 S/s 触发）— 同步直接写，单次操作不影响吞吐
    if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsSaveRequested())
    {
        g_pCloudShow->ClearSaveRequest();
        int seq = ++g_nSaveSeq;
        std::string dir = g_saveDir.empty() ? "." : g_saveDir;
        std::ostringstream oss;
        oss << dir << "/LaserData_" << seq << ".txt";
        PrintLog("开始存储: " + oss.str());
        g_pCloudShow->SaveToTxt(oss.str(), pclCloud);
        PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast<int>(pointCount)) + "点");
    }

    // 推送点云到显示窗口（限速 ~5fps，避免 PCL/VTK 渲染压力反传）
    if (g_bDisplay && g_pCloudShow)
    {
        auto now = std::chrono::steady_clock::now();
        bool shouldShow = false;
        {
            std::lock_guard<std::mutex> lk(g_displayTimeMutex);
            if (std::chrono::duration_cast<std::chrono::milliseconds>(now - g_lastDisplayTime).count() >= DISPLAY_INTERVAL_MS)
            {
                g_lastDisplayTime = now;
                shouldShow = true;
            }
        }
        if (shouldShow)
        {
            g_pCloudShow->UpdateCloud(pclCloud);
        }
    }
}

static void OnException(const RsExceptionInfo& info)
{
    PrintLog("[异常 " + std::to_string(info.code) + "] " + info.message);
}

// 异步保存线程：从 g_saveQueue 取任务调 SaveToTxt
// 队列空时 wait，g_saveExit 标记后 flush 剩余帧再退出
static void SaveLoop()
{
    while (true)
    {
        SaveTask task;
        {
            std::unique_lock<std::mutex> lk(g_saveMutex);
            g_saveCv.wait(lk, [] { return g_saveExit.load() || !g_saveQueue.empty(); });
            if (g_saveQueue.empty())
            {
                if (g_saveExit.load()) return;
                continue;
            }
            task = std::move(g_saveQueue.front());
            g_saveQueue.pop_front();
        }

        std::ostringstream oss;
        oss << g_saveDir << "/LaserData_" << task.seq << ".txt";
        g_pCloudShow->SaveToTxt(oss.str(), task.cloud);
        if (g_bVerbose)
        {
            PrintLog("存储完成: " + oss.str());
        }
    }
}

static void StatsLoop()
{
    while (!g_bExit)
    {
        std::this_thread::sleep_for(std::chrono::seconds(5));

        auto now = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - g_startTime).count();
        if (elapsed == 0) elapsed = 1;

        std::ostringstream oss;
        oss << "== " << elapsed << "s"
            << " | 帧=" << g_nFrameCount
            << " | 帧起始=" << g_nFrameBeginCount
            << " | fps=" << std::fixed << std::setprecision(1)
            << (static_cast<double>(g_nFrameCount) / elapsed)
            << " | pps=" << std::fixed << std::setprecision(0)
            << (static_cast<double>(g_nTotalPoints) / elapsed);

        // 应用层累计指标（窗口型字段由 Device 自身 5s stderr 日志输出，避免重复+竞态）
        if (g_pDevice)
        {
            oss << " | drop=" << g_pDevice->GetDroppedFrameCount()
                << " save_drop=" << g_saveDropped.load();
        }

        PrintLog(oss.str());
    }
}

static void PrintUsage(const char* prog)
{
    std::cout << "用法: " << prog << " [选项]" << std::endl;
    std::cout << std::endl;
    std::cout << "选项:" << std::endl;
    std::cout << "  --host <IP>        雷达主机地址 (默认: 0.0.0.0)" << std::endl;
    std::cout << "  --msop <port>      MSOP 端口 (默认: 6699)" << std::endl;
    std::cout << "  --difop <port>     DIFOP 端口 (默认: 7788)" << std::endl;
    std::cout << "  --type <name>      雷达型号 (默认: RSAIRY)" << std::endl;
    std::cout << "  --time <seconds>   运行时长 (默认: 30, 0=持续)" << std::endl;
    std::cout << "  --verbose          逐帧打印扫描生命周期" << std::endl;
    std::cout << "  --display          实时点云显示（非阻塞，PCL窗口 S=保存 Q=退出）" << std::endl;
    std::cout << "  --save <dir>       保存点云到指定目录（frame_<seq>.txt，异步写）" << std::endl;
    std::cout << "  --recvbuf <bytes>  socket 接收缓冲字节数 (默认 33554432 = 32MB)" << std::endl;
    std::cout << std::endl;
    std::cout << "雷达型号: RS16 RS32 RSBP RSAIRY RSHELIOS RS128 RS80 RS48 RSM1 RSM2 RSM3 等" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << prog << " --host 192.168.1.100 --type RSHELIOS --time 60 --verbose" << std::endl;
    std::cout << "  " << prog << " --host 192.168.1.100 --type RSAIRY --display --save ./data --time 120" << std::endl;
}

int main(int argc, char* argv[])
{
    SetConsoleOutputCP(CP_UTF8);

    RsLidarConfig config;
    int runSeconds = 0;

    for (int i = 1; i < argc; ++i)
    {
        std::string arg = argv[i];
        if (arg == "--help" || arg == "-h")
        {
            PrintUsage(argv[0]);
            return 0;
        }
        else if (arg == "--host" && i + 1 < argc)
            config.hostAddress = argv[++i];
        else if (arg == "--msop" && i + 1 < argc)
            config.msopPort = static_cast<uint16_t>(std::stoi(argv[++i]));
        else if (arg == "--difop" && i + 1 < argc)
            config.difopPort = static_cast<uint16_t>(std::stoi(argv[++i]));
        else if (arg == "--type" && i + 1 < argc)
            config.lidarType = argv[++i];
        else if (arg == "--time" && i + 1 < argc)
            runSeconds = std::stoi(argv[++i]);
        else if (arg == "--verbose")
            g_bVerbose = true;
        else if (arg == "--display")
            g_bDisplay = true;
        else if (arg == "--save" && i + 1 < argc)
            g_saveDir = argv[++i];
        else if (arg == "--recvbuf" && i + 1 < argc)
            config.socketRecvBufBytes = static_cast<unsigned int>(std::stoul(argv[++i]));
    }

    std::cout << "============================================" << std::endl;
    std::cout << "  RsLidarDevice 测试" << std::endl;
    std::cout << "============================================" << std::endl;
    std::cout << "雷达: " << config.lidarType << std::endl;
    std::cout << "地址: " << config.hostAddress << std::endl;
    std::cout << "端口: MSOP=" << config.msopPort << " DIFOP=" << config.difopPort << std::endl;
    std::cout << "时长: " << (runSeconds > 0 ? std::to_string(runSeconds) + "s" : "持续") << std::endl;
    std::cout << "详细: " << (g_bVerbose ? "ON" : "OFF") << std::endl;
    std::cout << "RecvBuf: " << (config.socketRecvBufBytes / (1024 * 1024)) << "MB" << std::endl;
    std::cout << "============================================" << std::endl;

    // 1. 创建
    IRsLidarDevice* pDevice = nullptr;
    if (IRsLidarDevice::CreateObject(&pDevice) != 0 || !pDevice)
    {
        std::cerr << "创建设备失败" << std::endl;
        return -1;
    }
    g_pDevice = pDevice;  // 供 StatsLoop 查询诊断接口
    PrintLog("设备实例创建完成");
    PrintLog("驱动版本: " + pDevice->GetVersion());

    // CloudShow 实例
    g_pCloudShow = ICloudShow::Create();

    // 2. 初始化
    if (pDevice->InitDevice() != 0)
    {
        std::cerr << "SDK 初始化失败" << std::endl;
        delete pDevice;
        return -1;
    }
    PrintLog("SDK 初始化完成");

    // 3. 打开
    if (pDevice->OpenDevice(config) != 0)
    {
        std::cerr << "打开设备失败" << std::endl;
        pDevice->CloseDevice();
        delete pDevice;
        return -1;
    }
    PrintLog("设备打开完成");

    // 4. 注册回调（原始回调—单次遍历完成全部处理）
    pDevice->SetPacketCallback(OnPacket);
    pDevice->SetRawCloudCallback(OnRawCloud);
    pDevice->SetExceptionCallback(OnException);

    // 5. 启动
    if (pDevice->Start() != 0)
    {
        std::cerr << "启动失败" << std::endl;
        pDevice->CloseDevice();
        delete pDevice;
        return -1;
    }
    g_startTime = std::chrono::steady_clock::now();
    PrintLog("数据采集已启动");

    // 6. 统计线程 + 异步保存线程
    std::thread statsThread(StatsLoop);
    std::thread saveThread;
    if (!g_saveDir.empty())
    {
        saveThread = std::thread(SaveLoop);
        PrintLog("异步保存线程已启动 -> " + g_saveDir);
    }

    // 7. 启动显示窗口（主线程创建 viewer + 驱动渲染）
    if (g_bDisplay && g_pCloudShow)
    {
        if (g_pCloudShow->Start("LiDAR PointCloud") == 0)
        {
            PrintLog("PCL 窗口已打开 — S=保存下一帧  Q=退出");
        }
        else
        {
            PrintLog("PCL 窗口创建失败，继续无显示运行");
            g_bDisplay = false;
        }
    }

    // 8. 主循环：驱动 PCL 渲染 + 轮询键盘 + 超时
    auto deadline = (runSeconds > 0)
        ? std::chrono::steady_clock::now() + std::chrono::seconds(runSeconds)
        : std::chrono::steady_clock::time_point::max();

    while (!g_bExit)
    {
        if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsRunning())
        {
            if (g_pCloudShow->SpinOnce(10) < 0)
            {
                PrintLog("PCL 窗口已关闭");
                break;
            }
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsQuitRequested())
        {
            PrintLog("用户退出 (Q)");
            break;
        }

        if (std::chrono::steady_clock::now() >= deadline)
            break;
    }

    // 9. 停止
    g_bExit = true;
    pDevice->Stop();
    if (statsThread.joinable()) statsThread.join();

    // 通知保存线程退出（flush 剩余 ≤8 帧）
    if (saveThread.joinable())
    {
        g_saveExit = true;
        g_saveCv.notify_all();
        PrintLog("等待保存线程 flush 剩余帧...");
        saveThread.join();
        PrintLog("保存线程退出");
    }

    auto endTime = std::chrono::steady_clock::now();
    auto totalElapsed = std::chrono::duration_cast<std::chrono::seconds>(endTime - g_startTime).count();
    if (totalElapsed == 0) totalElapsed = 1;

    std::cout << std::endl;
    std::cout << "============================================" << std::endl;
    std::cout << "  最终统计" << std::endl;
    std::cout << "============================================" << std::endl;
    std::cout << "运行: " << totalElapsed << "s" << std::endl;
    std::cout << "is_frame_begin: " << g_nFrameBeginCount << " 次" << std::endl;
    std::cout << "完整帧: " << g_nFrameCount << std::endl;
    std::cout << "总点数: " << g_nTotalPoints << std::endl;
    std::cout << "帧率: " << std::fixed << std::setprecision(2)
              << (static_cast<double>(g_nFrameCount) / totalElapsed) << " fps" << std::endl;
    std::cout << "点率: " << std::fixed << std::setprecision(0)
              << (static_cast<double>(g_nTotalPoints) / totalElapsed) << " pts/s" << std::endl;
    std::cout << "预期帧数: " << (totalElapsed * 10) << std::endl;
    std::cout << "丢失率: " << std::fixed << std::setprecision(1)
              << (100.0 * (totalElapsed * 10 - g_nFrameCount) / (totalElapsed * 10)) << "%" << std::endl;
    std::cout << "队列丢帧: " << pDevice->GetDroppedFrameCount() << std::endl;
    std::cout << "队列峰值: " << pDevice->GetStuffedQueuePeak() << std::endl;
    std::cout << "保存丢帧: " << g_saveDropped.load() << std::endl;
    std::cout << "异常 msopto(0x40): " << pDevice->GetExceptionCount(0x40) << std::endl;
    std::cout << "异常 pktof(0x48):  " << pDevice->GetExceptionCount(0x48) << std::endl;
    std::cout << "异常 cldof(0x49):  " << pDevice->GetExceptionCount(0x49) << std::endl;
    std::cout << "============================================" << std::endl;

    g_pDevice = nullptr;
    pDevice->CloseDevice();
    delete pDevice;

    if (g_pCloudShow)
    {
        g_pCloudShow->Stop();
        g_pCloudShow.reset();
    }

    return 0;
}