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)
{
    driver.set_conn_protocol(pavo2::PAVO2_PROTO_TYPE::TCP);

    std::cout << "[连接] 正在连接雷达 " << cfg.radar.ip << ":" << cfg.radar.port << " ..." << std::endl;

    if (!driver.pavo2_open(cfg.radar.ip, cfg.radar.port, OPEN_TIMEOUT_MS))
    {
        std::cerr << "[错误] 连接雷达失败！请检查IP和网络连接。" << std::endl;
        return false;
    }

    std::cout << "[连接] 雷达连接成功！" << std::endl;

    driver.enable_tail_filter(cfg.radar.tail_filter);
    driver.enable_front_filter(cfg.radar.front_filter);
    driver.enable_far_hr_filter(cfg.radar.far_hr_filter);
    driver.enable_near_hr(cfg.radar.near_hr_filter);

    std::string sn, pn, sdk_ver, fw_ver;
    if (driver.get_device_sn(sn))
        std::cout << "[信息] 设备SN: " << sn << std::endl;
    if (driver.get_device_pn(pn))
        std::cout << "[信息] 设备PN: " << pn << std::endl;
    if (driver.get_sdk_ver(sdk_ver))
        std::cout << "[信息] SDK版本: " << sdk_ver << std::endl;
    if (driver.get_firmware_ver(fw_ver))
        std::cout << "[信息] 固件版本: " << fw_ver << std::endl;

    if (!driver.enable_data(true))
    {
        std::cerr << "[错误] 开启数据传输失败!" << std::endl;
        return false;
    }

    std::cout << "[信息] 雷达数据传输已开启。" << 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="
                  << 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 (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=" << 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 (cfg.scan.z_speed_type == "configuration")
        return speed_profile_loaded() || cfg.scan.z_speed > 0.0;
    return cfg.scan.z_speed > 0.0;
}

double z_for_elapsed(double elapsed_ms)
{
    if (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;
        }
        // 表无效/不足两行 → 回退匀速 cfg.scan.z_speed（无 speed.txt 时的兜底）
    }
    return (cfg.scan.z_speed > 0)
        ? (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 << "# 首行后请勿保留其他说明; 时间须递增。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;
}

// ==================== 扫描会话 ====================
bool ScanSession::start(pavo2::pavo2_driver& driver)
{
    {
        std::lock_guard<std::mutex> lk(mtx_);
        if (running_) return false;
        running_ = true;
        stop_req_ = false;
    }
    try
    {
        th_ = std::thread(&ScanSession::collect_loop, this, &driver);
        return true;
    }
    catch (...)
    {
        std::lock_guard<std::mutex> lk(mtx_);
        running_ = false;
        stop_req_ = false;
        return false;
    }
}

size_t ScanSession::stop()
{
    {
        std::lock_guard<std::mutex> lk(mtx_);
        if (!running_) return points_.size();
        stop_req_ = true;
    }
    if (th_.joinable()) th_.join();
    std::lock_guard<std::mutex> lk(mtx_);
    running_ = false;
    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_;
}

void ScanSession::reset()
{
    std::lock_guard<std::mutex> lk(mtx_);
    points_.clear();
    running_ = false;
    stop_req_ = false;
}

void ScanSession::collect_loop(pavo2::pavo2_driver* driver)
{
    // 排空积压：丢弃 SDK 缓存中 start 之前雷达已转出的旧数据包。
    // 否则旧包会在图像开头 z 方向挤成一段密集点；排空完成才开始计时。
    int flushed = drain_stale_scans(*driver);

    {
        std::lock_guard<std::mutex> lk(mtx_);
        if (stop_req_) 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;
        if (driver->get_scanned_data(scan_info, 100))   // 100ms超时轮询，可及时响应stop
        {
            auto now = std::chrono::steady_clock::now();
            double elapsed_ms = (double)std::chrono::duration_cast<std::chrono::milliseconds>(
                                    now - start_tp_).count();
            double z_val = z_for_elapsed(elapsed_ms);

            std::lock_guard<std::mutex> lk(mtx_);
            if (stop_req_) break;
            scan_convert_points(scan_info, z_val, points_);
        }
        else
        {
            std::lock_guard<std::mutex> lk(mtx_);
            if (stop_req_) break;
        }
    }
}