371 lines
12 KiB
C++
371 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>
|
||
#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 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());
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 每收到一帧完整点云触发 — 可在此做后处理
|
||
static void OnPointCloud(const RsPointCloudData& data)
|
||
{
|
||
if (data.pointCount == 0) return;
|
||
|
||
g_nFrameCount++;
|
||
g_nTotalPoints += data.pointCount;
|
||
|
||
if (g_bVerbose)
|
||
{
|
||
const auto& first = data.points[0];
|
||
const auto& last = data.points[data.pointCount - 1];
|
||
|
||
std::ostringstream oss;
|
||
oss << " └─ 完整帧 #" << data.seq
|
||
<< " | " << data.pointCount << "点"
|
||
<< " | ts=" << std::fixed << std::setprecision(3) << data.timestamp
|
||
<< " | (" << first.x << "," << first.y << "," << first.z << ")"
|
||
<< "→(" << last.x << "," << last.y << "," << last.z << ")";
|
||
PrintLog(oss.str());
|
||
}
|
||
|
||
// 转换为 PCL 格式 — RsPointXYZI(packed 13B) 与 pcl::PointXYZI(20B) 布局不同，需逐字段拷贝
|
||
// NaN→0 和 m→mm 已在 RsLidarDevice 层处理
|
||
auto pclCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
|
||
pclCloud->height = data.height;
|
||
pclCloud->width = data.width;
|
||
pclCloud->is_dense = data.isDense;
|
||
pclCloud->points.resize(data.pointCount);
|
||
const float nan = std::numeric_limits<float>::quiet_NaN();
|
||
for (size_t i = 0; i < data.pointCount; ++i)
|
||
{
|
||
float x = data.points[i].x;
|
||
float y = data.points[i].y;
|
||
float z = data.points[i].z;
|
||
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>(data.points[i].intensity);
|
||
}
|
||
|
||
// 持续保存到目录（--save dir）
|
||
if (!g_saveDir.empty() && g_pCloudShow)
|
||
{
|
||
int seq = ++g_nSaveSeq;
|
||
std::ostringstream oss;
|
||
oss << g_saveDir << "/LaserData_" << seq << ".txt";
|
||
PrintLog("开始存储: " + oss.str());
|
||
g_pCloudShow->SaveToTxt(oss.str(), pclCloud);
|
||
PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast<int>(data.pointCount)) + "点");
|
||
}
|
||
|
||
// 单次保存（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>(data.pointCount)) + "点");
|
||
}
|
||
|
||
// 推送点云到显示窗口
|
||
if (g_bDisplay && g_pCloudShow)
|
||
{
|
||
g_pCloudShow->UpdateCloud(pclCloud);
|
||
}
|
||
}
|
||
|
||
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
|
||
<< " | 帧起始=" << 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);
|
||
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 << 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];
|
||
}
|
||
|
||
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 << "============================================" << std::endl;
|
||
|
||
// 1. 创建
|
||
IRsLidarDevice* pDevice = nullptr;
|
||
if (IRsLidarDevice::CreateObject(&pDevice) != 0 || !pDevice)
|
||
{
|
||
std::cerr << "创建设备失败" << std::endl;
|
||
return -1;
|
||
}
|
||
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->SetPointCloudCallback(OnPointCloud);
|
||
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. 启动显示窗口（主线程创建 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();
|
||
|
||
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 << "============================================" << std::endl;
|
||
|
||
pDevice->CloseDevice();
|
||
delete pDevice;
|
||
|
||
if (g_pCloudShow)
|
||
{
|
||
g_pCloudShow->Stop();
|
||
g_pCloudShow.reset();
|
||
}
|
||
|
||
return 0;
|
||
}
|