327 lines
12 KiB
C++
327 lines
12 KiB
C++
#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;
|
||
}
|