|
|
// lidar_utils.cpp - 雷达连接与扫描采集实现
|
|
|
#include "lidar_utils.h"
|
|
|
#include "config.h"
|
|
|
#include "filter.h"
|
|
|
#include <algorithm>
|
|
|
#include <cmath>
|
|
|
#include <fstream>
|
|
|
#include <iomanip>
|
|
|
#include <iostream>
|
|
|
#include <sstream>
|
|
|
|
|
|
// ==================== 雷达连接 ====================
|
|
|
bool connect_lidar(pavo2::pavo2_driver& driver, const RadarConfig& rc)
|
|
|
{
|
|
|
driver.set_conn_protocol(pavo2::PAVO2_PROTO_TYPE::TCP);
|
|
|
|
|
|
std::cout << "[连接][" << rc.id << "] 正在连接雷达 "
|
|
|
<< rc.radar.ip << ":" << rc.radar.port << " ..." << std::endl;
|
|
|
|
|
|
if (!driver.pavo2_open(rc.radar.ip, rc.radar.port, OPEN_TIMEOUT_MS))
|
|
|
{
|
|
|
driver.pavo2_close(); // 复位状态，避免半开连接影响下次重试
|
|
|
std::cout << "[连接][" << rc.id << "] 连接失败，稍后重试 ("
|
|
|
<< rc.radar.ip << ":" << rc.radar.port << ")" << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
std::cout << "[连接][" << rc.id << "] 雷达连接成功！" << std::endl;
|
|
|
|
|
|
driver.enable_tail_filter(rc.radar.tail_filter);
|
|
|
driver.enable_front_filter(rc.radar.front_filter);
|
|
|
driver.enable_far_hr_filter(rc.radar.far_hr_filter);
|
|
|
driver.enable_near_hr(rc.radar.near_hr_filter);
|
|
|
|
|
|
std::string sn, pn, sdk_ver, fw_ver;
|
|
|
if (driver.get_device_sn(sn))
|
|
|
std::cout << "[信息][" << rc.id << "] 设备SN: " << sn << std::endl;
|
|
|
if (driver.get_device_pn(pn))
|
|
|
std::cout << "[信息][" << rc.id << "] 设备PN: " << pn << std::endl;
|
|
|
if (driver.get_sdk_ver(sdk_ver))
|
|
|
std::cout << "[信息][" << rc.id << "] SDK版本: " << sdk_ver << std::endl;
|
|
|
if (driver.get_firmware_ver(fw_ver))
|
|
|
std::cout << "[信息][" << rc.id << "] 固件版本: " << fw_ver << std::endl;
|
|
|
|
|
|
if (!driver.enable_data(true))
|
|
|
{
|
|
|
std::cerr << "[错误][" << rc.id << "] 开启数据传输失败!" << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
std::cout << "[信息][" << rc.id << "] 雷达数据传输已开启。" << std::endl;
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
// ==================== 扫描辅助 ====================
|
|
|
int drain_stale_scans(pavo2::pavo2_driver& driver, int max_ms)
|
|
|
{
|
|
|
pavo2::pavo_packet_info_scan_t tmp;
|
|
|
int flushed = 0;
|
|
|
auto flush_begin = std::chrono::steady_clock::now();
|
|
|
while (true)
|
|
|
{
|
|
|
if (driver.get_scans_available() <= 0) break;
|
|
|
if (!driver.get_scanned_data(tmp, 0)) break; // 0ms = 立即返回，不阻塞
|
|
|
flushed++;
|
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
|
std::chrono::steady_clock::now() - flush_begin).count() > max_ms)
|
|
|
break;
|
|
|
}
|
|
|
return flushed;
|
|
|
}
|
|
|
|
|
|
// ==================== 变速速度剖面 (speed.txt) ====================
|
|
|
// 全局剖面: (时间ms, 距离mm)，时间严格递增。
|
|
|
namespace
|
|
|
{
|
|
|
std::mutex g_prof_mtx;
|
|
|
std::vector<std::pair<double, double>> g_profile;
|
|
|
|
|
|
// 时间→z 插值（调用方需已持有锁）。
|
|
|
// 命中返回 true；表不足两行返回 false（由调用方回退匀速）。
|
|
|
bool profile_lookup_locked(double t_ms, double* out)
|
|
|
{
|
|
|
size_t n = g_profile.size();
|
|
|
if (n < 2) return false;
|
|
|
// 表首之前：按前两点斜率回推（通常 t0=0，不会进入）
|
|
|
if (t_ms <= g_profile[0].first)
|
|
|
{
|
|
|
double dt = g_profile[1].first - g_profile[0].first;
|
|
|
double dz = g_profile[1].second - g_profile[0].second;
|
|
|
double slope = (dt > 0) ? (dz / dt) : 0.0;
|
|
|
*out = g_profile[0].second + slope * (t_ms - g_profile[0].first);
|
|
|
return true;
|
|
|
}
|
|
|
// 表末之后（需求5）：按最后两条的平均速度继续延伸
|
|
|
if (t_ms >= g_profile[n - 1].first)
|
|
|
{
|
|
|
double dt = g_profile[n - 1].first - g_profile[n - 2].first;
|
|
|
double dz = g_profile[n - 1].second - g_profile[n - 2].second;
|
|
|
double slope = (dt > 0) ? (dz / dt) : 0.0;
|
|
|
*out = g_profile[n - 1].second + slope * (t_ms - g_profile[n - 1].first);
|
|
|
return true;
|
|
|
}
|
|
|
// 线性扫区间插值（表一般几百行，足够快）
|
|
|
for (size_t i = 0; i + 1 < n; i++)
|
|
|
{
|
|
|
if (t_ms <= g_profile[i + 1].first)
|
|
|
{
|
|
|
double a = g_profile[i].first, b = g_profile[i + 1].first;
|
|
|
double f = (b > a) ? ((t_ms - a) / (b - a)) : 0.0;
|
|
|
*out = g_profile[i].second +
|
|
|
(g_profile[i + 1].second - g_profile[i].second) * f;
|
|
|
return true;
|
|
|
}
|
|
|
}
|
|
|
return false;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
bool load_speed_profile(const std::string& path)
|
|
|
{
|
|
|
std::vector<std::pair<double, double>> rows;
|
|
|
std::ifstream fin(path);
|
|
|
if (fin.is_open())
|
|
|
{
|
|
|
std::string line;
|
|
|
while (std::getline(fin, line))
|
|
|
{
|
|
|
size_t b = line.find_first_not_of(" \t\r\n");
|
|
|
if (b == std::string::npos) continue; // 空行
|
|
|
if (line[b] == '#' || line[b] == ';') continue; // 注释
|
|
|
std::istringstream iss(line.substr(b));
|
|
|
double t = 0.0, v = 0.0;
|
|
|
if (!(iss >> t >> v)) continue;
|
|
|
if (!rows.empty() && t <= rows.back().first)
|
|
|
{
|
|
|
std::cout << "[speed] 警告: speed.txt 第" << (rows.size() + 1)
|
|
|
<< "行时间未递增(" << t << ")，已跳过" << std::endl;
|
|
|
continue;
|
|
|
}
|
|
|
rows.push_back(std::make_pair(t, v));
|
|
|
}
|
|
|
fin.close();
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
std::cout << "[speed] 未找到 " << path << "，使用匀速 z_speed="
|
|
|
<< radar_cfg().scan.z_speed << "cm/s" << std::endl;
|
|
|
}
|
|
|
|
|
|
std::lock_guard<std::mutex> lk(g_prof_mtx);
|
|
|
if (rows.size() < 2)
|
|
|
{
|
|
|
g_profile.clear();
|
|
|
return false;
|
|
|
}
|
|
|
g_profile.swap(rows);
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
void reload_speed_profile()
|
|
|
{
|
|
|
if (radar_cfg().scan.z_speed_type == "configuration")
|
|
|
{
|
|
|
if (load_speed_profile())
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(g_prof_mtx);
|
|
|
std::cout << "[speed] 变速剖面已加载(" << g_profile.size() << "行): "
|
|
|
<< "首点(t=" << g_profile.front().first << "ms, z="
|
|
|
<< g_profile.front().second << "mm), 末点(t="
|
|
|
<< g_profile.back().first << "ms, z="
|
|
|
<< g_profile.back().second << "mm); 超出末点时间按最后两点平均速度延伸"
|
|
|
<< std::endl;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
std::cout << "[speed] 剖面无效，回退匀速 z_speed=" << radar_cfg().scan.z_speed
|
|
|
<< "cm/s(0=时间)" << std::endl;
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(g_prof_mtx);
|
|
|
g_profile.clear();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
bool speed_profile_loaded()
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(g_prof_mtx);
|
|
|
return g_profile.size() >= 2;
|
|
|
}
|
|
|
|
|
|
double speed_profile_avg_velocity_mm_s()
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(g_prof_mtx);
|
|
|
if (g_profile.size() < 2) return 0.0;
|
|
|
double dt_ms = g_profile.back().first - g_profile.front().first;
|
|
|
double dz = g_profile.back().second - g_profile.front().second;
|
|
|
if (dt_ms <= 0.0) return 0.0;
|
|
|
double v = dz / (dt_ms / 1000.0);
|
|
|
return (v >= 0) ? v : -v;
|
|
|
}
|
|
|
|
|
|
bool z_is_distance_mode()
|
|
|
{
|
|
|
if (radar_cfg().scan.z_speed_type == "configuration")
|
|
|
return speed_profile_loaded() || radar_cfg().scan.z_speed > 0.0;
|
|
|
return radar_cfg().scan.z_speed > 0.0;
|
|
|
}
|
|
|
|
|
|
double z_for_elapsed(double elapsed_ms)
|
|
|
{
|
|
|
if (radar_cfg().scan.z_speed_type == "configuration")
|
|
|
{
|
|
|
double z = 0.0;
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(g_prof_mtx);
|
|
|
if (profile_lookup_locked(elapsed_ms, &z)) return z;
|
|
|
}
|
|
|
// 表无效/不足两行 → 回退匀速 radar_cfg().scan.z_speed（无 speed.txt 时的兜底）
|
|
|
}
|
|
|
return (radar_cfg().scan.z_speed > 0)
|
|
|
? (radar_cfg().scan.z_speed * 10.0 * elapsed_ms / 1000.0) // cm/s ×10 = mm/s, ×秒 = mm
|
|
|
: (double)elapsed_ms;
|
|
|
}
|
|
|
|
|
|
// ConfigureSpeed 标定采集：雷达横放时物体沿 Y 移动，逐帧取(过滤后)Y最大值
|
|
|
// 即该时刻被测物在行进方向上的距离，生成 speed.txt 供变速识别使用。
|
|
|
int collect_speed_profile(pavo2::pavo2_driver& driver, double seconds,
|
|
|
std::string& err)
|
|
|
{
|
|
|
const std::string path = "./config/speed.txt";
|
|
|
int flushed = drain_stale_scans(driver, 500);
|
|
|
if (flushed > 0)
|
|
|
std::cout << "[ConfigureSpeed] 已丢弃积压扫描包 " << flushed << " 个" << std::endl;
|
|
|
|
|
|
auto t0 = std::chrono::steady_clock::now();
|
|
|
const double target_ms = seconds * 1000.0;
|
|
|
std::vector<std::pair<double, double>> rows; // (elapsed_ms, y_max_mm)
|
|
|
rows.reserve((size_t)(seconds * 200.0) + 64);
|
|
|
|
|
|
while (true)
|
|
|
{
|
|
|
pavo2::pavo_packet_info_scan_t scan_info;
|
|
|
bool got = driver.get_scanned_data(scan_info, 100); // 100ms轮询
|
|
|
double elapsed_ms = (double)std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
|
std::chrono::steady_clock::now() - t0).count();
|
|
|
if (got)
|
|
|
{
|
|
|
double max_y = -1e18;
|
|
|
for (const auto& s : scan_info.vec_buff)
|
|
|
{
|
|
|
double angle_rad = (s.angle / 100.0) * 3.14159265 / 180.0;
|
|
|
double x_mm = s.distance * sin(angle_rad) * 2.0;
|
|
|
double y_mm = -s.distance * cos(angle_rad) * 2.0;
|
|
|
if (!xy_filter_allowed(x_mm, y_mm)) continue;
|
|
|
if (y_mm > max_y) max_y = y_mm;
|
|
|
}
|
|
|
if (max_y > -1e17)
|
|
|
rows.push_back(std::make_pair(elapsed_ms, max_y));
|
|
|
}
|
|
|
if (elapsed_ms >= target_ms) break;
|
|
|
}
|
|
|
|
|
|
if (rows.size() < 2)
|
|
|
{
|
|
|
err = "采集样本过少(未收到有效雷达帧)";
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
// 平滑：抑制与总体趋势反向且超过5mm的抖动，保持时间→z 单调可用
|
|
|
double trend = (rows.back().second >= rows.front().second) ? 1.0 : -1.0;
|
|
|
const double tol = 5.0;
|
|
|
for (size_t i = 1; i < rows.size(); i++)
|
|
|
if ((rows[i].second - rows[i - 1].second) * trend < -tol)
|
|
|
rows[i].second = rows[i - 1].second;
|
|
|
|
|
|
// 写文件
|
|
|
std::ofstream f(path);
|
|
|
if (!f.is_open())
|
|
|
{
|
|
|
err = "无法写入 " + path;
|
|
|
return -1;
|
|
|
}
|
|
|
f << "# speed.txt - 变速运动z轴速度剖面(每行: 时间ms 距离mm; 可由ConfigureSpeed生成或手工编辑)\n";
|
|
|
f << "# 首行后请勿保留其他说明; 时间须递增。radar_cfg().scan.z_speed_type=configuration 时生效,\n";
|
|
|
f << "# 识别时间超出表末按最后两行平均速度延伸。\n";
|
|
|
f << std::fixed << std::setprecision(3);
|
|
|
for (const auto& r : rows)
|
|
|
f << (long long)std::lround(r.first) << " " << r.second << "\n";
|
|
|
f.close();
|
|
|
|
|
|
// 载入为新剖面（无论当前类型是否 configuration，先保存供切换后立即使用）
|
|
|
load_speed_profile(path);
|
|
|
err.clear();
|
|
|
return (int)rows.size();
|
|
|
}
|
|
|
|
|
|
size_t scan_convert_points(const pavo2::pavo_packet_info_scan_t& scan_info,
|
|
|
double z_val, std::vector<CountPoint>& out)
|
|
|
{
|
|
|
size_t before = out.size();
|
|
|
for (const auto& s : scan_info.vec_buff)
|
|
|
{
|
|
|
double angle_deg = s.angle / 100.0;
|
|
|
double angle_rad = angle_deg * 3.14159265 / 180.0;
|
|
|
double x_mm = s.distance * sin(angle_rad) * 2.0;
|
|
|
double y_mm = -s.distance * cos(angle_rad) * 2.0;
|
|
|
if (!xy_filter_allowed(x_mm, y_mm)) continue;
|
|
|
out.push_back({x_mm, y_mm, z_val});
|
|
|
}
|
|
|
return out.size() - before;
|
|
|
}
|
|
|
|
|
|
// ==================== 扫描会话 ====================
|
|
|
// 析构必须 join：std::thread 在仍 joinable 时析构会调用 std::terminate → abort，
|
|
|
// 进程瞬间消失、无异常、无日志。这是"某台机器上运行一会儿就没了"的典型根因。
|
|
|
ScanSession::~ScanSession()
|
|
|
{
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
stop_req_ = true;
|
|
|
}
|
|
|
if (th_.joinable()) th_.join();
|
|
|
}
|
|
|
|
|
|
bool ScanSession::start(pavo2::pavo2_driver& driver, const std::string& radar_id,
|
|
|
std::mutex* io_mtx)
|
|
|
{
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
if (running_) return false;
|
|
|
if (th_.joinable()) return false; // 上一轮线程尚未回收，禁止叠加启动
|
|
|
running_ = true;
|
|
|
stop_req_ = false;
|
|
|
radar_id_ = radar_id;
|
|
|
io_mtx_ = io_mtx;
|
|
|
}
|
|
|
try
|
|
|
{
|
|
|
th_ = std::thread(&ScanSession::collect_loop, this, &driver);
|
|
|
return true;
|
|
|
}
|
|
|
catch (...)
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
running_ = false;
|
|
|
stop_req_ = false;
|
|
|
io_mtx_ = nullptr;
|
|
|
std::cerr << "[错误] 采集线程创建失败(资源不足?)" << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 无条件置停止位 + 无条件 join。
|
|
|
// 原实现 "if (!running_) return;" 会在 running_ 为 false 但线程仍活着时漏掉 join，
|
|
|
// 残留的 joinable 线程随后在析构时触发 std::terminate。
|
|
|
size_t ScanSession::stop()
|
|
|
{
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
stop_req_ = true;
|
|
|
}
|
|
|
if (th_.joinable()) th_.join();
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
running_ = false;
|
|
|
io_mtx_ = nullptr;
|
|
|
return points_.size();
|
|
|
}
|
|
|
|
|
|
std::vector<CountPoint> ScanSession::snapshot() const
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
return points_;
|
|
|
}
|
|
|
|
|
|
bool ScanSession::running() const
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
return running_;
|
|
|
}
|
|
|
|
|
|
// 复位前必须先让采集线程退出：原实现持锁直接 clear points_，
|
|
|
// 若线程尚在运行会与 points_ 并发读写(vector 重分配时必崩)，
|
|
|
// 同时 running_ 被置 false 后 stop() 不再 join，线程变成野线程。
|
|
|
void ScanSession::reset()
|
|
|
{
|
|
|
stop(); // 内部持锁 + join，确保线程已结束
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
points_.clear();
|
|
|
running_ = false;
|
|
|
stop_req_ = false;
|
|
|
radar_id_.clear();
|
|
|
}
|
|
|
|
|
|
void ScanSession::collect_loop(pavo2::pavo2_driver* driver)
|
|
|
{
|
|
|
std::string rid;
|
|
|
std::mutex* iom = nullptr;
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
rid = radar_id_;
|
|
|
iom = io_mtx_;
|
|
|
}
|
|
|
RadarScope scope(rid); // 线程内绑定该雷达的配置(多边形/z轴换算等)
|
|
|
|
|
|
// 排空积压：丢弃 SDK 缓存中 start 之前雷达已转出的旧数据包。
|
|
|
// 否则旧包会在图像开头 z 方向挤成一段密集点；排空完成才开始计时。
|
|
|
// 与重连线程互斥（重连时 driver/socket 处于中间态，并发访问会崩）。
|
|
|
int flushed = 0;
|
|
|
if (iom) { std::lock_guard<std::mutex> lk(*iom); flushed = drain_stale_scans(*driver); }
|
|
|
else { flushed = drain_stale_scans(*driver); }
|
|
|
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
if (stop_req_) { running_ = false; return; } // 排空期间被 stop，直接退出
|
|
|
start_tp_ = std::chrono::steady_clock::now(); // 排空完成才开始计时
|
|
|
}
|
|
|
if (flushed > 0)
|
|
|
std::cout << "[lidar] 已丢弃积压扫描包 " << flushed << " 个" << std::endl;
|
|
|
|
|
|
while (true)
|
|
|
{
|
|
|
pavo2::pavo_packet_info_scan_t scan_info;
|
|
|
bool got = false;
|
|
|
if (iom)
|
|
|
{
|
|
|
// try_lock 而非 lock：重连线程可能正长时间占用(连接超时)，
|
|
|
// 采集线程不能因此卡死，等下一轮即可
|
|
|
if (!iom->try_lock())
|
|
|
{
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
if (stop_req_) break;
|
|
|
continue;
|
|
|
}
|
|
|
got = driver->get_scanned_data(scan_info, 100); // 100ms超时轮询，可及时响应stop
|
|
|
iom->unlock();
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
got = driver->get_scanned_data(scan_info, 100);
|
|
|
}
|
|
|
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
if (stop_req_) break;
|
|
|
if (got)
|
|
|
{
|
|
|
auto now = std::chrono::steady_clock::now();
|
|
|
double elapsed_ms = (double)std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
|
now - start_tp_).count();
|
|
|
scan_convert_points(scan_info, z_for_elapsed(elapsed_ms), points_);
|
|
|
}
|
|
|
}
|
|
|
// 线程即将结束：同步状态位，避免外部误判"仍在采集"而跳过重连
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
running_ = false;
|
|
|
}
|
|
|
|
|
|
// ==================== 多雷达管理实现 ====================
|
|
|
RadarManager radar_mgr;
|
|
|
|
|
|
RadarManager::~RadarManager()
|
|
|
{
|
|
|
if (!stop_.load()) shutdown(); // 兜底：确保连接线程被 join
|
|
|
}
|
|
|
|
|
|
void RadarManager::init()
|
|
|
{
|
|
|
sync_config();
|
|
|
}
|
|
|
|
|
|
// 配置重载后同步节点：新增配置中出现的雷达；配置里删掉的仅置为失效（节点/线程保留，避免悬空）
|
|
|
void RadarManager::sync_config()
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
for (const auto& id : cfg.ids)
|
|
|
{
|
|
|
RadarNode* exist = nullptr;
|
|
|
for (auto* n : nodes_) if (n->id == id) { exist = n; break; }
|
|
|
if (exist) { exist->enabled = true; continue; }
|
|
|
|
|
|
RadarNode* n = new RadarNode();
|
|
|
n->id = id;
|
|
|
n->driver = new pavo2::pavo2_driver();
|
|
|
nodes_.push_back(n);
|
|
|
std::cout << "[雷达] 新增雷达节点: " << id << " (后台异步连接中)" << std::endl;
|
|
|
try
|
|
|
{
|
|
|
ths_.push_back(std::thread(&RadarManager::connect_loop, this, n));
|
|
|
}
|
|
|
catch (...)
|
|
|
{
|
|
|
std::cerr << "[错误] 雷达 " << id << " 连接线程创建失败!" << std::endl;
|
|
|
}
|
|
|
}
|
|
|
for (auto* n : nodes_)
|
|
|
{
|
|
|
bool keep = std::find(cfg.ids.begin(), cfg.ids.end(), n->id) != cfg.ids.end();
|
|
|
if (n->enabled && !keep)
|
|
|
{
|
|
|
n->enabled = false;
|
|
|
n->connected = false;
|
|
|
n->session.stop();
|
|
|
std::cout << "[雷达] 配置中已移除，节点停用: " << n->id << std::endl;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
RadarNode* RadarManager::find(const std::string& id) const
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
if (!id.empty())
|
|
|
{
|
|
|
for (auto* n : nodes_)
|
|
|
if (n->id == id && n->enabled) return n;
|
|
|
return nullptr; // 指定标识但不存在：由调用方报错
|
|
|
}
|
|
|
for (auto* n : nodes_)
|
|
|
if (n->enabled) return n; // 默认雷达 = 第一个有效节点
|
|
|
return nullptr;
|
|
|
}
|
|
|
|
|
|
std::vector<RadarNode*> RadarManager::nodes() const
|
|
|
{
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
return nodes_;
|
|
|
}
|
|
|
|
|
|
std::string RadarManager::default_id() const
|
|
|
{
|
|
|
RadarNode* n = find("");
|
|
|
return n ? n->id : std::string();
|
|
|
}
|
|
|
|
|
|
bool RadarManager::wait_connected(const std::string& id, int timeout_ms)
|
|
|
{
|
|
|
RadarNode* n = find(id);
|
|
|
if (!n) return false;
|
|
|
auto t0 = std::chrono::steady_clock::now();
|
|
|
while (!n->connected.load())
|
|
|
{
|
|
|
if (std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
|
std::chrono::steady_clock::now() - t0).count() > timeout_ms)
|
|
|
return false;
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
}
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
// 后台连接线程：未连接就一直重试；已连接则在空闲时探测掉线，掉线后重新进入重试
|
|
|
void RadarManager::connect_loop(RadarNode* n)
|
|
|
{
|
|
|
RadarScope scope(n->id); // 线程内绑定该雷达配置（打印/参数用）
|
|
|
int retry = 0;
|
|
|
while (!stop_.load())
|
|
|
{
|
|
|
if (n->connected.load())
|
|
|
{
|
|
|
// 扫描中不动 driver，避免与采集线程并发使用同一连接；
|
|
|
// pavo2_is_open() 也必须在 io_mtx 内调用（采集取帧同样持有该锁），
|
|
|
// 否则会出现"重连关闭 socket 的同时采集正在读"的并发访问 → 崩溃。
|
|
|
if (!n->session.running() && n->io_mtx.try_lock())
|
|
|
{
|
|
|
bool open = n->driver->pavo2_is_open();
|
|
|
n->io_mtx.unlock();
|
|
|
if (!open)
|
|
|
{
|
|
|
n->connected = false;
|
|
|
std::cout << "[连接][" << n->id << "] 连接已断开，准备重连" << std::endl;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
else if (n->enabled.load())
|
|
|
{
|
|
|
std::shared_ptr<const RadarConfig> rc = cfg.find(n->id);
|
|
|
if (rc)
|
|
|
{
|
|
|
RadarConfig copy = *rc; // 拷贝，避免 load_config 重载时引用失效
|
|
|
std::lock_guard<std::mutex> lk(n->io_mtx);
|
|
|
if (connect_lidar(*n->driver, copy))
|
|
|
{
|
|
|
n->connected = true;
|
|
|
retry = 0;
|
|
|
}
|
|
|
else if (++retry == 1 || retry % 10 == 0)
|
|
|
{
|
|
|
std::cout << "[连接][" << n->id << "] 未连接(第" << retry
|
|
|
<< "次重试)，将持续尝试..." << std::endl;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
// 可中断等待（100ms 粒度）
|
|
|
for (int i = 0; i < RECONNECT_INTERVAL_MS / 100 && !stop_.load(); ++i)
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void RadarManager::shutdown()
|
|
|
{
|
|
|
stop_ = true;
|
|
|
for (auto& t : ths_) if (t.joinable()) t.join();
|
|
|
ths_.clear();
|
|
|
|
|
|
std::lock_guard<std::mutex> lk(mtx_);
|
|
|
for (auto* n : nodes_)
|
|
|
{
|
|
|
n->session.stop();
|
|
|
if (n->driver)
|
|
|
{
|
|
|
n->driver->pavo2_close();
|
|
|
delete n->driver;
|
|
|
n->driver = nullptr;
|
|
|
}
|
|
|
delete n;
|
|
|
}
|
|
|
nodes_.clear();
|
|
|
}
|