GrabBag/Test/RsLidarTest/main.cpp

#include <iostream>
#include <iomanip>
#include <thread>
#include <chrono>
#include <atomic>
#include <mutex>
#include <string>
#include <ctime>
#include <sstream>
#include <vector>
#include <memory>
#include <cstring>
#include <cmath>

#define NOMINMAX
#include <windows.h>
#include <dbghelp.h>
#include <ctime>
#include <cstdio>
#include <cstdlib>

#pragma comment(lib, "dbghelp.lib")

#include "IRsLidarDevice.h"
#include "ICloudShow.h"
#include "VZNL_Types.h"

// ============================================================
// 崩溃自动抓 Dump
// ============================================================
static LONG WINAPI OnCrash(EXCEPTION_POINTERS* pExceptionInfo)
{
    char dumpPath[MAX_PATH];
    auto now = std::time(nullptr);
    std::tm tm;
    localtime_s(&tm, &now);
    snprintf(dumpPath, sizeof(dumpPath),
             "crash_%04d%02d%02d_%02d%02d%02d.dmp",
             tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday,
             tm.tm_hour, tm.tm_min, tm.tm_sec);

    HANDLE hFile = CreateFileA(dumpPath, GENERIC_WRITE,
        FILE_SHARE_WRITE, nullptr, CREATE_ALWAYS,
        FILE_ATTRIBUTE_NORMAL, nullptr);

    if (hFile != INVALID_HANDLE_VALUE)
    {
        MINIDUMP_EXCEPTION_INFORMATION info;
        info.ThreadId          = GetCurrentThreadId();
        info.ExceptionPointers = pExceptionInfo;
        info.ClientPointers    = FALSE;

        MiniDumpWriteDump(GetCurrentProcess(), GetCurrentProcessId(),
            hFile, MiniDumpNormal, &info, nullptr, nullptr);

        CloseHandle(hFile);

        fprintf(stderr, "\n!!! CRASH DUMP: %s\n", dumpPath);
        fprintf(stderr, "   异常地址: 0x%p  异常码: 0x%08lX\n",
                pExceptionInfo->ExceptionRecord->ExceptionAddress,
                pExceptionInfo->ExceptionRecord->ExceptionCode);
    }
    return EXCEPTION_EXECUTE_HANDLER;
}

static void EnableCrashDump()
{
    SetUnhandledExceptionFilter(OnCrash);
}

/// 全局控制标志
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::mutex g_consoleMutex;
static bool g_bVerbose = false;
static bool g_bDisplay = true;
static std::string g_saveDir;
static int g_nSaveInterval = 10;   // 保存帧间隔，默认每10帧存1帧

static std::unique_ptr<ICloudShow> g_pCloudShow;
static IRsLidarDevice* g_pDevice = nullptr;

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

/// 点云回调 — Device 已完成所有处理，此处纯转发
static void OnPointCloud(const RsCloudData& cloudData, const RsFrameInfo& rsInfo)
{
    if (!g_pCloudShow) return;

    // 统计
    for (const auto& p : cloudData)
        g_nTotalPoints += p.second.nPointCount;
    g_nFrameCount++;

    if (g_bVerbose)
    {
        std::ostringstream oss;
        oss << "  └─ 帧 | " << cloudData.size() << "线"
            << " | height=" << rsInfo.height << " width=" << rsInfo.width;
        PrintLog(oss.str());
    }

    // RsFrameInfo → CloudFrameInfo (字段完全一致)
    CloudFrameInfo info;
    info.height  = rsInfo.height;
    info.width   = rsInfo.width;
    info.isDense = rsInfo.isDense;

    // 转发（CloudData 同型，直接传递）
    g_pCloudShow->PushFrame(cloudData, info);
}

/// 数据包回调
static void OnPacket(const RsPacketInfo& pkt)
{
    if (pkt.is_frame_begin && g_bVerbose)
    {
        std::ostringstream oss;
        oss << ">>> is_frame_begin=1 | seq=" << pkt.seq;
        PrintLog(oss.str());
    }
}

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

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
            << " | fps=" << std::fixed << std::setprecision(1)
            << (static_cast<double>(g_nFrameCount) / elapsed)
            << " | pps=" << std::fixed << std::setprecision(0)
            << (static_cast<double>(g_nTotalPoints) / elapsed);
        if (g_pDevice)
        {
            const auto cb = g_pDevice->GetCallbackLatencyStats();
            oss << " | drop=" << g_pDevice->GetDroppedFrameCount()
                << " | q=" << g_pDevice->GetStuffedQueueDepth()
                << "/" << g_pDevice->GetStuffedQueuePeak()
                << " | cb_us=" << cb.avgUs << "/" << cb.maxUs
                << " | exc48=" << g_pDevice->GetExceptionCount(0x48)
                << " | exc49=" << g_pDevice->GetExceptionCount(0x49);
        }
        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          实时点云显示（START=连续保存 S=单帧 Q=退出）" << std::endl;
    std::cout << "  --save <dir>       保存基目录（默认当前目录）" << std::endl;
    std::cout << "  --save-interval <n> 保存帧间隔（默认10，1=每帧都存）" << std::endl;
    std::cout << "  --recvbuf <bytes>  socket 接收缓冲字节数 (默认 33554432 = 32MB)" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << prog << " --host 192.168.1.100 --type RSEM4 --display --save ./data --verbose" << std::endl;
}

int main(int argc, char* argv[])
{
    EnableCrashDump();
    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]));
        else if (arg == "--save-interval" && i + 1 < argc) g_nSaveInterval = std::stoi(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 << "============================================" << std::endl;

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

    // 2. CloudShow 实例
    g_pCloudShow = ICloudShow::Create();
    g_pCloudShow->SetSaveBaseDir(g_saveDir.empty() ? "." : g_saveDir);
    g_pCloudShow->SetSaveFrameInterval(g_nSaveInterval);

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

    // 4. 注册回调（PointCloudCallback：Device 已完成 NaN→0 + 坐标变换 + 米→毫米）
    pDevice->SetPointCloudCallback(OnPointCloud);
    if (g_bVerbose)
        pDevice->SetPacketCallback(OnPacket);
    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);

    // 7. 显示窗口
    if (g_bDisplay && g_pCloudShow)
    {
        if (g_pCloudShow->Start("LiDAR PointCloud v1.1.1") == 0)
            PrintLog("PCL 窗口已打开 — START=连续保存  S=单帧  Q=退出");
        else{
            PrintLog("PCL 窗口创建失败，继续无显示运行"); g_bDisplay = false;
        }
    }

    // 8. 主循环
    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; }
            // 限制主循环帧率 ~30fps，防止 VTK spinOnce 快速返回导致 CPU 100%
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
        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();

    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 << "完整帧: " << 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 << "队列丢帧: " << pDevice->GetDroppedFrameCount() << std::endl;
    std::cout << "队列峰值: " << pDevice->GetStuffedQueuePeak() << std::endl;
    std::cout << "============================================" << std::endl;

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

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

    return 0;
}