494 lines
18 KiB
C++
494 lines
18 KiB
C++
#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;
|
||
}
|