|
|
|
|
|
// lidar_utils.h - 雷达连接与扫描采集工具
|
|
|
|
|
|
#ifndef LIDAR_UTILS_H
|
|
|
|
|
|
#define LIDAR_UTILS_H
|
|
|
|
|
|
|
|
|
|
|
|
#include <mutex>
|
|
|
|
|
|
#include <thread>
|
|
|
|
|
|
#include <vector>
|
|
|
|
|
|
#include <chrono>
|
|
|
|
|
|
#include "PAVO2_SDK/include/pavo2_driver.h"
|
|
|
|
|
|
#include "common.h"
|
|
|
|
|
|
|
|
|
|
|
|
// ==================== 雷达连接 ====================
|
|
|
|
|
|
// 连接雷达(TCP)，配置滤波并开启数据传输；失败返回 false。
|
|
|
|
|
|
// 注意：Windows 下需在 WSAStartup 之后调用。
|
|
|
|
|
|
bool connect_lidar(pavo2::pavo2_driver& driver);
|
|
|
|
|
|
|
|
|
|
|
|
// ==================== 扫描辅助 ====================
|
|
|
|
|
|
// 排空 SDK 队列中积压的旧扫描包（采集 start 之前雷达已缓存的数据）。
|
|
|
|
|
|
// 防御性：最多排空 max_ms 毫秒（默认500），避免雷达持续产数导致卡死。
|
|
|
|
|
|
// 返回丢弃包数。
|
|
|
|
|
|
int drain_stale_scans(pavo2::pavo2_driver& driver, int max_ms = 500);
|
|
|
|
|
|
|
|
|
|
|
|
// z轴值：Speed_Z>0 时为移动距离(mm)=Speed_Z(cm/s)*10*秒；
|
|
|
|
|
|
// Speed_Z=0 时 z 直接取 elapsed_ms（时间模式）。
|
|
|
|
|
|
double z_for_elapsed(double elapsed_ms);
|
|
|
|
|
|
|
|
|
|
|
|
// 将一帧扫描包 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:
|
|
|
|
|
|
// 启动后台采集线程。内部先排空 SDK 积压旧包，排空完成才开始 z 计时。
|
|
|
|
|
|
// 已在运行或线程创建失败时返回 false。
|
|
|
|
|
|
bool start(pavo2::pavo2_driver& driver);
|
|
|
|
|
|
|
|
|
|
|
|
// 请求停止并等待采集线程退出，返回当前已采集点数。
|
|
|
|
|
|
// 会话未运行（未 start 或已 stop）时仅返回现存点数，不做其他事。
|
|
|
|
|
|
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::chrono::steady_clock::time_point start_tp_;
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
#endif // LIDAR_UTILS_H
|
