leida_simi/lidar_utils.cpp

// 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();
}