|
|
// lidar_utils.h - 雷达连接与扫描采集工具
|
|
|
#ifndef LIDAR_UTILS_H
|
|
|
#define LIDAR_UTILS_H
|
|
|
|
|
|
#include <atomic>
|
|
|
#include <mutex>
|
|
|
#include <string>
|
|
|
#include <thread>
|
|
|
#include <vector>
|
|
|
#include <chrono>
|
|
|
#include "PAVO2_SDK/include/pavo2_driver.h"
|
|
|
#include "common.h"
|
|
|
#include "config.h"
|
|
|
|
|
|
// ==================== 雷达连接 ====================
|
|
|
// 连接单台雷达(TCP)：按 rc.radar 配置 IP/端口与雷达内置滤波，并开启数据传输；失败返回 false。
|
|
|
// 注意：Windows 下需在 WSAStartup 之后调用；调用方应保证同一 driver 不被并发使用。
|
|
|
bool connect_lidar(pavo2::pavo2_driver& driver, const RadarConfig& rc);
|
|
|
|
|
|
// ==================== 扫描辅助 ====================
|
|
|
// 排空 SDK 队列中积压的旧扫描包（采集 start 之前雷达已缓存的数据）。
|
|
|
// 防御性：最多排空 max_ms 毫秒（默认500），避免雷达持续产数导致卡死。
|
|
|
// 返回丢弃包数。
|
|
|
int drain_stale_scans(pavo2::pavo2_driver& driver, int max_ms = 500);
|
|
|
|
|
|
// z轴值换算（时间→z）：
|
|
|
// - cfg.scan.z_speed_type == "configuration" 且 speed.txt 有效：按速度剖面(时间→距离)变速插值，
|
|
|
// 超过表末尾时间按最后两条的平均速度外推延伸；
|
|
|
// - 否则按原逻辑：cfg.scan.z_speed>0 时为移动距离(mm)=cfg.scan.z_speed(cm/s)*10*秒；
|
|
|
// cfg.scan.z_speed=0 时 z 直接取 elapsed_ms（时间模式）。
|
|
|
double z_for_elapsed(double elapsed_ms);
|
|
|
|
|
|
// ==================== 变速速度剖面 (speed.txt) ====================
|
|
|
// speed.txt 每行两个数字: "时间(ms) 距离(mm)"（# 开头为注释，空行忽略），时间须递增。
|
|
|
// 当 cfg.scan.z_speed_type == "configuration" 时程序使用该表把采集时间换算为 z(行进距离, mm)，
|
|
|
// 从而实现变速运动识别；未找到有效表时自动回退使用 cfg.scan.z_speed 匀速。
|
|
|
// 默认路径 ./config/speed.txt（与 config/config 同目录）。
|
|
|
// 加载剖面（成功后内部替换当前剖面）。返回是否有效(≥2行)。
|
|
|
bool load_speed_profile(const std::string& path = "./config/speed.txt");
|
|
|
|
|
|
// 按当前 cfg.scan.z_speed_type 重载：configuration → 重新读 speed.txt；
|
|
|
// uniformSpeed → 清空剖面（不使用表）。配置重载(load_config)后应调用一次。
|
|
|
void reload_speed_profile();
|
|
|
|
|
|
// 当前剖面是否有效(≥2行，配置为 configuration 时 z 按表换算)。
|
|
|
bool speed_profile_loaded();
|
|
|
|
|
|
// 剖面平均速度(mm/s)；无效或非 distance 数据时返回 0。
|
|
|
double speed_profile_avg_velocity_mm_s();
|
|
|
|
|
|
// 当前 z 轴是否为距离(mm)：configuration 且剖面有效=true；
|
|
|
// uniformSpeed 时为 cfg.scan.z_speed>0。
|
|
|
bool z_is_distance_mode();
|
|
|
|
|
|
// ConfigureSpeed 标定采集：采集 seconds 秒，逐帧记录(时间, 过滤后Y最大值)，
|
|
|
// 平滑(抑制与总体趋势反向>5mm的抖动)后写入 ./config/speed.txt。
|
|
|
// 返回写入行数；失败返回 <0 并在 err 中给出原因。
|
|
|
// 注意：调用前应确保没有其他线程同时 get_scanned_data（与 ScanSession 互斥使用）。
|
|
|
int collect_speed_profile(pavo2::pavo2_driver& driver, double seconds,
|
|
|
std::string& err);
|
|
|
|
|
|
// 将一帧扫描包 vec_buff 中的点做 XY 过滤（原点噪点 + 过滤多边形）后追加到 out。
|
|
|
// z 由调用方给出（通常为 z_for_elapsed(elapsed_ms)）。
|
|
|
// 返回新增点数。
|
|
|
size_t scan_convert_points(const pavo2::pavo_packet_info_scan_t& scan_info,
|
|
|
double z_val,
|
|
|
std::vector<CountPoint>& out);
|
|
|
|
|
|
// ==================== 扫描会话 ====================
|
|
|
// 单次后台采集会话：reset() → start() 开启采集线程 → stop() 请求停止并等待线程退出。
|
|
|
// count/check 统一使用同一会话，保证同一时刻只有一个采集线程访问雷达。
|
|
|
// 线程安全；仅允许空闲时 reset()。
|
|
|
class ScanSession
|
|
|
{
|
|
|
public:
|
|
|
// 析构时必须 join 采集线程：std::thread 对象在仍 joinable 时析构会调用
|
|
|
// std::terminate → abort，进程"无声消失"且没有任何日志（历史崩溃主因之一）。
|
|
|
~ScanSession();
|
|
|
|
|
|
// 启动后台采集线程（radar_id 用于线程内绑定该雷达的配置）。
|
|
|
// 内部先排空 SDK 积压旧包，排空完成才开始 z 计时。
|
|
|
// 已在运行或线程创建失败时返回 false。
|
|
|
// io_mtx: 该雷达的 IO 互斥锁（RadarNode::io_mtx）。传入后采集线程取帧时与
|
|
|
// 后台重连线程互斥，避免"采集进行中掉线→重连"时两个线程并发操作
|
|
|
// 同一个 socket/driver（网络不好的机器上必崩）。
|
|
|
bool start(pavo2::pavo2_driver& driver, const std::string& radar_id,
|
|
|
std::mutex* io_mtx = nullptr);
|
|
|
|
|
|
// 请求停止并等待采集线程退出，返回当前已采集点数。
|
|
|
// 无论 running 与否都会置停止位并 join：只靠 running_ 判断会漏掉"线程已启动但
|
|
|
// 状态位未置"的情况，残留的 joinable 线程会在析构时 terminate。
|
|
|
size_t stop();
|
|
|
|
|
|
// 拷贝当前点集快照（线程安全）。
|
|
|
std::vector<CountPoint> snapshot() const;
|
|
|
|
|
|
// 是否正在采集运行中。
|
|
|
bool running() const;
|
|
|
|
|
|
// 复位（仅空闲时调用）：清空上一次点集并复位状态，供下一次 start 使用。
|
|
|
void reset();
|
|
|
|
|
|
// 后台采集线程执行体。
|
|
|
void collect_loop(pavo2::pavo2_driver* driver);
|
|
|
|
|
|
private:
|
|
|
mutable std::mutex mtx_;
|
|
|
bool running_ = false;
|
|
|
bool stop_req_ = false;
|
|
|
std::thread th_;
|
|
|
std::vector<CountPoint> points_;
|
|
|
std::string radar_id_; // 本会话所属雷达(线程内绑定配置用)
|
|
|
std::chrono::steady_clock::time_point start_tp_;
|
|
|
std::mutex* io_mtx_ = nullptr; // 与重连线程互斥的 driver 访问锁(可为 nullptr)
|
|
|
};
|
|
|
|
|
|
// ==================== 多雷达管理 ====================
|
|
|
// 配置里每个雷达标识对应一个节点：独立 driver、独立扫描会话、独立后台连接线程。
|
|
|
// 连接策略：未连接/已断开时后台线程按 RECONNECT_INTERVAL_MS 一直重试，不阻塞主程序。
|
|
|
struct RadarNode
|
|
|
{
|
|
|
std::string id; // 雷达标识
|
|
|
pavo2::pavo2_driver* driver = nullptr; // 该雷达独立连接
|
|
|
std::atomic<bool> connected{false}; // 是否已连接(后台线程维护)
|
|
|
std::atomic<bool> enabled{true}; // 配置中是否仍存在(重载后可能失效)
|
|
|
ScanSession session; // 该雷达独立扫描会话(可与其他雷达并行)
|
|
|
std::mutex io_mtx; // open/close/enable_data 串行化
|
|
|
};
|
|
|
|
|
|
class RadarManager
|
|
|
{
|
|
|
public:
|
|
|
// 兜底清理：若 main 未调用 shutdown()，全局对象析构时 ths_ 中的线程仍 joinable，
|
|
|
// std::thread 析构会 std::terminate → abort（进程无声消失）。
|
|
|
~RadarManager();
|
|
|
|
|
|
// 按 cfg 建立节点并启动后台连接线程（不阻塞：连不上也继续跑，后台一直重试）。
|
|
|
void init();
|
|
|
|
|
|
// 停止后台线程、结束采集、关闭并释放所有连接。
|
|
|
void shutdown();
|
|
|
|
|
|
// 配置重载后同步：新增配置里新出现的雷达；配置中删掉的雷达置为失效(不再可用)。
|
|
|
void sync_config();
|
|
|
|
|
|
// 按标识取节点；id 为空或不存在时返回默认(第一个)有效节点；无节点返回 nullptr。
|
|
|
RadarNode* find(const std::string& id) const;
|
|
|
|
|
|
// 所有节点（含失效的，按配置顺序）。
|
|
|
std::vector<RadarNode*> nodes() const;
|
|
|
|
|
|
// 默认雷达标识（配置第一个；无则空串）。
|
|
|
std::string default_id() const;
|
|
|
|
|
|
// 等待指定雷达连上（exe 模式用）；timeout_ms 内连上返回 true。
|
|
|
bool wait_connected(const std::string& id, int timeout_ms);
|
|
|
|
|
|
private:
|
|
|
void connect_loop(RadarNode* n); // 后台：断线自动重连
|
|
|
mutable std::mutex mtx_;
|
|
|
std::vector<RadarNode*> nodes_;
|
|
|
std::vector<std::thread> ths_;
|
|
|
std::atomic<bool> stop_{false};
|
|
|
};
|
|
|
|
|
|
// 全局多雷达管理器（lidar_utils.cpp 中定义）
|
|
|
extern RadarManager radar_mgr;
|
|
|
|
|
|
#endif // LIDAR_UTILS_H
|