#include "lidar_fusion_node.h"
#include "time_sync.h"
#include "lidarheart.h" // 读取雷达状态
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <thread>
#include <functional>
#include <chrono>
#include <map>
#include <limits>
#include <params.h>
#include <atomic>
#include <std_msgs/Empty.h>
#include <cmath>
#include <cstdint>
// 可视化预测圆柱
#include "visualization.h"

// 记录最近一次有效拟合的时间戳与延迟
namespace
{
    std::atomic<double> s_last_c1_stamp_sec{0.0};
    std::atomic<double> s_last_c1_delay_sec{0.0};
    std::atomic<double> s_last_c2_stamp_sec{0.0};
    std::atomic<double> s_last_c2_delay_sec{0.0};
}

LidarFusionNode::LidarFusionNode(ros::NodeHandle &pnh, bool wait_for_master, double master_timeout_sec) : pnh_(pnh),
                                                                                                          c1_thread_running_(true),
                                                                                                          c2_thread_running_(true),
                                                                                                          c1_thread_pool_(4),
                                                                                                          c2_thread_pool_(4),
                                                                                                          c1_fit_pool_(2),
                                                                                                          c2_fit_pool_(2),
                                                                                                          c1_fitter(loadLidarFusionParams(pnh)),
                                                                                                          c2_fitter(loadLidarFusionParams(pnh)),
                                                                                                          c1_start_time_(0),
                                                                                                          c2_start_time_(0)
{
    // 首次加载全部参数
    params_ = loadLidarFusionParams(pnh_);

    // 构建两组雷达的预测器
    c1_predictor_ = std::make_unique<PosePredictor>();
    c2_predictor_ = std::make_unique<PosePredictor>();
    if (params_.kf_init_center_speed.size() == 3)
    {
        Eigen::Vector3d v0(params_.kf_init_center_speed[0], params_.kf_init_center_speed[1], params_.kf_init_center_speed[2]);
        c1_predictor_->setInitCenterVelocity(v0);
        c2_predictor_->setInitCenterVelocity(v0);
    }
    c1_predictor_->setAccelThreshold(params_.accel_threshold_mps2);
    c1_predictor_->setSpeedThreshold(params_.speed_threshold_mps);
    c1_predictor_->setForceUpdateInterval(params_.force_update_interval_s);
    c2_predictor_->setAccelThreshold(params_.accel_threshold_mps2);
    c2_predictor_->setSpeedThreshold(params_.speed_threshold_mps);
    c2_predictor_->setForceUpdateInterval(params_.force_update_interval_s);

    // 区分 C1 / C2 拟合器来源，后续可用于选择不同可视化话题
    c1_fitter.setSourceId(1);
    c2_fitter.setSourceId(2);

    wait_for_master_ = wait_for_master;
    master_timeout_sec_ = master_timeout_sec;
    if (wait_for_master_)
    {
        waitForMaster(master_timeout_sec_);
    }
    std::vector<double> C1T2_to_1_vec, C1T3_to_1_vec, C1T4_to_1_vec, C1T1_to_S_vec, C1S_to_Ship_vec;
    std::vector<double> C2T2_to_1_vec, C2T3_to_1_vec, C2T4_to_1_vec, C2T1_to_S_vec, C2S_to_Ship_vec;
    auto params = params_; // 使用缓存的参数

    C1T2_to_1_vec = params.C1T2_to_1_vec; // 加载单机1外参
    C1T3_to_1_vec = params.C1T3_to_1_vec;
    C1T4_to_1_vec = params.C1T4_to_1_vec;
    C1T1_to_S_vec = params.C1T1_to_S_vec;
    C1S_to_Ship_vec = params.C1S_to_Ship_vec;

    C2T2_to_1_vec = params.C2T2_to_1_vec; // 加载单机2外参
    C2T3_to_1_vec = params.C2T3_to_1_vec;
    C2T4_to_1_vec = params.C2T4_to_1_vec;
    C2T1_to_S_vec = params.C2T1_to_S_vec;
    C2S_to_Ship_vec = params.C2S_to_Ship_vec;

    Comb1lidar1 = params.Comb1lidar1; // 加载话题名称
    Comb1lidar2 = params.Comb1lidar2;
    Comb1lidar3 = params.Comb1lidar3;
    Comb1lidar4 = params.Comb1lidar4;
    Comb1cloud = params.Comb1cloud;
    Comb2lidar1 = params.Comb2lidar1;
    Comb2lidar2 = params.Comb2lidar2;
    Comb2lidar3 = params.Comb2lidar3;
    Comb2lidar4 = params.Comb2lidar4;
    Comb2cloud = params.Comb2cloud;
    std::vector<float> roi1 = params.roi1;     // C1 第1块 ROI
    std::vector<float> roi1_b = params.roi1_b; // C1 第2块 ROI
    std::vector<float> roi2 = params.roi2;     // C2 第1块 ROI
    std::vector<float> roi2_b = params.roi2_b; // C2 第2块 ROI
    pixel_size = params.pixel_size;            // 加载预处理部分横向滤波纵向滤波的像素大小
    // 缓存过滤阈值参数
    heightmap_min_diff_ = params.heightmap_min_diff;
    widthmap_max_diff_ = params.widthmap_max_diff;
    morph_min_contour_points_ = params.morph_min_contour_points;
    morph_ratio_min_ = params.morph_ratio_min;
    morph_ratio_max_ = params.morph_ratio_max;
    rope_filter_linearity_thresh_ = params_.rope_filter_linearity_thresh;
    rocket_extract_voxel_ratio_ = params_.rocket_extract_voxel_ratio;
    rocket_cluster_threshold_ = params_.rocket_cluster_threshold;
    rocket_cluster_min_size_ = params_.rocket_cluster_min_size;
    rope_filter_roi_.z_min = static_cast<float>(params_.rope_filter_roi.z_min);
    rope_filter_roi_.z_max = static_cast<float>(params_.rope_filter_roi.z_max);
    rope_filter_roi_.x_min = static_cast<float>(params_.rope_filter_roi.x_min);
    rope_filter_roi_.x_max = static_cast<float>(params_.rope_filter_roi.x_max);
    rope_filter_roi_.y_min = static_cast<float>(params_.rope_filter_roi.y_min);
    rope_filter_roi_.y_max = static_cast<float>(params_.rope_filter_roi.y_max);

    // 组合两块 ROI：前 6 个为 roi_filter1，后 6 个为 roi_filter1_b
    if (roi1.size() == 6 && roi1_b.size() == 6)
    {
        std::copy(roi1.begin(), roi1.end(), roi_1.begin());         // [0..5]
        std::copy(roi1_b.begin(), roi1_b.end(), roi_1.begin() + 6); // [6..11]
    }

    if (roi2.size() == 6 && roi2_b.size() == 6)
    {
        std::copy(roi2.begin(), roi2.end(), roi_2.begin());         // [0..5]
        std::copy(roi2_b.begin(), roi2_b.end(), roi_2.begin() + 6); // [6..11]
    }

    C1T2_to_1f_ = vecToAffine3f(C1T2_to_1_vec); // 将单机1外参向量转换为仿射矩阵
    C1T3_to_1f_ = vecToAffine3f(C1T3_to_1_vec);
    C1T4_to_1f_ = vecToAffine3f(C1T4_to_1_vec);
    C1T1_to_Sf_ = vecToAffine3f(C1T1_to_S_vec);
    C1Sf_to_Shipf_ = vecToAffine3f(C1S_to_Ship_vec);

    C2T2_to_1f_ = vecToAffine3f(C2T2_to_1_vec);
    C2T3_to_1f_ = vecToAffine3f(C2T3_to_1_vec);
    C2T4_to_1f_ = vecToAffine3f(C2T4_to_1_vec);
    C2T1_to_Sf_ = vecToAffine3f(C2T1_to_S_vec);
    C2Sf_to_Shipf_ = vecToAffine3f(C2S_to_Ship_vec);

    C1T1_to_Shipf_ = mergeTransforms(Eigen::Affine3f::Identity(), C1T1_to_Sf_, C1Sf_to_Shipf_); // 单机1雷达1到船体
    C1T2_to_Shipf_ = mergeTransforms(C1T2_to_1f_, C1T1_to_Sf_, C1Sf_to_Shipf_);                 // 单机1雷达2到船体
    C1T3_to_Shipf_ = mergeTransforms(C1T3_to_1f_, C1T1_to_Sf_, C1Sf_to_Shipf_);                 // 单机1雷达3到船体
    C1T4_to_Shipf_ = mergeTransforms(C1T4_to_1f_, C1T1_to_Sf_, C1Sf_to_Shipf_);                 // 单机1雷达4到船体

    C2T1_to_Shipf_ = mergeTransforms(Eigen::Affine3f::Identity(), C2T1_to_Sf_, C2Sf_to_Shipf_); // 单机2雷达1到船体
    C2T2_to_Shipf_ = mergeTransforms(C2T2_to_1f_, C2T1_to_Sf_, C2Sf_to_Shipf_);                 // 单机2雷达2到船体
    C2T3_to_Shipf_ = mergeTransforms(C2T3_to_1f_, C2T1_to_Sf_, C2Sf_to_Shipf_);                 // 单机2雷达3到船体
    C2T4_to_Shipf_ = mergeTransforms(C2T4_to_1f_, C2T1_to_Sf_, C2Sf_to_Shipf_);                 // 单机2雷达4到船体

    // 盲区平面转换到 ship 坐标系（仅使用 lidar1_to_shell 与 shell_to_ship，顺序为先 lidar1_to_shell 后 shell_to_ship）
    auto vec4_to_f = [](const std::vector<double> &v) -> Eigen::Vector4f
    {
        if (v.size() != 4)
            return Eigen::Vector4f::Zero();
        return Eigen::Vector4f(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2]), static_cast<float>(v[3]));
    };
    auto transform_plane = [](const Eigen::Matrix4f &H, const Eigen::Vector4f &pi) -> Eigen::Vector4f
    {
        Eigen::Matrix4f HinvT = H.inverse().transpose();
        return HinvT * pi;
    };
    // C1
    {
        Eigen::Matrix4f H = (C1Sf_to_Shipf_ * C1T1_to_Sf_).matrix();
        c1_blind_planes_ship_[0] = transform_plane(H, vec4_to_f(params_.C1_blind_34_lidar4_plane));
        c1_blind_planes_ship_[1] = transform_plane(H, vec4_to_f(params_.C1_blind_34_lidar3_plane));
        c1_blind_planes_ship_[2] = transform_plane(H, vec4_to_f(params_.C1_blind_23_lidar3_plane));
        c1_blind_planes_ship_[3] = transform_plane(H, vec4_to_f(params_.C1_blind_23_lidar2_plane));
        c1_blind_planes_ship_[4] = transform_plane(H, vec4_to_f(params_.C1_blind_1_lidar1_plane));
    }
    // C2
    {
        Eigen::Matrix4f H = (C2Sf_to_Shipf_ * C2T1_to_Sf_).matrix();
        c2_blind_planes_ship_[0] = transform_plane(H, vec4_to_f(params_.C2_blind_34_lidar4_plane));
        c2_blind_planes_ship_[1] = transform_plane(H, vec4_to_f(params_.C2_blind_34_lidar3_plane));
        c2_blind_planes_ship_[2] = transform_plane(H, vec4_to_f(params_.C2_blind_23_lidar3_plane));
        c2_blind_planes_ship_[3] = transform_plane(H, vec4_to_f(params_.C2_blind_23_lidar2_plane));
        c2_blind_planes_ship_[4] = transform_plane(H, vec4_to_f(params_.C2_blind_1_lidar1_plane));
    }

    // 订阅雷达点云话题
    c1_sub1_ = pnh_.subscribe(Comb1lidar1, 10, &LidarFusionNode::c1_cb1, this, ros::TransportHints().tcpNoDelay());
    c1_sub2_ = pnh_.subscribe(Comb1lidar2, 10, &LidarFusionNode::c1_cb2, this, ros::TransportHints().tcpNoDelay());
    c1_sub3_ = pnh_.subscribe(Comb1lidar3, 10, &LidarFusionNode::c1_cb3, this, ros::TransportHints().tcpNoDelay());
    c1_sub4_ = pnh_.subscribe(Comb1lidar4, 10, &LidarFusionNode::c1_cb4, this, ros::TransportHints().tcpNoDelay());

    c2_sub1_ = pnh_.subscribe(Comb2lidar1, 10, &LidarFusionNode::c2_cb1, this, ros::TransportHints().tcpNoDelay());
    c2_sub2_ = pnh_.subscribe(Comb2lidar2, 10, &LidarFusionNode::c2_cb2, this, ros::TransportHints().tcpNoDelay());
    c2_sub3_ = pnh_.subscribe(Comb2lidar3, 10, &LidarFusionNode::c2_cb3, this, ros::TransportHints().tcpNoDelay());
    c2_sub4_ = pnh_.subscribe(Comb2lidar4, 10, &LidarFusionNode::c2_cb4, this, ros::TransportHints().tcpNoDelay());

    c1_pub_ = pnh_.advertise<sensor_msgs::PointCloud2>(Comb1cloud, 10);
    c2_pub_ = pnh_.advertise<sensor_msgs::PointCloud2>(Comb2cloud, 10);

    // ===== 注意：先读取参数再启动依赖这些参数的线程，避免线程读取到默认值 =====
    // （之前 softsync 线程在参数赋值之前启动，导致 c1_sync_wait_param_ / c2_sync_wait_param_ 仍是默认值）

    pnh_.param<int>("/common/simulation", simu, 0);
    pnh_.param<int>("/common/delay_ms", delay_JT_ms, 77);
    pnh_.param<string>("/common/canid", canid, "0x513");
    pnh_.param<string>("/common/sim_path", filename, "1.csv");
    pnh_.param<string>("/common/log_path", logpath, "/home/log");

    pnh_.param<string>("/common/serverip", sIP, "0xC0A8A0B6");
    pnh_.param<int>("/common/serverport", sPORT, 10188);
    pnh_.param<string>("/common/clientip", cIP, "0xC0A89791");
    pnh_.param<int>("/common/clientport", cPORT, 10188);

    pnh_.param<string>("/common/serverip_bak", sIP_bak, "0xC0A8A0B7");
    pnh_.param<int>("/common/serverport_bak", sPORT_bak, 10188);
    pnh_.param<string>("/common/clientip_bak", cIP_bak, "0xC0A89792");
    pnh_.param<int>("/common/clientport_bak", cPORT_bak, 10188);

    pnh_.param<double>("/coordinate/SheXiang", SheXiang_para, 0.0);
    pnh_.param<double>("/coordinate/Lever1", Lever_para[0], 0.0);
    pnh_.param<double>("/coordinate/Lever2", Lever_para[1], 0.0);
    pnh_.param<double>("/coordinate/Lever3", Lever_para[2], 0.0);

    pnh_.param<double>("/coordinate/JT1", JT_para[0], 0.0);
    pnh_.param<double>("/coordinate/JT2", JT_para[1], 0.0);
    pnh_.param<double>("/coordinate/JT3", JT_para[2], 0.0);

    pnh_.param<double>("/coordinate/GZatt1", GZ_para[0], 0.0);
    pnh_.param<double>("/coordinate/GZatt2", GZ_para[1], 0.0);
    pnh_.param<double>("/coordinate/GZatt3", GZ_para[2], 0.0);

    printf("%s", canid.c_str());
    can_main.setValue_id(std::stoi(canid, nullptr, 16));
    can_hpyo.setValue_id(std::stoi(canid, nullptr, 16));

    can_main.setValue_can(0);
    can_hpyo.setValue_can(1);
    can_main.set_path(logpath);
    can_hpyo.set_path(logpath);
    // can_main.setValue_mode(simu);
    // can_hpyo.setValue_mode(simu);
    // can_main.setValue_filepath(filename);
    // can_hpyo.setValue_filepath(filename);

    if (!readCsvFile(filename, csv_data_list))
    {
        ROS_ERROR("read CSV failed");
    }
    else
    {
        ROS_INFO("read file %s success", filename.c_str());
    }

    print_count = csv_data_list.size();

    logger.set_path(logpath);
    logger.start();

    can_main.start();
    can_hpyo.start();
    // logger.start();

    udp_client.set_path(logpath);
    udp_client.InitUDP(std::stoul(sIP, nullptr, 16), (unsigned int)sPORT, std::stoul(cIP, nullptr, 16), (unsigned int)cPORT);

    udp_client_bak.set_path(logpath);
    udp_client_bak.InitUDP(std::stoul(sIP_bak, nullptr, 16), (unsigned int)sPORT_bak, std::stoul(cIP_bak, nullptr, 16), (unsigned int)cPORT_bak);

    c1_sync_wait_param_ = params.c1_sync_wait_sec;
    c2_sync_wait_param_ = params.c2_sync_wait_sec;

    c1_SYNC_WAIT_ = ros::Duration(c1_sync_wait_param_);
    c2_SYNC_WAIT_ = ros::Duration(c2_sync_wait_param_);
    last_stat_time_ = ros::Time::now();

    // 决策等待时间参数
    decision_wait_sec_ = params.decision_wait_sec;
    // 启动结果对策线程
    // 线程启动顺序：确保 softsync 线程最后启动，这样其构造的 Inputs 能拿到更新后的 *_sync_wait_param_
    c1_merge_thread_ = std::thread(&LidarFusionNode::c1_mergeThreadLoop, this);
    c2_merge_thread_ = std::thread(&LidarFusionNode::c2_mergeThreadLoop, this);
    c1_sync_thread_ = std::thread(&LidarFusionNode::c1_doSyncWork, this);
    c2_sync_thread_ = std::thread(&LidarFusionNode::c2_doSyncWork, this);
    c1_softsync_thread_ = std::thread(&LidarFusionNode::c1_handleSoftSync, this);
    c2_softsync_thread_ = std::thread(&LidarFusionNode::c2_handleSoftSync, this);
    decision_thread_ = std::thread(&LidarFusionNode::decisionThreadLoop, this);

    double hb_timeout = params.lidar_heartbeat_timeout;
    int hb_mode = params.lidar_heartbeat_mode; // 0: bit=异常  1: bit=正常

    try
    {
        LidarHeart::setTimeout(hb_timeout);
        LidarHeart::setMode(hb_mode);
        ROS_INFO_STREAM("LidarHeart initialized: timeout=" << hb_timeout << "s mode=" << (hb_mode == 0 ? "bit=abnormal" : "bit=normal"));
    }
    catch (const std::exception &e)
    {
        ROS_ERROR_STREAM("Failed to initialize LidarHeart: " << e.what());
    }

    // 订阅运行期参数刷新命令
    reload_params_sub_ = pnh_.subscribe<std_msgs::Empty>("/reload_lidar_params", 1,
                                                         &LidarFusionNode::reloadParamsCallback, this);
}

// 分割字符串函数
std::vector<std::string> LidarFusionNode::split(const std::string &s, char delimiter)
{
    std::vector<std::string> tokens;
    std::string token;
    std::istringstream tokenStream(s);
    while (std::getline(tokenStream, token, delimiter))
    {
        tokens.push_back(token);
    }
    return tokens;
}

// 从CSV文件读取数据
bool LidarFusionNode::readCsvFile(const std::string &file_path, std::vector<CsvData> &data_list)
{
    std::ifstream file(file_path);
    if (!file.is_open())
    {
        ROS_ERROR("can not open CSV file: %s", file_path.c_str());
        return false;
    }

    std::string line;
    // 跳过表头
    if (std::getline(file, line))
    {
        ROS_INFO("ignore first line");
    }

    // 读取每一行数据
    while (std::getline(file, line))
    {
        std::vector<std::string> tokens = split(line, ',');

        // 检查列数是否正确
        if (tokens.size() != 21)
        {
            ROS_WARN("line data error,jump: %s", line.c_str());
            continue;
        }

        try
        {
            CsvData data;
            data.index = std::stoi(tokens[0]);
            data.timestamp = std::stod(tokens[1]);
            data.x = std::stod(tokens[2]);
            data.y = std::stod(tokens[3]);
            data.z = std::stod(tokens[4]);
            data.a = std::stod(tokens[5]);
            data.b = std::stod(tokens[6]);
            data.c = std::stod(tokens[7]);
            data.yaw = std::stod(tokens[8]);
            data.pitch = std::stod(tokens[9]);
            data.point_count = std::stoi(tokens[10]);
            data.residual_mean = std::stod(tokens[11]);
            data.is_cross_plane = (std::stoi(tokens[12]) != 0);
            data.delay = std::stod(tokens[13]);
            data.is_visible = (std::stoi(tokens[14]) != 0);
            data.is_prediction = (std::stoi(tokens[15]) != 0);
            data.radar_state = std::stoi(tokens[16]);
            data.each_lidarstate = std::stoi(tokens[17]);
            data.solve_state = std::stoi(tokens[18]);
            data.data_state =  std::stoi(tokens[19]);

            data_list.push_back(data);

            // printCsvData(data);
        }
        catch (const std::exception &e)
        {
            ROS_WARN("data error: %s, jump", e.what());
            continue;
        }
    }

    file.close();
    ROS_INFO("read file success, %zu data", data_list.size());
    return true;
}

void LidarFusionNode::reloadParamsCallback(const std_msgs::Empty::ConstPtr &)
{
    try
    {
        ROS_INFO("Reloading LidarFusionParams from parameter server...");
        LidarFusionParams new_params = loadLidarFusionParams(pnh_);
        params_ = new_params;

        // 同步运行期可更新的参数
        heightmap_min_diff_ = params_.heightmap_min_diff;
        widthmap_max_diff_ = params_.widthmap_max_diff;
        morph_min_contour_points_ = params_.morph_min_contour_points;
        morph_ratio_min_ = params_.morph_ratio_min;
        morph_ratio_max_ = params_.morph_ratio_max;
        rope_filter_linearity_thresh_ = params_.rope_filter_linearity_thresh;
        rocket_extract_voxel_ratio_ = params_.rocket_extract_voxel_ratio;
        rocket_cluster_threshold_ = params_.rocket_cluster_threshold;
        rocket_cluster_min_size_ = params_.rocket_cluster_min_size;
        rope_filter_roi_.z_min = static_cast<float>(params_.rope_filter_roi.z_min);
        rope_filter_roi_.z_max = static_cast<float>(params_.rope_filter_roi.z_max);
        rope_filter_roi_.x_min = static_cast<float>(params_.rope_filter_roi.x_min);
        rope_filter_roi_.x_max = static_cast<float>(params_.rope_filter_roi.x_max);
        rope_filter_roi_.y_min = static_cast<float>(params_.rope_filter_roi.y_min);
        rope_filter_roi_.y_max = static_cast<float>(params_.rope_filter_roi.y_max);

        // ROI 参数实时更新（两块 ROI 组合成 12 元数组）
        if (params_.roi1.size() == 6 && params_.roi1_b.size() == 6)
        {
            std::copy(params_.roi1.begin(), params_.roi1.end(), roi_1.begin());
            std::copy(params_.roi1_b.begin(), params_.roi1_b.end(), roi_1.begin() + 6);
        }
        if (params_.roi2.size() == 6 && params_.roi2_b.size() == 6)
        {
            std::copy(params_.roi2.begin(), params_.roi2.end(), roi_2.begin());
            std::copy(params_.roi2_b.begin(), params_.roi2_b.end(), roi_2.begin() + 6);
        }

        c1_sync_wait_param_ = params_.c1_sync_wait_sec;
        c2_sync_wait_param_ = params_.c2_sync_wait_sec;
        c1_SYNC_WAIT_ = ros::Duration(c1_sync_wait_param_);
        c2_SYNC_WAIT_ = ros::Duration(c2_sync_wait_param_);
        decision_wait_sec_ = params_.decision_wait_sec;

        double hb_timeout = params_.lidar_heartbeat_timeout;
        int hb_mode = params_.lidar_heartbeat_mode; // 0: bit=异常  1: bit=正常
        LidarHeart::setTimeout(hb_timeout);
        LidarHeart::setMode(hb_mode);

        // 重新计算盲区平面（ship）
        auto vec4_to_f = [](const std::vector<double> &v) -> Eigen::Vector4f
        {
            if (v.size() != 4)
                return Eigen::Vector4f::Zero();
            return Eigen::Vector4f(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2]), static_cast<float>(v[3]));
        };
        auto transform_plane = [](const Eigen::Matrix4f &H, const Eigen::Vector4f &pi) -> Eigen::Vector4f
        {
            Eigen::Matrix4f HinvT = H.inverse().transpose();
            return HinvT * pi;
        };
        {
            Eigen::Matrix4f H = (C1Sf_to_Shipf_ * C1T1_to_Sf_).matrix();
            c1_blind_planes_ship_[0] = transform_plane(H, vec4_to_f(params_.C1_blind_34_lidar4_plane));
            c1_blind_planes_ship_[1] = transform_plane(H, vec4_to_f(params_.C1_blind_34_lidar3_plane));
            c1_blind_planes_ship_[2] = transform_plane(H, vec4_to_f(params_.C1_blind_23_lidar3_plane));
            c1_blind_planes_ship_[3] = transform_plane(H, vec4_to_f(params_.C1_blind_23_lidar2_plane));
            c1_blind_planes_ship_[4] = transform_plane(H, vec4_to_f(params_.C1_blind_1_lidar1_plane));
        }
        {
            Eigen::Matrix4f H = (C2Sf_to_Shipf_ * C2T1_to_Sf_).matrix();
            c2_blind_planes_ship_[0] = transform_plane(H, vec4_to_f(params_.C2_blind_34_lidar4_plane));
            c2_blind_planes_ship_[1] = transform_plane(H, vec4_to_f(params_.C2_blind_34_lidar3_plane));
            c2_blind_planes_ship_[2] = transform_plane(H, vec4_to_f(params_.C2_blind_23_lidar3_plane));
            c2_blind_planes_ship_[3] = transform_plane(H, vec4_to_f(params_.C2_blind_23_lidar2_plane));
            c2_blind_planes_ship_[4] = transform_plane(H, vec4_to_f(params_.C2_blind_1_lidar1_plane));
        }

        ROS_INFO("LidarFusionParams reloaded successfully.");
    }
    catch (const std::exception &e)
    {
        ROS_ERROR("Failed to reload LidarFusionParams: %s", e.what());
    }
}

LidarFusionNode::~LidarFusionNode()
{
    can_main.stop();
    can_hpyo.stop();
    logger.stop();

    shutting_down_.store(true, std::memory_order_relaxed);
    fitting_enabled_.store(false, std::memory_order_relaxed);

    // 关闭决策线程
    decision_running_.store(false, std::memory_order_relaxed);
    {
        std::lock_guard<std::mutex> lk(decision_any_mutex_);
    }
    decision_any_cv_.notify_all();

    c1_thread_pool_.stopAccepting();
    c2_thread_pool_.stopAccepting();
    c1_fit_pool_.stopAccepting();
    c2_fit_pool_.stopAccepting();
    c1_fit_pool_.discardPending();
    c2_fit_pool_.discardPending();

    if (c1_syncTimer_.isValid())
        c1_syncTimer_.stop();
    if (c2_syncTimer_.isValid())
        c2_syncTimer_.stop();
    c1_thread_running_.store(false, std::memory_order_relaxed);
    c2_thread_running_.store(false, std::memory_order_relaxed);
    c1_sync_thread_running_.store(false, std::memory_order_relaxed);
    c2_sync_thread_running_.store(false, std::memory_order_relaxed);
    c1_softsync_running_.store(false, std::memory_order_relaxed);
    c2_softsync_running_.store(false, std::memory_order_relaxed);

    {
        std::lock_guard<std::mutex> lk(c1_cv_mutex_);
        c1_cv_.notify_all();
    }
    {
        std::lock_guard<std::mutex> lk(c2_cv_mutex_);
        c2_cv_.notify_all();
    }
    if (decision_thread_.joinable())
        decision_thread_.join();
    if (c1_merge_thread_.joinable())
        c1_merge_thread_.join();
    if (c2_merge_thread_.joinable())
        c2_merge_thread_.join();
    if (c1_sync_thread_.joinable())
        c1_sync_thread_.join();
    if (c2_sync_thread_.joinable())
        c2_sync_thread_.join();
    if (c1_softsync_thread_.joinable())
        c1_softsync_thread_.join();
    if (c2_softsync_thread_.joinable())
        c2_softsync_thread_.join();

    c1_thread_pool_.discardPending();
    c2_thread_pool_.discardPending();

    c1_thread_pool_.shutdownNow();
    c2_thread_pool_.shutdownNow();
    c1_fit_pool_.shutdownNow();
    c2_fit_pool_.shutdownNow();

    {
        std::lock_guard<std::mutex> lg(c1_pending_mutex_);
        while (!c1_pending_clouds_.empty())
            c1_pending_clouds_.pop();
    }
    {
        std::lock_guard<std::mutex> lg(c2_pending_mutex_);
        while (!c2_pending_clouds_.empty())
            c2_pending_clouds_.pop();
    }
    {
        std::lock_guard<std::mutex> lg(c1_pose_mutex_);
        while (!c1_pose_queue_.empty())
            c1_pose_queue_.pop();
    }
    {
        std::lock_guard<std::mutex> lg(c2_pose_mutex_);
        while (!c2_pose_queue_.empty())
            c2_pose_queue_.pop();
    }
}

Eigen::Affine3f LidarFusionNode::vecToAffine3f(const std::vector<double> &vec)
{

    Eigen::Matrix4d mat_d;
    for (int i = 0; i < 4; ++i)
    {
        for (int j = 0; j < 4; ++j)
        {
            mat_d(i, j) = vec[i * 4 + j];
        }
    }
    Eigen::Matrix4f mat_f = mat_d.cast<float>();
    return Eigen::Affine3f(mat_f);
}

// 从frame_id中提取需要计算延迟的起始时间，frame_id格式为："<orig_frame>|<time>"
double LidarFusionNode::extractFirstPointTime(const sensor_msgs::PointCloud2ConstPtr &msg)
{

    const std::string &frame_id = msg->header.frame_id;

    // 情况1：frame_id 仅为 "map"（或以斜杠开头的 "/map"），时间戳直接取 header.stamp
    if (frame_id == "map" || frame_id == "/map")
    {
        return ros::Time::now().toSec();
    }

    // 情况2：frame_id 形如 "<orig_frame>|<first_point_time>"
    size_t pos = frame_id.find('|');
    if (pos != std::string::npos && pos + 1 < frame_id.length())
    {
        std::string time_str = frame_id.substr(pos + 1);
        try
        {
            double result = std::stod(time_str);
            return result;
        }
        catch (const std::exception &e)
        {
            ROS_WARN_THROTTLE(5.0, "Failed to parse first_point_time from frame_id: %s", e.what());
        }
    }

    // 无法解析则回退使用 header.stamp
    double stamp_sec = msg->header.stamp.toSec();
    return stamp_sec > 0 ? stamp_sec : 0;
}

// 从frame_id中提取原始frame_id，去除时间信息
std::string LidarFusionNode::getOriginalFrameId(const std::string &encoded_frame_id)
{
    size_t pos = encoded_frame_id.find('|');
    if (pos != std::string::npos)
    {
        return encoded_frame_id.substr(0, pos);
    }
    return encoded_frame_id;
}

// c1_cb1 - c1_cb4: 单机1四个雷达的回调函数
void LidarFusionNode::c1_cb1(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c1_in_mutexes_[0]);
        c1_in_queues_[0].push(msg);
        // printf("%s %f \n", "c1_cb1 = ", msg->header.stamp.toSec());
        // printf("%s %f \n", "rosTime1() = ", ros::Time::now().toSec());
    }
    c1_cv_.notify_one();
}

void LidarFusionNode::c1_cb2(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c1_in_mutexes_[1]);
        c1_in_queues_[1].push(msg);
        // printf("%s %f \n", "stamp2 = ", msg->header.stamp.toSec());
        // printf("%s %f \n", "rosTime2() = ", ros::Time::now().toSec());
    }
    c1_cv_.notify_one();
}
void LidarFusionNode::c1_cb3(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c1_in_mutexes_[2]);
        c1_in_queues_[2].push(msg);
        // printf("%s %f \n", "stamp3 = ", msg->header.stamp.toSec());
        // printf("%s %f \n", "rosTime3() = ", ros::Time::now().toSec());
    }
    c1_cv_.notify_one();
}
void LidarFusionNode::c1_cb4(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c1_in_mutexes_[3]);
        c1_in_queues_[3].push(msg);
        // printf("%s %f \n", "stamp4 = ", msg->header.stamp.toSec());
        // printf("%s %f \n", "rosTime4() = ", ros::Time::now().toSec());
    }
    c1_cv_.notify_one();
}

void LidarFusionNode::c2_cb1(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c2_in_mutexes_[0]);
        c2_in_queues_[0].push(msg);
    }
    c2_cv_.notify_one();
}
void LidarFusionNode::c2_cb2(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c2_in_mutexes_[1]);
        c2_in_queues_[1].push(msg);
    }
    c2_cv_.notify_one();
}
void LidarFusionNode::c2_cb3(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c2_in_mutexes_[2]);
        c2_in_queues_[2].push(msg);
    }
    c2_cv_.notify_one();
}
void LidarFusionNode::c2_cb4(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    if (shutting_down_.load(std::memory_order_relaxed))
        return;
    {
        std::lock_guard<std::mutex> lg(c2_in_mutexes_[3]);
        c2_in_queues_[3].push(msg);
    }
    c2_cv_.notify_one();
}

// 软同步处理函数:处理逻辑为：从各个雷达的输入队列中取出点云数据，进行时间软同步，并将同步后的数据批次推入同步队列中，供后续处理使用。
void LidarFusionNode::c1_handleSoftSync()
{
    time_sync::Inputs in{
        &c1_in_queues_,
        &c1_in_mutexes_,
        &c1_cv_mutex_,
        &c1_cv_,
        &c1_softsync_running_,
        c1_sync_wait_param_};
    // 使用 each_mask 的低 4 位（0=正常，1=异常）选择参与同步的雷达通道
    in.get_active = []()
    {
        std::array<bool, 4> act{true, true, true, true};
        try
        {
            uint8_t each_mask = 0;
            int overall = 0;
            LidarHeart::getStatus(each_mask, overall);
            for (int i = 0; i < 4; ++i)
            {
                bool abnormal = ((each_mask >> i) & 0x1) != 0; // 1=异常
                act[i] = !abnormal;                            // 仅正常通道参与
            }
        }
        catch (...)
        {
            // 失败则默认全部参与
        }
        // act = {false,true,true,true};
        return act;
    };
    auto push_fn = [this](const std::vector<int> &ids,
                          const std::vector<sensor_msgs::PointCloud2ConstPtr> &msgs)
    {
        std::lock_guard<std::mutex> qlg(c1_sync_queue_mutex_);
        c1_sync_queue_.push(SyncBatch{ids, msgs});
    };
    // 实时更新 C1 时间同步状态（0=正常，1=异常）
    in.set_time_sync_state = [this](bool ok)
    {
        c1_time_sync_state_.store(ok ? 0 : 1, std::memory_order_relaxed);
    };
    time_sync::two_phase_soft_sync(in, push_fn);
}

void LidarFusionNode::c2_handleSoftSync()
{
    time_sync::Inputs in{
        &c2_in_queues_,
        &c2_in_mutexes_,
        &c2_cv_mutex_,
        &c2_cv_,
        &c2_softsync_running_,
        c2_sync_wait_param_};
    // 使用 each_mask 的高 4 位（bit[4..7]，0=正常，1=异常）选择参与同步的雷达通道
    in.get_active = []()
    {
        std::array<bool, 4> act{true, true, true, true};
        try
        {
            uint8_t each_mask = 0;
            int overall = 0;
            LidarHeart::getStatus(each_mask, overall);
            for (int j = 0; j < 4; ++j)
            {
                bool abnormal = ((each_mask >> (4 + j)) & 0x1) != 0; // 1=异常
                act[j] = !abnormal;                                  // 仅正常通道
            }
        }
        catch (...)
        {
            // 失败则默认全部
        }
        return act;
    };
    auto push_fn = [this](const std::vector<int> &ids,
                          const std::vector<sensor_msgs::PointCloud2ConstPtr> &msgs)
    {
        std::lock_guard<std::mutex> qlg(c2_sync_queue_mutex_);
        c2_sync_queue_.push(SyncBatch{ids, msgs});
    };
    // 实时更新 C2 时间同步状态（0=正常，1=异常）
    in.set_time_sync_state = [this](bool ok)
    {
        c2_time_sync_state_.store(ok ? 0 : 1, std::memory_order_relaxed);
    };
    time_sync::two_phase_soft_sync(in, push_fn);
}

// 同步批次入队函数：将已到达的点云数据打包成同步批次，推入同步队列中，供后续处理使用。
void LidarFusionNode::c1_enqueueSyncBatch()
{
    std::vector<int> ids;
    std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
    {
        std::lock_guard<std::mutex> lg(c1_mtx_);
        for (int i = 0; i < 4; ++i)
        {
            if (c1_box_[i].arrived)
            {
                ids.push_back(i);
                clouds.push_back(c1_box_[i].cloud);
                c1_box_[i].arrived = false;
                c1_box_[i].cloud.reset();
            }
        }
    }
    if (ids.empty())
        return;
    {
        std::lock_guard<std::mutex> qlg(c1_sync_queue_mutex_);
        c1_sync_queue_.push(SyncBatch{ids, clouds});
    }
}

// 同步工作处理函数：从同步队列中取出同步批次，使用线程池并行处理每个雷达的点云坐标转换和ROI，处理完成后将结果打包成待处理云数据，推入待处理队列中。
void LidarFusionNode::c1_doSyncWork()
{
    while (c1_sync_thread_running_.load(std::memory_order_relaxed) && ros::ok())
    {
        SyncBatch batch;
        bool has = false;
        // 从同步队列中取出一个批次
        {
            std::lock_guard<std::mutex> lg(c1_sync_queue_mutex_);
            if (!c1_sync_queue_.empty())
            {
                batch = c1_sync_queue_.front();
                c1_sync_queue_.pop();
                has = true;
            }
        }
        if (!has)
        {
            continue;
        }

        std::vector<CloudType> processed_clouds(4);
        std::vector<std::future<void>> futures;
        // 并行处理每个雷达的点云数据
        for (size_t idx = 0; idx < batch.lidar_ids.size(); ++idx)
        {
            int lidar_id = batch.lidar_ids[idx];
            auto cloud_ptr = batch.clouds[idx];
            futures.emplace_back(c1_thread_pool_.AddTask([this, lidar_id, cloud_ptr, &processed_clouds]()
                                                         {
                try {
                    switch (lidar_id) {
                        case 0:
                            processed_clouds[lidar_id] = this->c1_funcLidar1(cloud_ptr);//ROI和坐标转换
                            break;
                        case 1:
                            processed_clouds[lidar_id] = this->c1_funcLidar2(cloud_ptr);
                            break;
                        case 2:
                            processed_clouds[lidar_id] = this->c1_funcLidar3(cloud_ptr);
                            break;
                        case 3:
                            processed_clouds[lidar_id] = this->c1_funcLidar4(cloud_ptr);
                            break;
                    }
                } catch(const std::exception &e) {
                    ROS_ERROR("C1 Lidar %d task failed: %s", lidar_id, e.what());
                } }));
        }
        for (auto &f : futures)
        {
            if (!f.valid())
            {
                continue;
            }
            try
            {
                f.wait();
            }
            catch (const std::exception &e)
            {
                ROS_ERROR("Wait C1 future failed: %s", e.what());
            }
        }
        PendingCloud data;
        data.ready_lidar_ids = batch.lidar_ids;
        data.header = batch.clouds[0]->header;
        double earliest_time = std::numeric_limits<double>::max();
        double lidar_time = std::numeric_limits<double>::max();
        // 找到所有点云中时间最早的一个作为当前同步批次的时间戳
        for (auto &cloud_ptr : batch.clouds)
        {
            double t = cloud_ptr->header.stamp.toSec();
            if (t > 0 && t < lidar_time)
            {
                lidar_time = t;
            }
        }
        data.header.stamp = ros::Time(lidar_time);

        for (auto &cloud_ptr : batch.clouds)
        {
            double t = extractFirstPointTime(cloud_ptr);
            if (t > 0 && t < earliest_time)
            {
                earliest_time = t;
            }
        }
        // i = 0;
        data.lidar_start_time = (earliest_time != std::numeric_limits<double>::max())
                                    ? earliest_time
                                    : 0;

        // // 将该批次的时间戳写入文本文件（追加模式）
        // try {
        //     std::ofstream ts_out("/home/ubuntu/c1_batches_timestamps.txt", std::ios::app);
        //     if (ts_out.is_open()) {
        //         // 记录批次的统一时间戳（header.stamp）与首点时间（lidar_start_time）以及批次大小
        //         ts_out << std::fixed << std::setprecision(9)
        //               << "batch_begin header_stamp_sec=" << data.header.stamp.toSec() << ", "
        //               << "lidar_start_time=" << data.lidar_start_time << ", "
        //               << "batch_size=" << batch.clouds.size() << std::endl;

        //         // 逐帧记录：每个通道在该批次中的原始 header.stamp 与首点时间
        //         for (size_t i = 0; i < batch.clouds.size(); ++i) {
        //             const auto &msg = batch.clouds[i];
        //             double stamp_sec = msg->header.stamp.toSec();
        //             double first_point_time = extractFirstPointTime(msg);
        //             int lidar_id = (i < batch.lidar_ids.size()) ? batch.lidar_ids[i] : -1;
        //             ts_out << "  frame lidar_id=" << lidar_id
        //                    << ", header_stamp_sec=" << stamp_sec
        //                    << ", first_point_time=" << first_point_time
        //                    << ", frame_id=\"" << msg->header.frame_id << "\""
        //                    << std::endl;
        //         }

        //         ts_out << "batch_end" << std::endl;
        //     } else {
        //         ROS_WARN_THROTTLE(5.0, "Failed to open c1_batches_timestamps.txt for writing");
        //     }
        // } catch (const std::exception &e) {
        //     ROS_WARN_THROTTLE(5.0, "Write C1 timestamps failed: %s", e.what());
        // }

        // 将处理后的点云数据存入待处理云数据结构中
        for (int id : batch.lidar_ids)
        {
            data.ready_clouds[id] = processed_clouds[id];
        }
        {
            std::lock_guard<std::mutex> pending_lg(c1_pending_mutex_);
            c1_pending_clouds_.push(data);
        }
    }
}

void LidarFusionNode::c2_enqueueSyncBatch()
{
    std::vector<int> ids;
    std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
    {
        std::lock_guard<std::mutex> lg(c2_mtx_);
        for (int i = 0; i < 4; ++i)
        {
            if (c2_box_[i].arrived)
            {
                ids.push_back(i);
                clouds.push_back(c2_box_[i].cloud);
                c2_box_[i].arrived = false;
                c2_box_[i].cloud.reset();
            }
        }
    }
    if (ids.empty())
        return;
    {
        std::lock_guard<std::mutex> qlg(c2_sync_queue_mutex_);
        c2_sync_queue_.push(SyncBatch{ids, clouds});
    }
}

void LidarFusionNode::c2_doSyncWork()
{
    while (c2_sync_thread_running_.load(std::memory_order_relaxed) && ros::ok())
    {
        SyncBatch batch;
        bool has = false;
        {
            std::lock_guard<std::mutex> lg(c2_sync_queue_mutex_);
            if (!c2_sync_queue_.empty())
            {
                batch = c2_sync_queue_.front();
                c2_sync_queue_.pop();
                has = true;
            }
        }
        if (!has)
        {
            continue;
        }
        std::vector<CloudType> processed_clouds(4);
        std::vector<std::future<void>> futures;
        for (size_t idx = 0; idx < batch.lidar_ids.size(); ++idx)
        {
            int lidar_id = batch.lidar_ids[idx];
            auto cloud_ptr = batch.clouds[idx];
            futures.emplace_back(
                c2_thread_pool_.AddTask([this, lidar_id, cloud_ptr, &processed_clouds]()
                                        {
                    try {
                        switch (lidar_id) {
                            case 0:
                                processed_clouds[lidar_id] = this->c2_funcLidar1(cloud_ptr);
                                break;
                            case 1:
                                processed_clouds[lidar_id] = this->c2_funcLidar2(cloud_ptr);
                                break;
                            case 2:
                                processed_clouds[lidar_id] = this->c2_funcLidar3(cloud_ptr);
                                break;
                            case 3:
                                processed_clouds[lidar_id] = this->c2_funcLidar4(cloud_ptr);
                                break;
                        }
                    } catch (const std::exception &e) {
                        ROS_ERROR("C2 Lidar %d task failed: %s", lidar_id, e.what());
                    } }));
        }
        for (auto &f : futures)
        {
            if (!f.valid())
            {
                continue;
            }
            try
            {
                f.wait();
            }
            catch (const std::exception &e)
            {
                ROS_ERROR("Wait C2 future failed: %s", e.what());
            }
        }
        PendingCloud data;
        data.ready_lidar_ids = batch.lidar_ids;
        data.header = batch.clouds[0]->header;
        double earliest_time = std::numeric_limits<double>::max();
        double lidar_time = std::numeric_limits<double>::max();
        for (auto &cloud_ptr : batch.clouds)
        {
            double t = cloud_ptr->header.stamp.toSec();
            if (t > 0 && t < lidar_time)
            {
                lidar_time = t;
            }
        }
        data.header.stamp = ros::Time(lidar_time);
        for (auto &cloud_ptr : batch.clouds)
        {
            double t = extractFirstPointTime(cloud_ptr);
            if (t > 0 && t < earliest_time)
            {
                earliest_time = t;
            }
        }
        data.lidar_start_time = (earliest_time != std::numeric_limits<double>::max()) ? earliest_time : 0;

        // // 将该批次的时间戳写入文本文件（追加模式）
        // try {
        //     std::ofstream ts_out("/home/ubuntu/c2_batches_timestamps.txt", std::ios::app);
        //     if (ts_out.is_open()) {
        //         ts_out << std::fixed << std::setprecision(9)
        //               << "batch_begin header_stamp_sec=" << data.header.stamp.toSec() << ", "
        //               << "lidar_start_time=" << data.lidar_start_time << ", "
        //               << "batch_size=" << batch.clouds.size() << std::endl;
        //         for (size_t i = 0; i < batch.clouds.size(); ++i) {
        //             const auto &msg = batch.clouds[i];
        //             double stamp_sec = msg->header.stamp.toSec();
        //             double first_point_time = extractFirstPointTime(msg);
        //             int lidar_id = (i < batch.lidar_ids.size()) ? batch.lidar_ids[i] : -1;
        //             ts_out << "  frame lidar_id=" << lidar_id
        //                    << ", header_stamp_sec=" << stamp_sec
        //                    << ", first_point_time=" << first_point_time
        //                    << ", frame_id=\"" << msg->header.frame_id << "\""
        //                    << std::endl;
        //         }
        //         ts_out << "batch_end" << std::endl;
        //     } else {
        //         ROS_WARN_THROTTLE(5.0, "Failed to open c2_batches_timestamps.txt for writing");
        //     }
        // } catch (const std::exception &e) {
        //     ROS_WARN_THROTTLE(5.0, "Write C2 timestamps failed: %s", e.what());
        // }

        // 将处理后的点云数据存入待处理云数据结构中
        for (int id : batch.lidar_ids)
        {
            data.ready_clouds[id] = processed_clouds[id];
        }
        {
            std::lock_guard<std::mutex> pending_lg(c2_pending_mutex_);
            c2_pending_clouds_.push(data);
        }
    }
}

// 决策函数：根据两个PoseData的有效性和拟合残差选择最佳结果
PoseData LidarFusionNode::decideTwo(const PoseData &m1, const PoseData &m2)
{
    const bool m1_ok = (m1.valid == 0);
    const bool m2_ok = (m2.valid == 0);

    if (1)
    {
        if (m1_ok && m2_ok)
        {
            // 若两结果均为测量值，则取延迟小的结果作为输出结果
            if (m1.IsMeasure == 1 && m2.IsMeasure == 1)
            {
                return m1;
            }
            else if (m1.IsMeasure == 1 && m2.IsMeasure == 0)
            {
                return m1; // 若m1结果为测量值，则返回m1的结果
            }
            else if (m1.IsMeasure == 0 && m2.IsMeasure == 1)
            {
                return m2; // 若m2结果为测量值，则返回m2的结果
            }
            else if (m1.IsMeasure == 0 && m2.IsMeasure == 0)
            {
                return m1; // 若两结果均为预测值，则取延迟小的结果作为输出结果
            }
        }
        else if (m1_ok)
        {
            return m1;
        }
        else if (m2_ok)
        {
            return m2;
        }
        else
        {
            return m1; // 都无效时保持 m1
        }
    }
}

// 决策函数：仅有一个PoseData时，直接返回该结果
PoseData LidarFusionNode::decideOne(const PoseData &m1)
{
    return m1;
}

// 获取最佳结果函数：从第二个雷达队列中获取数据，并与第一个雷达的数据进行决策，返回最佳结果
PoseData LidarFusionNode::getBestResult(PoseData &data1)
{
    bool has_data2 = false;
    PoseData data2, result;
    lidar2_mutex_.lock();
    if (!lidarCombine2.empty())
    {
        while (lidarCombine2.size() > 1)
            lidarCombine2.pop();
        data2 = lidarCombine2.front();
        lidarCombine2.pop();
        has_data2 = true;
    }
    lidar2_mutex_.unlock();

    if (has_data2)
    {
        result = decideTwo(data1, data2);
    }
    else
    {
        result = decideOne(data1);
    }
    return result;
}

// 拼接处理线程函数：从待处理云队列中取出数据，进行点云拼接和姿态拟合，并发布结果
void LidarFusionNode::c1_mergeThreadLoop()
{
    while (c1_thread_running_.load(std::memory_order_relaxed) && ros::ok())
    {

        PendingCloud data;
        bool has_data = false;
        // 从待处理云队列中取出一个数据
        {
            std::lock_guard<std::mutex> pending_lg(c1_pending_mutex_);
            if (!c1_pending_clouds_.empty())
            {
                data = c1_pending_clouds_.front();
                c1_pending_clouds_.pop();
                has_data = true;
            }
        }
        // 进行点云拼接和姿态拟合
        if (has_data)
        {
            CloudType merged_cloud;

            size_t total_points = 0;
            // 计算总点数以预分配内存
            for (int lidar_id : data.ready_lidar_ids)
            {
                total_points += data.ready_clouds[lidar_id].size();
            }
            merged_cloud.reserve(total_points);
            // 拼接各雷达点云
            for (int lidar_id : data.ready_lidar_ids)
            {
                const auto &cloud = data.ready_clouds[lidar_id];
                merged_cloud.insert(merged_cloud.end(), cloud.begin(), cloud.end());
            }
            double gps_timestamp = data.header.stamp.toNSec() / 1e9;
            bool save_pcd = true;
            if (save_pcd)
            {
                // Save debug cloud when timestamp is within 1 ms of the target
                constexpr double target_ts = 10.100152;
                constexpr double ts_tolerance = 0.001; // seconds (1 ms)
                cout << std::fixed << std::setprecision(6) << "target_ts: " << target_ts << endl;
                cout << std::fixed << std::setprecision(6) << "gps_timestamp: " << gps_timestamp << endl;
                if (std::abs(gps_timestamp - target_ts) <= ts_tolerance)
                {
                    std::ostringstream oss;
                    oss << "/tmp/processedCloudC1" << std::fixed << std::setprecision(6) << gps_timestamp << ".pcd";
                    const std::string pcd_path = oss.str();
                    pcl::io::savePCDFileBinary(pcd_path, merged_cloud);
                    std::cout << "[CylinderFitter] cp or dir NaN at t=" << std::fixed << std::setprecision(6)
                              << gps_timestamp << ", saved cloud: " << pcd_path << std::endl;
                }
            }

            TicToc t1;
            // 提取Rocket点云：rocketExtract 仍使用单块 6 维 ROI，这里取 roi_1 的前 6 个元素
            CloudType rocket_cloud = rocketExtract(
                merged_cloud,
                std::array<float, 6>{roi_1[0], roi_1[1],
                                     roi_1[2], roi_1[3],
                                     roi_1[10], roi_1[5]},
                gps_timestamp);

            LOG(INFO) << std::fixed << std::setprecision(6) << "gps_timestamp: " << gps_timestamp << std::endl;
            LOG(INFO) << "C1 rocket rocketExtract: " << t1.toc() << " ms" << std::endl;

            getTempResult1_ = false;
            // 提交异步拟合任务
            if (!shutting_down_.load(std::memory_order_relaxed) && fitting_enabled_.load(std::memory_order_relaxed))
            {
                try
                {
                    auto fit_task = [this, rocket_cloud, gps_timestamp, lidar_start = data.lidar_start_time]()
                    {
                        if (shutting_down_.load(std::memory_order_relaxed) || !fitting_enabled_.load(std::memory_order_relaxed))
                        {
                            return PoseData();
                        }

                        ros::Time fit_start = ros::Time::now();
                        CloudType::Ptr cloud_ptr(new CloudType(rocket_cloud));

                        // double timestamp = lidar_start > 0.0f ? lidar_start : ros::Time::now().toSec();
                        TicToc t_fit;
                        PoseData pose = c1_fitter.fitCylinder(cloud_ptr, gps_timestamp, &c1_blind_planes_ship_);
                        // 同步状态写入结果（0=正常，1=异常）
                        LOG(INFO) << "C1 fitCylinder: " << t_fit.toc() << " ms" << std::endl;
                        pose.time_sync_state = c1_time_sync_state_.load(std::memory_order_relaxed);
                        if (shutting_down_.load(std::memory_order_relaxed))
                        {
                            return pose;
                        }
                        ros::Time end_time = ros::Time::now();

                        pose.delay_time = lidar_start;

                        double detection_time = end_time.toSec() - fit_start.toSec();
                        // 更新预测器历史帧
                        if (c1_predictor_ && pose.valid == 0 && !pose.bottomCenter.hasNaN() && !pose.bottomCenter.array().isInf().any())
                        {
                            c1_predictor_->setBlindZoneActive(c1_fitter.isBlindZoneActive());
                            Eigen::Vector3f cen = pose.bottomCenter.cast<float>();
                            Eigen::Vector3f ax = pose.axis.cast<float>();
                            c1_predictor_->updateHistoryAndFilter(pose.timestamp, cen, ax, pose.fitResidual);
                            // 记录最近一次时间基准
                            s_last_c1_stamp_sec.store(pose.timestamp, std::memory_order_relaxed);
                            s_last_c1_delay_sec.store(pose.delay_time, std::memory_order_relaxed);
                            posedetect_vis::publishPoint(cen, "posedetect_points", 101, 0.1f, 0.95f, 0.1f, 0.95f, 1.0f);
                        }

                        decision_any_cv_.notify_all();
                        // 记录 C1 拟合结果
                        {
                            static std::ofstream log_c1_fit("/home/zxy/results/c1_fit_result.log", std::ios::app);
                            static bool header_written_c1 = false;
                            if (!header_written_c1 && log_c1_fit.is_open())
                            {
                                log_c1_fit << "timestamp delay_time bottomCenter_x bottomCenter_y bottomCenter_z axis_x axis_y axis_z elevation azimuth fitResidual valid cloudNumber is_predict IsMeasure height_source" << std::endl;
                                log_c1_fit.flush();
                                header_written_c1 = true;
                            }
                            if (log_c1_fit.is_open())
                            {
                                log_c1_fit << std::fixed << std::setprecision(6)
                                           << pose.timestamp << " "
                                           << pose.delay_time << " "
                                           << pose.bottomCenter.x() << " " << pose.bottomCenter.y() << " " << pose.bottomCenter.z() << " "
                                           << pose.axis.x() << " " << pose.axis.y() << " " << pose.axis.z() << " "
                                           << pose.elevation_angle << " "
                                           << pose.azimuth_angle << " "
                                           << pose.fitResidual << " "
                                           << (int)pose.valid << " "
                                           << pose.cloudNumber << " "
                                           << (int)pose.is_predict << " "
                                           << (int)pose.IsMeasure << " "
                                           << pose.bottom_height_source << std::endl;
                                log_c1_fit.flush();
                            }
                        }
                        return pose;
                    };
                    c1_fit_tasks_++;
                    c1_fit_pool_.AddTask(std::move(fit_task));
                }
                catch (const std::exception &e)
                {
                    ROS_ERROR("c1 async fit task submission failed: %s", e.what());
                }
            }
            // 发布拼接后的点云
            sensor_msgs::PointCloud2 output_msg;
            pcl::toROSMsg(rocket_cloud, output_msg);
            output_msg.header = data.header;
            output_msg.header.frame_id = "map";

            if (ros::ok())
            {
                c1_pub_.publish(output_msg);
            }
            c1_frames_++;
            if ((c1_frames_ % 100 == 0) && ros::Time::now() - last_stat_time_ > ros::Duration(1.0))
            {
                // ROS_INFO_STREAM("[STAT] C1 frames=" << c1_frames_.load() << " fit_tasks=" << c1_fit_tasks_.load());
                last_stat_time_ = ros::Time::now();
            }
        }
    }
    fitting_enabled_.store(false, std::memory_order_relaxed);
    ROS_INFO("拼接线程已正常退出");
}

void LidarFusionNode::c2_mergeThreadLoop()
{
    while (c2_thread_running_.load(std::memory_order_relaxed) && ros::ok())
    {
        PendingCloud data;
        bool has_data = false;

        {
            std::lock_guard<std::mutex> pending_lg(c2_pending_mutex_);
            if (!c2_pending_clouds_.empty())
            {
                data = c2_pending_clouds_.front();
                c2_pending_clouds_.pop();
                has_data = true;
            }
        }
        if (has_data)
        {
            CloudType merged_cloud;
            size_t total_points = 0;
            for (int lidar_id : data.ready_lidar_ids)
            {
                total_points += data.ready_clouds[lidar_id].size();
            }
            merged_cloud.reserve(total_points);
            for (int lidar_id : data.ready_lidar_ids)
            {
                const auto &cloud = data.ready_clouds[lidar_id];
                merged_cloud.insert(merged_cloud.end(), cloud.begin(), cloud.end());
            }
            double gps_timestamp = data.header.stamp.toNSec() / 1e9;
            bool save_pcd = false;
            if (save_pcd)
            {
                // Save debug cloud when timestamp is within 1 ms of the target
                constexpr double target_ts = 5.900152;
                constexpr double ts_tolerance = 0.001; // seconds (1 ms)
                cout << std::fixed << std::setprecision(6) << "target_ts: " << target_ts << endl;
                cout << std::fixed << std::setprecision(6) << "gps_timestamp: " << gps_timestamp << endl;
                if (std::abs(gps_timestamp - target_ts) <= ts_tolerance)
                {
                    std::ostringstream oss;
                    oss << "/tmp/processedCloudC2" << std::fixed << std::setprecision(6) << gps_timestamp << ".pcd";
                    const std::string pcd_path = oss.str();
                    pcl::io::savePCDFileBinary(pcd_path, merged_cloud);
                    std::cout << "[CylinderFitter] cp or dir NaN at t=" << std::fixed << std::setprecision(6)
                              << gps_timestamp << ", saved cloud: " << pcd_path << std::endl;
                }
            }

            // 提取Rocket点云：rocketExtract 仍使用单块 6 维 ROI，这里取 roi_2 的前 6 个元素
            ros::Time extract_start_time = ros::Time::now();
            CloudType rocket_cloud = rocketExtract(
                merged_cloud,
                std::array<float, 6>{roi_2[0], roi_2[1],
                                     roi_2[2], roi_2[3],
                                     roi_2[10], roi_2[5]},
                gps_timestamp);
            ros::Time extract_end_time = ros::Time::now();
            const double extract_time_ms = (extract_end_time - extract_start_time).toSec() * 1000.0; // ms
            // ROS_INFO_STREAM("C2 Rocket Extract time: " << std::fixed << std::setprecision(3)
            //                 << extract_time_ms << " ms");
            if (!shutting_down_.load(std::memory_order_relaxed) && fitting_enabled_.load(std::memory_order_relaxed))
            {
                try
                {
                    auto fit_task = [this, rocket_cloud, gps_timestamp, lidar_start = data.lidar_start_time]()
                    {
                        if (shutting_down_.load(std::memory_order_relaxed) || !fitting_enabled_.load(std::memory_order_relaxed))
                            return PoseData();
                        CloudType::Ptr cloud_ptr(new CloudType(rocket_cloud));
                        // printf("%s %f \n" , "lidar_start = ", lidar_start);
                        // printf("%s %d \n" , "cloud_ptr = ", static_cast<double>(cloud_ptr->header.stamp) / 1e9);

                        // double timestamp = lidar_start > 0.0f ? lidar_start : ros::Time::now().toSec();
                        // std::cout << "gps_timestamp: " << gps_timestamp << std::endl;
                        PoseData pose = c2_fitter.fitCylinder(cloud_ptr, gps_timestamp, &c2_blind_planes_ship_);
                        // 同步状态写入结果（0=正常，1=异常）
                        pose.time_sync_state = c2_time_sync_state_.load(std::memory_order_relaxed);
                        if (shutting_down_.load(std::memory_order_relaxed))
                            return pose;
                        ros::Time end_time = ros::Time::now();
                        // if (lidar_start > 0) {
                        //     pose.delay_time = lidar_start;
                        // } else {
                        //     pose.delay_time = 0;
                        // }

                        pose.delay_time = lidar_start;

                        // 更新预测器历史帧
                        if (c2_predictor_ && pose.valid == 0 && !pose.bottomCenter.hasNaN() && !pose.bottomCenter.array().isInf().any())
                        {
                            c2_predictor_->setBlindZoneActive(c2_fitter.isBlindZoneActive());
                            Eigen::Vector3f cen = pose.bottomCenter.cast<float>();
                            Eigen::Vector3f ax = pose.axis.cast<float>();
                            c2_predictor_->updateHistoryAndFilter(pose.timestamp, cen, ax, pose.fitResidual);
                            // 记录最近一次时间基准（C2）
                            s_last_c2_stamp_sec.store(pose.timestamp, std::memory_order_relaxed);
                            s_last_c2_delay_sec.store(pose.delay_time, std::memory_order_relaxed);
                            // Eigen::Vector3f quan_cen(0.0f, 0.0f, 0.0f);
                            posedetect_vis::publishPoint(cen, "posedetect_points", 102, 0.1f, 0.95f, 0.1f, 0.95f, 1.0f);
                        }
                        decision_any_cv_.notify_all();
                        ROS_DEBUG_STREAM_THROTTLE(10, "c2 fitCylinder completed");
                        // 记录 C2 拟合结果
                        {
                            static std::ofstream log_c2_fit("/home/zxy/results/c2_fit_result.log", std::ios::app);
                            static bool header_written_c2 = false;
                            if (!header_written_c2 && log_c2_fit.is_open())
                            {
                                log_c2_fit << "timestamp delay_time bottomCenter_x bottomCenter_y bottomCenter_z axis_x axis_y axis_z elevation azimuth fitResidual valid cloudNumber is_predict IsMeasure height_source" << std::endl;
                                log_c2_fit.flush();
                                header_written_c2 = true;
                            }
                            if (log_c2_fit.is_open())
                            {
                                log_c2_fit << std::fixed << std::setprecision(6)
                                           << pose.timestamp << " "
                                           << pose.delay_time << " "
                                           << pose.bottomCenter.x() << " " << pose.bottomCenter.y() << " " << pose.bottomCenter.z() << " "
                                           << pose.axis.x() << " " << pose.axis.y() << " " << pose.axis.z() << " "
                                           << pose.elevation_angle << " "
                                           << pose.azimuth_angle << " "
                                           << pose.fitResidual << " "
                                           << (int)pose.valid << " "
                                           << pose.cloudNumber << " "
                                           << (int)pose.is_predict << " "
                                           << (int)pose.IsMeasure << " "
                                           << pose.bottom_height_source << std::endl;
                                log_c2_fit.flush();
                            }
                        }
                        return pose;
                    };
                    c2_fit_tasks_++;
                    c2_fit_pool_.AddTask(std::move(fit_task));
                }
                catch (const std::exception &e)
                {
                    ROS_ERROR("c2 async fit task submission failed: %s", e.what());
                }
            }

            sensor_msgs::PointCloud2 output_msg;
            pcl::toROSMsg(rocket_cloud, output_msg);
            output_msg.header = data.header;
            output_msg.header.frame_id = "map";

            if (ros::ok())
            {
                c2_pub_.publish(output_msg);
            }
            c2_frames_++;
            if ((c2_frames_ % 100 == 0) && ros::Time::now() - last_stat_time_ > ros::Duration(1.0))
            {
                ROS_INFO_STREAM("[STAT] C2 frames=" << c2_frames_.load() << " fit_tasks=" << c2_fit_tasks_.load());
                last_stat_time_ = ros::Time::now();
            }
        }
    }
    fitting_enabled_.store(false, std::memory_order_relaxed);
    ROS_INFO("拼接线程已正常退出");
}

// 单机1四个雷达的点云处理函数：包括坐标转换和ROI提取
CloudType LidarFusionNode::c1_funcLidar1(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C1T1_to_Shipf_);

    extractROI(local_cloud, roi_1);

    return local_cloud;
}

CloudType LidarFusionNode::c1_funcLidar2(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C1T2_to_Shipf_);

    extractROI(local_cloud, roi_1);

    return local_cloud;
}

CloudType LidarFusionNode::c1_funcLidar3(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C1T3_to_Shipf_);

    extractROI(local_cloud, roi_1);

    return local_cloud;
}
CloudType LidarFusionNode::c1_funcLidar4(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C1T4_to_Shipf_);

    extractROI(local_cloud, roi_1);

    return local_cloud;
}

CloudType LidarFusionNode::c2_funcLidar1(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C2T1_to_Shipf_);

    extractROI(local_cloud, roi_2);

    return local_cloud;
}

CloudType LidarFusionNode::c2_funcLidar2(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C2T2_to_Shipf_);

    extractROI(local_cloud, roi_2);

    return local_cloud;
}

CloudType LidarFusionNode::c2_funcLidar3(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C2T3_to_Shipf_);

    extractROI(local_cloud, roi_2);

    return local_cloud;
}
CloudType LidarFusionNode::c2_funcLidar4(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    CloudType local_cloud;
    pcl::fromROSMsg(*cloud, local_cloud);

    TransformPointCloud(local_cloud, local_cloud, C2T4_to_Shipf_);

    extractROI(local_cloud, roi_2);

    return local_cloud;
}

// Rocket点云提取函数：对合并后的点云进行Rocket点云提取处理
CloudType LidarFusionNode::rocketExtract(const CloudType &cloud, const std::array<float, 6> &roi, double gps_timestamp)
{

    bool useFilter = true;
    if (useFilter)
    {

        // TicToc t_filter;
        // 先利用网格降采样
        float rocketRadius = params_.rocket_radius;
        pcl::PointCloud<PointType> tempCloud = cloud;
        // cout<<"rocket_extract_voxel_ratio_ "<< rocket_extract_voxel_ratio_<<endl;
        voxel_filter(tempCloud, rocketRadius * static_cast<float>(rocket_extract_voxel_ratio_));
        // std::cout<<"Rocket Extract voxel_filter: "<<t_filter.toc()<<" ms"<<std::endl;
        (void)gps_timestamp; // unused in clustering path, retained for interface compatibility

        // TicToc t_rope;
        // 绳索滤除：在指定 ROI 内用 PCA 线性度去除绳索点云
        CloudType result_cloud;
        if (1)
        {

            if (!cloud.empty())
            {
                result_cloud = FilterRopeByLinearity(
                    tempCloud,
                    rope_filter_roi_,
                    /*cluster_tolerance=*/0.3f,
                    /*min_cluster_size=*/5,
                    /*linearity_thresh=*/static_cast<float>(rope_filter_linearity_thresh_),
                    /*max_cross_radius=*/0.15f);
            }
        }
        // std::cout<<"Rocket Extract FilterRopeByLinearity: "<<t_rope.toc()<<" ms"<<std::endl;

        // TicToc t_cluster;
        // 2. 聚类并保留点数最多的点云块
        const double cluster_threshold = rocket_cluster_threshold_; // 欧式聚类距离阈值（米）
        const int min_cluster_size = rocket_cluster_min_size_;      // 最小有效聚类点数
        // cout<<"cluster threshold "<< cluster_threshold<< "  min_cluster_size"<< min_cluster_size<<endl;

        result_cloud = clusterKeepLargest(result_cloud, cluster_threshold, min_cluster_size);
        if (result_cloud.empty())
        {
            return CloudType();
        }

        // std::cout<<"Rocket Extract clusterKeepLargest: "<<t_cluster.toc()<<" ms"<<std::endl;
        return result_cloud;
    }
    else
    {
        return cloud;
    }
}

// 决策线程函数：从两个雷达的姿态队列中取出数据，进行决策并输出结果
void LidarFusionNode::decisionThreadLoop()
{
    int csvCount = 0, simuCount = 0, realCount = 0, testCount = 0;

    unsigned int recursion_time = 0, recursion_overflag = 0;
    double COM_ZL[3] = {0}, pos_bottom_N[3] = {0};
    double COM_V[3] = {0}, vel[3] = {0};
    double COM_att[3] = {0}, att_hook[3] = {0};

    double SheXiang = SheXiang_para;
    double att[3] = {0}, att_net[3] = {0};
    double Lever[3] = {Lever_para[0], Lever_para[1], Lever_para[2]};
    double JBottom2JM_JT[3] = {JT_para[0], JT_para[1], JT_para[2]};

    ros::Rate rate(100.0);
    while (decision_running_.load(std::memory_order_relaxed) && ros::ok())
    {
        // 直接进行两组预测
        PoseData result;
        bool have_any = false;
        const double now_sec = ros::Time::now().toSec();
        // 在预测前，按时间窗口裁剪历史，避免长期无更新导致历史积压预测无法停止
        // 窗口选择：2s
        double hist_max_age = 2;
        if (c1_predictor_)
            c1_predictor_->pruneHistoryByTime(now_sec, hist_max_age);
        if (c2_predictor_)
            c2_predictor_->pruneHistoryByTime(now_sec, hist_max_age);
        // 以“最新帧timestamp + (now - 最新delay_time)”作为各自的预测时间
        double c1_last_ts = s_last_c1_stamp_sec.load(std::memory_order_relaxed);
        double c2_last_ts = s_last_c2_stamp_sec.load(std::memory_order_relaxed);
        double c1_last_delay = s_last_c1_delay_sec.load(std::memory_order_relaxed);
        double c2_last_delay = s_last_c2_delay_sec.load(std::memory_order_relaxed);
        double c1_delay = now_sec - c1_last_delay;
        double c2_delay = now_sec - c2_last_delay;
        double t_pred_c1 = (c1_last_ts > 0.0 && c1_last_delay > 0.0) ? (c1_last_ts + (now_sec - c1_last_delay)) : now_sec;
        double t_pred_c2 = (c2_last_ts > 0.0 && c2_last_delay > 0.0) ? (c2_last_ts + (now_sec - c2_last_delay)) : now_sec;
        // std::cout << "c1_last_ts = "<< std::fixed << std::setprecision(6) << c1_last_ts <<std::endl;
        // std::cout << "c1_last_delay = "<< std::fixed << std::setprecision(6) << c1_last_delay <<std::endl;
        // std::cout << "c1_delay = "<< std::fixed << std::setprecision(6) << c1_delay <<std::endl;
        // 由 center/axis 构建 PoseData（预测值）改为调用头文件中的内联方法

        PoseData c1_pred, c2_pred;
        bool has_c1 = false;
        bool has_c2 = false;

        if (simu == 1)
        {
            // ROS_INFO("CAN%d csvCount = %d",this->can_interface_,csvCount);
            const auto &data = csv_data_list[csvCount];
            csvCount++;
            if (csvCount >= print_count)
            {
                csvCount = 0;
            }

            // 仿真数据输出到 can_main & can_hpyo
            can_main.setValue_coor_x(data.x);
            can_main.setValue_coor_y(data.y);
            can_main.setValue_coor_z(data.z);
            can_main.setValue_projection_B(data.pitch);
            can_main.setValue_projection_A(data.yaw);
            can_main.setValue_output_delay(data.delay);
            can_main.setValue_state((char)(data.radar_state));
            can_main.setValue_timestate(1);
            can_main.setValue_visible((char)(data.is_visible));
            can_main.setValue_valid((char)(data.solve_state));
            can_main.setValue_timestamp(data.timestamp * 1e3);
            can_main.setValue_state_lidar((char)(data.each_lidarstate));
            can_main.setValue_transplane((char)(data.is_cross_plane));
            can_main.setValue_predict((char)(data.is_prediction));
            can_main.setValue_state_value(0);
            can_main.setValue_count(simuCount);
            can_main.setValue_datasource((char)(data.data_state));
            
            can_hpyo.setValue_coor_x(data.x);
            can_hpyo.setValue_coor_y(data.y);
            can_hpyo.setValue_coor_z(data.z);
            can_hpyo.setValue_projection_B(data.pitch);
            can_hpyo.setValue_projection_A(data.yaw);
            can_hpyo.setValue_output_delay(data.delay);
            can_hpyo.setValue_state((char)(data.radar_state));
            can_hpyo.setValue_timestate(1);
            can_hpyo.setValue_visible((char)(data.is_visible));
            can_hpyo.setValue_valid((char)(data.solve_state));
            can_hpyo.setValue_timestamp(data.timestamp * 1e3);
            can_hpyo.setValue_state_lidar((char)(data.each_lidarstate));
            can_hpyo.setValue_transplane((char)(data.is_cross_plane));
            can_hpyo.setValue_predict((char)(data.is_prediction));
            can_hpyo.setValue_state_value(0);
            can_hpyo.setValue_count(simuCount);
            can_hpyo.setValue_datasource((char)(data.data_state));

            simuCount++;
            if (simuCount > 255)
            {
                simuCount = 0;
            }
        }
        else if (simu == 2)
        {
            // double COM_ZL[3] = {-100.0, -200.0, 300.0};    // 箭下传数据,单位：米
            // double SheXiang = 130.0;               // 射向 单位：度
            // double att[3] = {5.0, -3.0, 100.0};     // 惯组输出的网系姿态，单位：度
            // double Lever[3] = {1, -60, 1};     // 摇心在网系坐标系下的杆臂，单位：米
            PoseData result_out;
            result_out.ProjectedCenter.x() = 0;
            result_out.ProjectedCenter.y() = 0;
            result_out.axis.x() = 0.0;
            result_out.axis.y() = 0.0;
            result_out.axis.z() = 0.0;

            result_out.cloudNumber = 0;
            result_out.fitResidual = 0.0;
            result_out.is_predict = 0.0;

            // double COM_ZL[3] = {0};
            // double COM_V[3] = {0};
            // double COM_att[3] = {0};
            // double SheXiang = SheXiang_para;
            // double att[3] = {0};
            // double Lever[3] = {Lever_para[0], Lever_para[1], Lever_para[2]};
            // double JBottom2JM_JT[3] = {JT_para[0], JT_para[1],JT_para[2]};
            recursion_time++;
            if (udp_client.get_update_flag())
            {
                udp_client.set_update_flag(0);
                recursion_time = 0;
                recursion_overflag = 0;

                COM_V[0] = udp_client.get_vx_value();
                COM_V[1] = udp_client.get_vy_value();
                COM_V[2] = udp_client.get_vz_value();

                COM_ZL[0] = udp_client.get_x_value() + COM_V[0] * delay_JT_ms / 1000.0;
                COM_ZL[1] = udp_client.get_y_value() + COM_V[1] * delay_JT_ms / 1000.0;
                COM_ZL[2] = udp_client.get_z_value() + COM_V[2] * delay_JT_ms / 1000.0;

                COM_att[0] = udp_client.get_pitch_value();
                COM_att[1] = udp_client.get_yaw_value();
                COM_att[2] = udp_client.get_roll_value();

                can_main.setValue_predict(0x01);
                can_hpyo.setValue_predict(0x01);
                result_out.IsMeasure = 0x01;
            }
            else if (udp_client_bak.get_update_flag())
            {
                udp_client_bak.set_update_flag(0);
                recursion_time = 0;
                recursion_overflag = 0;

                COM_V[0] = udp_client_bak.get_vx_value();
                COM_V[1] = udp_client_bak.get_vy_value();
                COM_V[2] = udp_client_bak.get_vz_value();

                COM_ZL[0] = udp_client_bak.get_x_value() + COM_V[0] * delay_JT_ms / 1000.0;
                COM_ZL[1] = udp_client_bak.get_y_value() + COM_V[1] * delay_JT_ms / 1000.0;
                COM_ZL[2] = udp_client_bak.get_z_value() + COM_V[2] * delay_JT_ms / 1000.0;

                COM_att[0] = udp_client_bak.get_pitch_value();
                COM_att[1] = udp_client_bak.get_yaw_value();
                COM_att[2] = udp_client_bak.get_roll_value();

                can_main.setValue_predict(0x01);
                can_hpyo.setValue_predict(0x01);
                result_out.IsMeasure = 0x01;
            }
            else
            {
                if (recursion_time < 200)
                {
                    recursion_overflag = 0;
                    // 递推
                    COM_ZL[0] += COM_V[0] * 0.01;
                    COM_ZL[1] += COM_V[1] * 0.01;
                    COM_ZL[2] += COM_V[2] * 0.01;

                    can_main.setValue_predict(0x00);
                    can_hpyo.setValue_predict(0x00);
                    result_out.IsMeasure = 0x00;
                }
                else
                {
                    recursion_overflag = 1;
                }

            }

            double ZL_distance = sqrt(COM_ZL[0] * COM_ZL[0] + COM_ZL[1] * COM_ZL[1] + COM_ZL[2] * COM_ZL[2]);
            double COM_att_norm = sqrt(COM_att[0] * COM_att[0] + COM_att[1] * COM_att[1] + COM_att[2] * COM_att[2]);

            if (can_main.get_rec_flag())
            {
                att[0] = can_main.get_pitch_value();
                att[1] = can_main.get_roll_value();
                att[2] = can_main.get_yaw_value();                 
            }
            else if (can_hpyo.get_rec_flag())
            {
                att[0] = can_hpyo.get_pitch_value();
                att[1] = can_hpyo.get_roll_value();
                att[2] = can_hpyo.get_yaw_value();
            }
            else
            {
                att[0] = GZ_para[0];
                att[1] = GZ_para[1];
                att[2] = GZ_para[2];
            }

            double COM_ZL_processed[3] = {COM_ZL[0], COM_ZL[1], COM_ZL[2]};
            double JBottom_ZL[3] = {0};
            calculate_ZL_WX(COM_ZL_processed, COM_att, JBottom2JM_JT, JBottom_ZL);

            double CM_WX[3] = {0};
            calculate_CM_WX(COM_ZL_processed, SheXiang, att, Lever, CM_WX);

            double JBottom_WX[3] = {0};
            calculate_CM_WX(JBottom_ZL, SheXiang, att, Lever, JBottom_WX);

            double CM_WX_15s[3] = {CM_WX[0], -CM_WX[2], CM_WX[1]};
            double JBottom_WX_15s[3] = {JBottom_WX[0], -JBottom_WX[2], JBottom_WX[1]};

            double rocks[3];
            double rock[3];

            rocks[0] = CM_WX_15s[0] - JBottom_WX_15s[0];
            rocks[1] = CM_WX_15s[1] - JBottom_WX_15s[1];
            rocks[2] = CM_WX_15s[2] - JBottom_WX_15s[2];

            double rock_norm = sqrt(rocks[0] * rocks[0] + rocks[1] * rocks[1] + rocks[2] * rocks[2]);
            if (fabs(rock_norm) < 1e-9)
            {
                rock[0] = rock[1] = rock[2] = 0.0;
            }
            else
            {
                rock[0] = rocks[0] / rock_norm;
                rock[1] = rocks[1] / rock_norm;
                rock[2] = rocks[2] / rock_norm;
            }

            double azimuth_deg, elevation_deg, theta;
            azimuth_deg = atan2(rock[1], rock[0]) * 180.0f / PI;
            if (azimuth_deg < 0)
            {
                azimuth_deg += 360.0;
            }

            theta = acos(rock[2]) * 180.0f / PI;
            elevation_deg = 90.0 - theta;
            if (elevation_deg < 0)
            {
                elevation_deg = 0.0;
            }

            char flag_chuanwang = 0;
            double J0_WX_15s[3] = {0.0, 0.0, 0.0};

            if (JBottom_WX[1] < 0)
            {
                flag_chuanwang = 1;

                double J0_WX[3];
                double diff[3] = {
                    JBottom_WX[0] - CM_WX[0],
                    JBottom_WX[1] - CM_WX[1],
                    JBottom_WX[2] - CM_WX[2]};

                double denominator = fabs(JBottom_WX[1]) + CM_WX[1];
                if (fabs(denominator) < 1e-9)
                {
                    J0_WX[0] = CM_WX[0];
                    J0_WX[1] = CM_WX[1];
                    J0_WX[2] = CM_WX[2];
                }
                else
                {
                    double scalar = CM_WX[1] / denominator;
                    J0_WX[0] = diff[0] * scalar + CM_WX[0];
                    J0_WX[1] = diff[1] * scalar + CM_WX[1];
                    J0_WX[2] = diff[2] * scalar + CM_WX[2];
                }

                J0_WX_15s[0] = J0_WX[0];
                J0_WX_15s[1] = -J0_WX[2];
                J0_WX_15s[2] = JBottom_WX[1];

                if ((ZL_distance < 1500.0) && (ZL_distance > 0.01) && (COM_att_norm > 0.0001))
                {
                    can_main.setValue_visible(0x00);
                    can_main.setValue_valid(0x00);
                    can_main.setValue_transplane(flag_chuanwang);

                    can_hpyo.setValue_visible(0x00);
                    can_hpyo.setValue_valid(0x00);
                    can_hpyo.setValue_transplane(flag_chuanwang);

                    result_out.valid = 0x00;
                    result_out.Isvisible = 0x00;
                    result_out.TransPlane = flag_chuanwang;
                }
                else
                {
                    can_main.setValue_visible(0x01);
                    can_main.setValue_valid(0x01);
                    can_main.setValue_transplane(0x00);

                    can_hpyo.setValue_visible(0x01);
                    can_hpyo.setValue_valid(0x01);
                    can_hpyo.setValue_transplane(0x00);

                    result_out.valid = 0x01;
                    result_out.Isvisible = 0x01;
                    result_out.TransPlane = 0x00;
                }

                can_main.setValue_coor_x((float)J0_WX_15s[0]);
                can_main.setValue_coor_y((float)J0_WX_15s[1]);
                can_main.setValue_coor_z((float)J0_WX_15s[2]);
                // can_main.setValue_transplane(flag_chuanwang);

                can_hpyo.setValue_coor_x((float)J0_WX_15s[0]);
                can_hpyo.setValue_coor_y((float)J0_WX_15s[1]);
                can_hpyo.setValue_coor_z((float)J0_WX_15s[2]);
                // can_hpyo.setValue_transplane(flag_chuanwang);

                result_out.bottomCenter.x() = (float)J0_WX_15s[0];
                result_out.bottomCenter.y() = (float)J0_WX_15s[1];
                result_out.bottomCenter.z() = (float)J0_WX_15s[2];
                // result_out.TransPlane = flag_chuanwang;
            }
            else
            {
                if ((ZL_distance < 1500.0) && (ZL_distance > 0.01) && (COM_att_norm > 0.0001))
                {
                    can_main.setValue_visible(0x00);
                    can_main.setValue_valid(0x00);

                    can_hpyo.setValue_visible(0x00);
                    can_hpyo.setValue_valid(0x00);

                    result_out.valid = 0x00;
                    result_out.Isvisible = 0x00;
                }
                else
                {
                    can_main.setValue_visible(0x01);
                    can_main.setValue_valid(0x01);

                    can_hpyo.setValue_visible(0x01);
                    can_hpyo.setValue_valid(0x01);

                    result_out.valid = 0x01;
                    result_out.Isvisible = 0x01;
                }

                can_main.setValue_coor_x((float)JBottom_WX_15s[0]);
                can_main.setValue_coor_y((float)JBottom_WX_15s[1]);
                can_main.setValue_coor_z((float)JBottom_WX_15s[2]);
                can_main.setValue_transplane(flag_chuanwang);

                can_hpyo.setValue_coor_x((float)JBottom_WX_15s[0]);
                can_hpyo.setValue_coor_y((float)JBottom_WX_15s[1]);
                can_hpyo.setValue_coor_z((float)JBottom_WX_15s[2]);
                can_hpyo.setValue_transplane(flag_chuanwang);

                result_out.bottomCenter.x() = (float)JBottom_WX_15s[0];
                result_out.bottomCenter.y() = (float)JBottom_WX_15s[1];
                result_out.bottomCenter.z() = (float)JBottom_WX_15s[2];
                result_out.TransPlane = flag_chuanwang;
            }

            // printf("azimuth_deg=%.6f\n", azimuth_deg);
            // printf("elevation_deg=%.6f\n", elevation_deg);

            // printf("穿网前底部中心点（网系坐标系下）结果：%.6f %.6f %.6f\n",
            //     JBottom_WX_15s[0], JBottom_WX_15s[1], JBottom_WX_15s[2]);

            // if (flag_chuanwang == 1) {
            //     printf("穿网后火箭截面与网系的交点结果：%.6f %.6f %.6f\n",
            //         J0_WX_15s[0], J0_WX_15s[1], J0_WX_15s[2]);
            // } else {
            //     printf("未穿网\n");
            // }
            if(recursion_overflag == 1)
            {
                can_main.setValue_visible(0x01);
                can_main.setValue_valid(0x01);

                can_hpyo.setValue_visible(0x01);
                can_hpyo.setValue_valid(0x01);

                result_out.valid = 0x01;
                result_out.Isvisible = 0x01;
            }

            can_main.setValue_output_delay(0.0);
            can_main.setValue_state(0x00);
            can_main.setValue_timestate(0x00);
            can_main.setValue_timestamp(0);
            can_main.setValue_state_lidar(0x00);
            can_main.setValue_state_value(0x00);
            can_main.setValue_projection_B((float)elevation_deg);
            can_main.setValue_projection_A((float)azimuth_deg);
            can_main.setValue_count(testCount);

            can_hpyo.setValue_output_delay(0.0);
            can_hpyo.setValue_state(0x00);
            can_hpyo.setValue_timestate(0x00);
            can_hpyo.setValue_timestamp(0);
            can_hpyo.setValue_state_lidar(0x00);
            can_hpyo.setValue_state_value(0x00);
            can_hpyo.setValue_projection_B((float)elevation_deg);
            can_hpyo.setValue_projection_A((float)azimuth_deg);
            can_hpyo.setValue_count(testCount);

            result_out.timestamp = 0;
            result_out.elevation_angle = (float)elevation_deg;
            result_out.azimuth_angle = (float)azimuth_deg;
            result_out.delay_time = 0.0;
            result_out.angle_zero_state = 0x00;
            result_out.each_lidar_status = 0x00;
            result_out.time_sync_state = 0x00;
            result_out.lidar_status = 0x00;

            testCount++;
            if (testCount > 255)
            {
                testCount = 0;
            }

            result_out.dataState = 1;
            can_main.setValue_datasource((char)(result_out.dataState));
            can_hpyo.setValue_datasource((char)(result_out.dataState));

            auto now = std::chrono::system_clock::now();
            // 转换为自1970年以来的毫秒数
            uint64_t timestamp_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                                        now.time_since_epoch())
                                        .count();

            // 日志记录
            logger.setValue_result(result_out.timestamp,
                                   result_out.bottomCenter.x(), result_out.bottomCenter.y(), result_out.bottomCenter.z(),
                                   result_out.axis.x(), result_out.axis.y(), result_out.axis.z(),
                                   result_out.azimuth_angle, result_out.elevation_angle,
                                   result_out.cloudNumber, result_out.fitResidual, result_out.TransPlane, result_out.delay_time,
                                   result_out.Isvisible, result_out.IsMeasure, result_out.lidar_status, (int)(result_out.each_lidar_status), result_out.valid, result_out.dataState, timestamp_ms);
        }
        else if (simu == 3)
        {

            if (c1_predictor_)
            {
                Eigen::Vector3f c, a;
                bool predict_success = false;

                // 情况1：尝试预测（仅在延迟<=30秒时）
                if (c1_delay <= 30.0 && c1_predictor_->predictOnly(t_pred_c1, c, a))
                {
                    c1_pred = makePoseFromPredict(c, a, 1, t_pred_c1);
                    // if(c1_time_sync_state_.load(std::memory_order_relaxed))
                    // {
                    //     c1_pred.time_sync_state = 1;
                    // }
                    // else
                    // {
                    //     c1_pred.time_sync_state = 0;
                    // }
                    predict_success = true;

                    // 根据时间逻辑设置状态
                    double delay_ms = c1_delay * 1000.0;
                    c1_pred.delay_time = delay_ms;
                    if (delay_ms <= 50.0)
                    {
                        c1_pred.IsMeasure = 1;
                        c1_pred.valid = 0; // 有效
                    }
                    else if (delay_ms <= 1000.0)
                    {
                        c1_pred.IsMeasure = 0;
                        c1_pred.valid = 0; // 有效（延迟在50ms到1秒之间）
                    }
                    else if (c1_delay <= 30.0)
                    {
                        c1_pred.IsMeasure = 0;
                        c1_pred.valid = 1; // 无效（延迟在1秒到30秒之间）
                    }
                    else
                    {
                        // 这里理论上不会执行，因为上面已经判断c1_delay <= 30.0
                        c1_pred.valid = 1; // 无效
                        c1_pred.IsMeasure = 0;
                    }
                }

                // 处理预测结果
                if (predict_success)
                {
                    if (c1_predictor_->time_first < 1e-9)
                    {
                        c1_predictor_->time_first = now_sec;
                    }
                    /*如果超过1s或者更新次数超过10次，就开始输出*/
                    if (c1_pred.valid == 0 && (now_sec - c1_predictor_->time_first > 1.0 || c1_predictor_->num_inter_Update >= 100))
                    {
                        // 当前预测有效：更新上一次有效结果
                        last_valid_result_c1_ = c1_pred;
                        has_c1 = true;
                    }
                    else
                    {
                        // 当前预测无效：使用上一次有效结果
                        if (last_valid_result_c1_.valid == 0)
                        { // 确保上一次结果本身有效
                            c1_pred = last_valid_result_c1_;
                            c1_pred.timestamp = t_pred_c1; // 更新时间戳为当前时刻
                            c1_pred.valid = 1;             // 标记为无效（因为当前预测无效）
                            c1_pred.IsMeasure = 0;         // 不是实时测量
                            has_c1 = true;
                        }
                        // 如果上一次结果也无效，则不输出任何结果
                    }
                }
                else
                {
                    // 预测失败或延迟超过30秒：使用上一次有效结果
                    if (last_valid_result_c1_.valid == 0)
                    { // 确保上一次结果本身有效
                        c1_pred = last_valid_result_c1_;
                        c1_pred.timestamp = t_pred_c1; // 更新时间戳为当前时刻
                        c1_pred.valid = 1;             // 标记为无效（因为当前没有有效预测）
                        c1_pred.IsMeasure = 0;         // 不是实时测量
                        has_c1 = true;
                    }
                    // 如果上一次结果无效，则不输出任何结果
                }

                // 可视化
                if (has_c1)
                {
                    posedetect_vis::publishPredictCylinder(
                        c1_pred.bottomCenter.cast<float>(),
                        c1_pred.axis.cast<float>(),
                        static_cast<float>(params_.rocket_radius),
                        static_cast<float>(params_.rocket_length),
                        1);
                }
            }
            if (c2_predictor_)
            {
                Eigen::Vector3f c, a;
                bool predict_success = false;

                // 情况1：尝试预测（仅在延迟<=30秒时）
                if (c2_delay <= 30.0 && c2_predictor_->predictOnly(t_pred_c2, c, a))
                {
                    c2_pred = makePoseFromPredict(c, a, 2, t_pred_c2);
                    // if(c2_time_sync_state_.load(std::memory_order_relaxed))
                    // {
                    //     c2_pred.time_sync_state = 1;
                    // }
                    // else
                    // {
                    //     c2_pred.time_sync_state = 0;
                    // }
                    predict_success = true;

                    // 根据时间逻辑设置状态
                    double delay_ms = c2_delay * 1000.0;
                    c2_pred.delay_time = delay_ms;
                    if (delay_ms <= 50.0)
                    {
                        c2_pred.IsMeasure = 1;
                        c2_pred.valid = 0; // 有效
                    }
                    else if (delay_ms <= 1000.0)
                    {
                        c2_pred.IsMeasure = 0;
                        c2_pred.valid = 0; // 有效（延迟在50ms到1秒之间）
                    }
                    else if (c2_delay <= 30.0)
                    {
                        c2_pred.IsMeasure = 0;
                        c2_pred.valid = 1; // 无效（延迟在1秒到30秒之间）
                    }
                    else
                    {
                        // 这里理论上不会执行，因为上面已经判断c2_delay <= 30.0
                        c2_pred.valid = 1; // 无效
                        c2_pred.IsMeasure = 0;
                    }
                }

                // 处理预测结果
                if (predict_success)
                {
                    if (c2_predictor_->time_first < 1e-9)
                    {
                        c2_predictor_->time_first = now_sec;
                    }
                    if (c2_pred.valid == 0 && (now_sec - c2_predictor_->time_first > 1.0 || c2_predictor_->num_inter_Update >= 100))
                    {
                        // 当前预测有效：更新上一次有效结果
                        last_valid_result_c2_ = c2_pred;
                        has_c2 = true;
                    }
                    else
                    {
                        // 当前预测无效：使用上一次有效结果
                        if (last_valid_result_c2_.valid == 0)
                        { // 确保上一次结果本身有效
                            c2_pred = last_valid_result_c2_;
                            c2_pred.timestamp = t_pred_c2; // 更新时间戳为当前时刻
                            c2_pred.valid = 1;             // 标记为无效（因为当前预测无效）
                            c2_pred.IsMeasure = 0;         // 不是实时测量
                            has_c2 = true;
                        }
                        // 如果上一次结果也无效，则不输出任何结果
                    }
                }
                else
                {
                    // 预测失败或延迟超过30秒：使用上一次有效结果
                    if (last_valid_result_c2_.valid == 0)
                    { // 确保上一次结果本身有效
                        c2_pred = last_valid_result_c2_;
                        c2_pred.timestamp = t_pred_c2; // 更新时间戳为当前时刻
                        c2_pred.valid = 1;             // 标记为无效（因为当前没有有效预测）
                        c2_pred.IsMeasure = 0;         // 不是实时测量
                        has_c2 = true;
                    }
                    // 如果上一次结果无效，则不输出任何结果
                }

                // 预测结果可视化（单机2）
                if (has_c2)
                {
                    posedetect_vis::publishPredictCylinder(
                        c2_pred.bottomCenter.cast<float>(),
                        c2_pred.axis.cast<float>(),
                        static_cast<float>(params_.rocket_radius),
                        static_cast<float>(params_.rocket_length),
                        2);
                }
            }

            if (has_c1 || has_c2)
            {
                have_any = true;
                if (has_c1 && has_c2)
                {
                    result = decideTwo(c1_pred, c2_pred);
                }
                else if (has_c1)
                {
                    result = decideOne(c1_pred);
                }
                else
                {
                    result = decideOne(c2_pred);
                }

                // 输出调试日志：c1_pred、c2_pred、result 到文件
                {
                    static std::ofstream log_c1("/home/zxy/results/c1_pred.log", std::ios::app);
                    static std::ofstream log_c2("/home/zxy/results/c2_pred.log", std::ios::app);
                    static std::ofstream log_result("/home/zxy/results/result.log", std::ios::app);
                    static bool header_written = false;

                    if (!header_written)
                    {
                        if (log_c1.is_open())
                        {
                            log_c1 << "timestamp bottomCenter_x bottomCenter_y bottomCenter_z axis_x axis_y axis_z azimuth elevation IsMeasure valid delay" << std::endl;
                            log_c1.flush();
                        }
                        if (log_c2.is_open())
                        {
                            log_c2 << "timestamp bottomCenter_x bottomCenter_y bottomCenter_z axis_x axis_y axis_z azimuth elevation IsMeasure valid delay" << std::endl;
                            log_c2.flush();
                        }
                        if (log_result.is_open())
                        {
                            log_result << "timestamp bottomCenter_x bottomCenter_y bottomCenter_z axis_x axis_y axis_z IsMeasure valid azimuth elevation" << std::endl;
                            log_result.flush();
                        }
                        header_written = true;
                    }

                    if (has_c1 && log_c1.is_open())
                    {
                        log_c1 << std::fixed << std::setprecision(6)
                               << c1_pred.timestamp << " "
                               << c1_pred.bottomCenter.x() << " " << c1_pred.bottomCenter.y() << " " << c1_pred.bottomCenter.z() << " "
                               << c1_pred.axis.x() << " " << c1_pred.axis.y() << " " << c1_pred.axis.z() << " "
                               << c1_pred.azimuth_angle << " " << c1_pred.elevation_angle << " "
                               << (int)c1_pred.IsMeasure << " "
                               << (int)c1_pred.valid << " "
                               << c1_delay << std::endl;
                        log_c1.flush();
                    }

                    if (has_c2 && log_c2.is_open())
                    {
                        log_c2 << std::fixed << std::setprecision(6)
                               << c2_pred.timestamp << " "
                               << c2_pred.bottomCenter.x() << " " << c2_pred.bottomCenter.y() << " " << c2_pred.bottomCenter.z() << " "
                               << c2_pred.axis.x() << " " << c2_pred.axis.y() << " " << c2_pred.axis.z() << " "
                               << c2_pred.azimuth_angle << " " << c2_pred.elevation_angle << " "
                               << (int)c2_pred.IsMeasure << " "
                               << (int)c2_pred.valid << " "
                               << c2_delay << std::endl;
                        log_c2.flush();
                    }

                    if (have_any && log_result.is_open())
                    {
                        log_result << std::fixed << std::setprecision(6)
                                   << result.timestamp << " "
                                   << result.bottomCenter.x() << " " << result.bottomCenter.y() << " " << result.bottomCenter.z() << " "
                                   << result.axis.x() << " " << result.axis.y() << " " << result.axis.z() << " "
                                   << (int)result.IsMeasure << " "
                                   << (int)result.valid << " "
                                   << result.azimuth_angle << " "
                                   << result.elevation_angle << std::endl;
                        log_result.flush();
                    }
                }

                // 输出到 can_main & can_hpyo
                // printf("%s %f \n", "delay_time = ", result.delay_time);
                // printf("%s %f \n", "timestamp = ", result.timestamp);
                can_main.setValue_coor_x(result.bottomCenter.x());
                can_main.setValue_coor_y(result.bottomCenter.y());
                can_main.setValue_coor_z(result.bottomCenter.z());
                can_main.setValue_projection_B(result.elevation_angle);
                can_main.setValue_projection_A(result.azimuth_angle);
                can_main.setValue_output_delay(result.delay_time);
                // can_main.setValue_state((char)(result.lidar_status));
                // can_main.setValue_timestate((char)(result.time_sync_state));
                can_main.setValue_visible((char)(result.Isvisible));
                can_main.setValue_valid((char)(result.valid));
                can_main.setValue_timestamp(result.timestamp * 1e3);
                // can_main.setValue_state_lidar((char)(result.each_lidar_status));
                can_main.setValue_transplane((char)(result.TransPlane));
                can_main.setValue_predict((char)(result.IsMeasure));
                can_main.setValue_state_value((char)(result.angle_zero_state));

                can_hpyo.setValue_coor_x(result.bottomCenter.x());
                can_hpyo.setValue_coor_y(result.bottomCenter.y());
                can_hpyo.setValue_coor_z(result.bottomCenter.z());
                can_hpyo.setValue_projection_B(result.elevation_angle);
                can_hpyo.setValue_projection_A(result.azimuth_angle);
                can_hpyo.setValue_output_delay(result.delay_time);
                // can_hpyo.setValue_state((char)(result.lidar_status));
                // can_hpyo.setValue_timestate((char)(result.time_sync_state));
                can_hpyo.setValue_visible((char)(result.Isvisible));
                can_hpyo.setValue_valid((char)(result.valid));
                can_hpyo.setValue_timestamp(result.timestamp * 1e3);
                // can_hpyo.setValue_state_lidar((char)(result.each_lidar_status));
                can_hpyo.setValue_transplane((char)(result.TransPlane));
                can_hpyo.setValue_predict((char)(result.IsMeasure));
                can_hpyo.setValue_state_value((char)(result.angle_zero_state));
            }

            uint8_t each_mask = 0xFF;
            int overall = 1;
            try
            {
                LidarHeart::getStatus(each_mask, overall);
                result.each_lidar_status = each_mask;
                result.lidar_status = overall;
            }
            catch (const std::exception &e)
            {
                ROS_WARN_THROTTLE(5.0, "LidarHeart getStatus failed: %s", e.what());
            }

            if ((!c1_time_sync_state_.load(std::memory_order_relaxed)) && (!c2_time_sync_state_.load(std::memory_order_relaxed)))
            {
                result.time_sync_state = 0;
            }
            else
            {
                result.time_sync_state = 1;
            }

            result.dataState = 0;
            can_main.setValue_datasource((char)(result.dataState));
            can_hpyo.setValue_datasource((char)(result.dataState));

            can_main.setValue_timestate((char)(result.time_sync_state));
            can_main.setValue_state((char)(result.lidar_status));
            can_main.setValue_state_lidar((char)(result.each_lidar_status));
            can_main.setValue_count(realCount);

            can_hpyo.setValue_timestate((char)(result.time_sync_state));
            can_hpyo.setValue_state((char)(result.lidar_status));
            can_hpyo.setValue_state_lidar((char)(result.each_lidar_status));

            can_hpyo.setValue_count(realCount);

            realCount++;
            if (realCount > 255)
            {
                realCount = 0;
            }

            auto now_lidar = std::chrono::system_clock::now();
            // 转换为自1970年以来的毫秒数
            uint64_t timestamp_ms_lidar = std::chrono::duration_cast<std::chrono::milliseconds>(
                                              now_lidar.time_since_epoch())
                                              .count();

            // 日志记录
            logger.setValue_result(result.timestamp,
                                   result.bottomCenter.x(), result.bottomCenter.y(), result.bottomCenter.z(),
                                   result.axis.x(), result.axis.y(), result.axis.z(),
                                   result.azimuth_angle, result.elevation_angle,
                                   result.cloudNumber, result.fitResidual, result.TransPlane, result.delay_time,
                                   result.Isvisible, result.IsMeasure, result.lidar_status, (int)(result.each_lidar_status), result.valid, result.dataState, timestamp_ms_lidar);
        }
        else
        {   // *****************实际测量数据处理 决策逻辑开始******************
            PoseData result_out;
            result_out.ProjectedCenter.x() = 0;
            result_out.ProjectedCenter.y() = 0;
            result_out.axis.x() = 0.0;
            result_out.axis.y() = 0.0;
            result_out.axis.z() = 0.0;

            result_out.cloudNumber = 0;
            result_out.fitResidual = 0.0;
            result_out.is_predict = 0.0;

            // double COM_ZL[3] = {0};
            // double COM_V[3] = {0};
            // double COM_att[3] = {0};
            // double SheXiang = SheXiang_para;
            // double att[3] = {0};
            // double Lever[3] = {Lever_para[0], Lever_para[1], Lever_para[2]};
            // double JBottom2JM_JT[3] = {JT_para[0], JT_para[1],JT_para[2]};
            recursion_time++;
            if (udp_client.get_update_flag())
            {
                udp_client.set_update_flag(0);
                recursion_time = 0;
                recursion_overflag = 0;

                COM_V[0] = udp_client.get_vx_value();
                COM_V[1] = udp_client.get_vy_value();
                COM_V[2] = udp_client.get_vz_value();

                COM_ZL[0] = udp_client.get_x_value() + COM_V[0] * delay_JT_ms / 1000.0;
                COM_ZL[1] = udp_client.get_y_value() + COM_V[1] * delay_JT_ms / 1000.0;
                COM_ZL[2] = udp_client.get_z_value() + COM_V[2] * delay_JT_ms / 1000.0;

                COM_att[0] = udp_client.get_pitch_value();
                COM_att[1] = udp_client.get_yaw_value();
                COM_att[2] = udp_client.get_roll_value();

                att_hook[0] = udp_client.get_pitch_value();
                att_hook[1] = udp_client.get_yaw_value();
                att_hook[2] = udp_client.get_roll_value();

                vel[0] = udp_client.get_vx_value();
                vel[1] = udp_client.get_vy_value();
                vel[2] = udp_client.get_vz_value();
                
                can_main.setValue_predict(0x01);
                can_hpyo.setValue_predict(0x01);
                result_out.IsMeasure = 0x01;
            }
            else if (udp_client_bak.get_update_flag())
            {
                udp_client_bak.set_update_flag(0);
                recursion_time = 0;
                recursion_overflag = 0;

                COM_V[0] = udp_client_bak.get_vx_value();
                COM_V[1] = udp_client_bak.get_vy_value();
                COM_V[2] = udp_client_bak.get_vz_value();

                COM_ZL[0] = udp_client_bak.get_x_value() + COM_V[0] * delay_JT_ms / 1000.0;
                COM_ZL[1] = udp_client_bak.get_y_value() + COM_V[1] * delay_JT_ms / 1000.0;
                COM_ZL[2] = udp_client_bak.get_z_value() + COM_V[2] * delay_JT_ms / 1000.0;

                COM_att[0] = udp_client_bak.get_pitch_value();
                COM_att[1] = udp_client_bak.get_yaw_value();
                COM_att[2] = udp_client_bak.get_roll_value();

                att_hook[0] = udp_client_bak.get_pitch_value();
                att_hook[1] = udp_client_bak.get_yaw_value();
                att_hook[2] = udp_client_bak.get_roll_value();

                vel[0] = udp_client_bak.get_vx_value();
                vel[1] = udp_client_bak.get_vy_value();
                vel[2] = udp_client_bak.get_vz_value();

                can_main.setValue_predict(0x01);
                can_hpyo.setValue_predict(0x01);
                result_out.IsMeasure = 0x01;
            }
            else
            {
                if (recursion_time < 200)
                {
                    recursion_overflag = 0;
                    // 递推
                    COM_ZL[0] += COM_V[0] * 0.01;
                    COM_ZL[1] += COM_V[1] * 0.01;
                    COM_ZL[2] += COM_V[2] * 0.01;

                    can_main.setValue_predict(0x00);
                    can_hpyo.setValue_predict(0x00);
                    result_out.IsMeasure = 0x00;
                }
                else
                {
                    recursion_overflag = 1;
                }

            }

            double ZL_distance = sqrt(COM_ZL[0] * COM_ZL[0] + COM_ZL[1] * COM_ZL[1] + COM_ZL[2] * COM_ZL[2]);
            double COM_att_norm = sqrt(COM_att[0] * COM_att[0] + COM_att[1] * COM_att[1] + COM_att[2] * COM_att[2]);

            if (can_main.get_rec_flag())
            {
                att[0] = can_main.get_pitch_value();
                att[1] = can_main.get_roll_value();
                att[2] = can_main.get_yaw_value();
                                               
                att_net[0] = can_main.get_pitch_value();
                att_net[1] = can_main.get_roll_value();
                att_net[2] = can_main.get_yaw_value();
            }
            else if (can_hpyo.get_rec_flag())
            {
                att[0] = can_hpyo.get_pitch_value();
                att[1] = can_hpyo.get_roll_value();
                att[2] = can_hpyo.get_yaw_value();

                att_net[0] = can_hpyo.get_pitch_value();
                att_net[1] = can_hpyo.get_roll_value();
                att_net[2] = can_hpyo.get_yaw_value();
            }
            else
            {
                att[0] = GZ_para[0];
                att[1] = GZ_para[1];
                att[2] = GZ_para[2];

                att_net[0] = GZ_para[0];
                att_net[1] = GZ_para[1];
                att_net[2] = GZ_para[2];
            }

            double COM_ZL_processed[3] = {COM_ZL[0], COM_ZL[1], COM_ZL[2]};
            double JBottom_ZL[3] = {0};
            calculate_ZL_WX(COM_ZL_processed, COM_att, JBottom2JM_JT, JBottom_ZL);

            double CM_WX[3] = {0};
            calculate_CM_WX(COM_ZL_processed, SheXiang, att, Lever, CM_WX);

            double JBottom_WX[3] = {0};
            calculate_CM_WX(JBottom_ZL, SheXiang, att, Lever, JBottom_WX);

            double CM_WX_15s[3] = {CM_WX[0], -CM_WX[2], CM_WX[1]};
            double JBottom_WX_15s[3] = {JBottom_WX[0], -JBottom_WX[2], JBottom_WX[1]};
            

            double rocks[3];
            double rock[3];

            rocks[0] = CM_WX_15s[0] - JBottom_WX_15s[0];
            rocks[1] = CM_WX_15s[1] - JBottom_WX_15s[1];
            rocks[2] = CM_WX_15s[2] - JBottom_WX_15s[2];

            double rock_norm = sqrt(rocks[0] * rocks[0] + rocks[1] * rocks[1] + rocks[2] * rocks[2]);
            if (fabs(rock_norm) < 1e-9)
            {
                rock[0] = rock[1] = rock[2] = 0.0;
            }
            else
            {
                rock[0] = rocks[0] / rock_norm;
                rock[1] = rocks[1] / rock_norm;
                rock[2] = rocks[2] / rock_norm;
            }

            double azimuth_deg, elevation_deg, theta;
            azimuth_deg = atan2(rock[1], rock[0]) * 180.0f / PI;
            if (azimuth_deg < 0)
            {
                azimuth_deg += 360.0;
            }

            theta = acos(rock[2]) * 180.0f / PI;
            elevation_deg = 90.0 - theta;
            if (elevation_deg < 0)
            {
                elevation_deg = 0.0;
            }

            char flag_chuanwang = 0;
            double J0_WX_15s[3] = {0.0, 0.0, 0.0};

            if (JBottom_WX[1] < 0)
            {
                flag_chuanwang = 1;

                double J0_WX[3];
                double diff[3] = {
                    JBottom_WX[0] - CM_WX[0],
                    JBottom_WX[1] - CM_WX[1],
                    JBottom_WX[2] - CM_WX[2]};

                double denominator = fabs(JBottom_WX[1]) + CM_WX[1];
                if (fabs(denominator) < 1e-9)
                {
                    J0_WX[0] = CM_WX[0];
                    J0_WX[1] = CM_WX[1];
                    J0_WX[2] = CM_WX[2];
                }
                else
                {
                    double scalar = CM_WX[1] / denominator;
                    J0_WX[0] = diff[0] * scalar + CM_WX[0];
                    J0_WX[1] = diff[1] * scalar + CM_WX[1];
                    J0_WX[2] = diff[2] * scalar + CM_WX[2];
                }

                J0_WX_15s[0] = J0_WX[0];
                J0_WX_15s[1] = -J0_WX[2];
                J0_WX_15s[2] = JBottom_WX[1];

                if ((ZL_distance < 1500.0) && (ZL_distance > 0.01) && (COM_att_norm > 0.0001))
                {
                    result_out.valid = 0x00;
                    result_out.Isvisible = 0x00;
                    result_out.TransPlane = flag_chuanwang;
                }
                else
                {
                    result_out.valid = 0x01;
                    result_out.Isvisible = 0x01;
                    result_out.TransPlane = 0x00;
                }
                result_out.bottomCenter.x() = (float)J0_WX_15s[0];
                result_out.bottomCenter.y() = (float)J0_WX_15s[1];
                result_out.bottomCenter.z() = (float)J0_WX_15s[2];
                // result_out.TransPlane = flag_chuanwang;
            }
            else
            {
                if ((ZL_distance < 1500.0) && (ZL_distance > 0.01) && (COM_att_norm > 0.0001))
                {
                    result_out.valid = 0x00;
                    result_out.Isvisible = 0x00;
                }
                else
                {
                    can_main.setValue_visible(0x01);
                    can_main.setValue_valid(0x01);

                    can_hpyo.setValue_visible(0x01);
                    can_hpyo.setValue_valid(0x01);

                    result_out.valid = 0x01;
                    result_out.Isvisible = 0x01;
                }
                result_out.bottomCenter.x() = (float)JBottom_WX_15s[0];
                result_out.bottomCenter.y() = (float)JBottom_WX_15s[1];
                result_out.bottomCenter.z() = (float)JBottom_WX_15s[2];
                result_out.TransPlane = flag_chuanwang;
            }

            if(recursion_overflag == 1)
            {
                can_main.setValue_visible(0x01);
                can_main.setValue_valid(0x01);

                can_hpyo.setValue_visible(0x01);
                can_hpyo.setValue_valid(0x01);

                result_out.valid = 0x01;
                result_out.Isvisible = 0x01;
            }

            result_out.timestamp = 0;
            result_out.elevation_angle = (float)elevation_deg;
            result_out.azimuth_angle = (float)azimuth_deg;
            result_out.delay_time = 0.0;
            result_out.angle_zero_state = 0x00;
            result_out.each_lidar_status = 0x00;
            result_out.time_sync_state = 0x00;
            result_out.lidar_status = 0x00;

            testCount++;
            if (testCount > 255)
            {
                testCount = 0;
            }

            if (c1_predictor_)
            {
                Eigen::Vector3f c, a;
                bool predict_success = false;

                if (c1_delay <= 30.0 && c1_predictor_->predictOnly(t_pred_c1, c, a))
                {
                    c1_pred = makePoseFromPredict(c, a, 1, t_pred_c1);
                    predict_success = true;

                    double delay_ms = c1_delay * 1000.0;
                    c1_pred.delay_time = delay_ms;
                    if (delay_ms <= 50.0)
                    {
                        c1_pred.IsMeasure = 1;
                        c1_pred.valid = 0;
                    }
                    else if (delay_ms <= 1000.0)
                    {
                        c1_pred.IsMeasure = 0;
                        c1_pred.valid = 0;
                    }
                    else if (c1_delay <= 30.0)
                    {
                        c1_pred.IsMeasure = 0;
                        c1_pred.valid = 1;
                    }
                    else
                    {
                        c1_pred.valid = 1;
                        c1_pred.IsMeasure = 0;
                    }
                }

                if (predict_success)
                {
                    if (c1_predictor_->time_first < 1e-9)
                    {
                        c1_predictor_->time_first = now_sec;
                    }
                    /*如果超过1s或者更新次数超过10次，就开始输出*/
                    if (c1_pred.valid == 0 && (now_sec - c1_predictor_->time_first > 1.0 || c1_predictor_->num_inter_Update >= 100))
                    {
                        // 当前预测有效：更新上一次有效结果
                        last_valid_result_c1_ = c1_pred;
                        has_c1 = true;
                    }
                    else
                    {
                        if (last_valid_result_c1_.valid == 0)
                        {
                            c1_pred = last_valid_result_c1_;
                            c1_pred.timestamp = t_pred_c1; // 更新时间戳为当前时刻
                            c1_pred.valid = 1;             // 标记为无效（因为当前预测无效）
                            c1_pred.IsMeasure = 0;         // 不是实时测量
                            has_c1 = true;
                        }
                    }
                }
                else
                {
                    if (last_valid_result_c1_.valid == 0)
                    {
                        c1_pred = last_valid_result_c1_;
                        c1_pred.timestamp = t_pred_c1; // 更新时间戳为当前时刻
                        c1_pred.valid = 1;             // 标记为无效（因为当前没有有效预测）
                        c1_pred.IsMeasure = 0;         // 不是实时测量
                        has_c1 = true;
                    }
                }

                if (has_c1)
                {
                    posedetect_vis::publishPredictCylinder(
                        c1_pred.bottomCenter.cast<float>(),
                        c1_pred.axis.cast<float>(),
                        static_cast<float>(params_.rocket_radius),
                        static_cast<float>(params_.rocket_length),
                        1);
                }
            }
            if (c2_predictor_)
            {
                Eigen::Vector3f c, a;
                bool predict_success = false;

                if (c2_delay <= 30.0 && c2_predictor_->predictOnly(t_pred_c2, c, a))
                {
                    c2_pred = makePoseFromPredict(c, a, 2, t_pred_c2);
                    predict_success = true;

                    double delay_ms = c2_delay * 1000.0;
                    c2_pred.delay_time = delay_ms;
                    if (delay_ms <= 50.0)
                    {
                        c2_pred.IsMeasure = 1;
                        c2_pred.valid = 0;
                    }
                    else if (delay_ms <= 1000.0)
                    {
                        c2_pred.IsMeasure = 0;
                        c2_pred.valid = 0;
                    }
                    else if (c2_delay <= 30.0)
                    {
                        c2_pred.IsMeasure = 0;
                        c2_pred.valid = 1;
                    }
                    else
                    {
                        c2_pred.valid = 1;
                        c2_pred.IsMeasure = 0;
                    }
                }

                if (predict_success)
                {
                    if (c2_predictor_->time_first < 1e-9)
                    {
                        c2_predictor_->time_first = now_sec;
                    }
                    if (c2_pred.valid == 0 && (now_sec - c2_predictor_->time_first > 1.0 || c2_predictor_->num_inter_Update >= 100))
                    {
                        // 当前预测有效：更新上一次有效结果
                        last_valid_result_c2_ = c2_pred;
                        has_c2 = true;
                    }
                    else
                    {
                        if (last_valid_result_c2_.valid == 0)
                        {
                            c2_pred = last_valid_result_c2_;
                            c2_pred.timestamp = t_pred_c2;
                            c2_pred.valid = 1;
                            c2_pred.IsMeasure = 0;
                            has_c2 = true;
                        }
                    }
                }
                else
                {
                    if (last_valid_result_c2_.valid == 0)
                    {
                        c2_pred = last_valid_result_c2_;
                        c2_pred.timestamp = t_pred_c2;
                        c2_pred.valid = 1;
                        c2_pred.IsMeasure = 0;
                        has_c2 = true;
                    }
                }

                if (has_c2)
                {
                    posedetect_vis::publishPredictCylinder(
                        c2_pred.bottomCenter.cast<float>(),
                        c2_pred.axis.cast<float>(),
                        static_cast<float>(params_.rocket_radius),
                        static_cast<float>(params_.rocket_length),
                        2);
                }
            }

            if (has_c1 || has_c2)
            {
                have_any = true;
                if (has_c1 && has_c2)
                {
                    result = decideTwo(c1_pred, c2_pred);
                }
                else if (has_c1)
                {
                    result = decideOne(c1_pred);
                }
                else
                {
                    result = decideOne(c2_pred);
                }
            }

            uint8_t each_mask = 0xFF;
            int overall = 1;
            try
            {
                LidarHeart::getStatus(each_mask, overall);
                result.each_lidar_status = each_mask;
                result.lidar_status = overall;
            }
            catch (const std::exception &e)
            {
                ROS_WARN_THROTTLE(5.0, "LidarHeart getStatus failed: %s", e.what());
            }

            if ((!c1_time_sync_state_.load(std::memory_order_relaxed)) && (!c2_time_sync_state_.load(std::memory_order_relaxed)))
            {
                result.time_sync_state = 0;
            }
            else
            {
                result.time_sync_state = 1;
            }

            realCount++;
            if (realCount > 255)
            {
                realCount = 0;
            }

            // fusion New code
            result_out.dataState = 1;
            result.dataState = 0;
            
//            pos_bottom_N[0] = result.bottomCenter.x();
//            pos_bottom_N[1] = result.bottomCenter.y();
//            pos_bottom_N[2] = result.bottomCenter.z();

            static PoseData result_fusion_out;
            static float update_h = 200.0; //必须比较高
            static int valid_cnt = 0;
            static float old_result_xyz[3] = {0.0, 0.0, 0.0};
            static double ipos_bottom_N[3] = {0.0, 0.0, 200.0};

            //更新update_h高度.默认值200
            if(result.valid == 0)
            {
               update_h = result.bottomCenter.z(); 
            }else if(result_out.valid == 0)
            {
               update_h = result_out.bottomCenter.z(); 
            }

            result_fusion_out = result_out;//默认使用J上下传数据结果
            if( (update_h<1500) && (update_h>110)){
                if(result_out.valid == 0){
                    result_fusion_out = result_out;
                    ipos_bottom_N[0] = result_out.bottomCenter.x();
                    ipos_bottom_N[1] = result_out.bottomCenter.y();
                    ipos_bottom_N[2] = result_out.bottomCenter.z();
                    update_h = result_out.bottomCenter.z();
                }
            }else if(update_h <= 110){
                result_fusion_out.valid = 1; //默认状态无效
                if (result.valid == 0) //一旦有新的有效的位姿数据就更新ipos_bottom_N
                {
                    valid_cnt = 1;
                    result_fusion_out = result;
                    result_fusion_out.valid = 0;
                    update_h = result_fusion_out.bottomCenter.z();
                    ipos_bottom_N[0] = result.bottomCenter.x();
                    ipos_bottom_N[1] = result.bottomCenter.y();
                    ipos_bottom_N[2] = result.bottomCenter.z();
                }
                else
                {
                 //   if (valid_cnt == 1)  不论如何也要递推计算
                    {
                        //result_fusion_out = result;
                        //result_fusion_out.valid = 0;

                        // 递推  在pos_bottom_N基础上递推
                        //double CM_WX_15s[3] = {CM_WX[0], -CM_WX[2], CM_WX[1]};
                        double pos_hook[3] = {0}, pos_hook_N_new[3] = {0}, pos_bottom_N_new[3] = {0};
                        double pos_hook_N_new_1b[3] = {0}, pos_bottom_N_new_1b[3] = {0};
                        double  ipos_bottom_N_1b[3] = {ipos_bottom_N[0], ipos_bottom_N[2], -ipos_bottom_N[1]};
                        posbottomN2poshookL(ipos_bottom_N_1b, att_hook, att_net, pos_hook);

                        pos_hook[0] += vel[0] * 0.01;
                        pos_hook[1] += vel[1] * 0.01;
                        pos_hook[2] += vel[2] * 0.01;

                        poshookL2posbottomN(pos_hook, att_hook, att_net, pos_bottom_N_new_1b, pos_hook_N_new_1b);
                        pos_bottom_N_new = {pos_bottom_N_new_1b[0], -pos_bottom_N_new_1b[2], pos_bottom_N_new_1b[1]};
                        pos_hook_N_new = {pos_hook_N_new_1b[0], -pos_hook_N_new_1b[2], pos_hook_N_new_1b[1]};

                        ipos_bottom_N[0] = pos_bottom_N_new[0];
                        ipos_bottom_N[1] = pos_bottom_N_new[1];
                        ipos_bottom_N[2] = pos_bottom_N_new[2]; 

                        getRockDirectionAndFlags(pos_hook_N_new, pos_bottom_N_new,
                                                 &azimuth_deg, &elevation_deg, J0_WX_15s,
                                                 &flag_chuanwang);
                        if (flag_chuanwang == 0)
                        {
                            result_fusion_out.bottomCenter.x() = pos_bottom_N_new[0];
                            result_fusion_out.bottomCenter.y() = pos_bottom_N_new[1];
                            result_fusion_out.bottomCenter.z() = pos_bottom_N_new[2];
                        }
                        else
                        {
                            result_fusion_out.bottomCenter.x() = J0_WX_15s[0];
                            result_fusion_out.bottomCenter.y() = J0_WX_15s[1];
                            result_fusion_out.bottomCenter.z() = J0_WX_15s[2];
                        }
                        result_fusion_out.azimuth_angle = azimuth_deg;
                        result_fusion_out.elevation_angle = elevation_deg;
                        result_fusion_out.TransPlane = flag_chuanwang;

                        update_h = result_fusion_out.bottomCenter.z();
                    }
                }

                if (update_h <= 60 )
                {
                    //result_fusion_out = result_out;
                    result_fusion_out.valid = 0;
                    //update_h = result_fusion_out.bottomCenter.z();
                }
                else
                {
                    //result_fusion_out = result_out;
                    result_fusion_out.valid = 1;
                    //update_h = result_fusion_out.bottomCenter.z();
                }
            }

           
            can_main.setValue_coor_x(result_fusion_out.bottomCenter.x());
            can_main.setValue_coor_y(result_fusion_out.bottomCenter.y());
            can_main.setValue_coor_z(result_fusion_out.bottomCenter.z());
            can_main.setValue_projection_B(result_fusion_out.elevation_angle);
            can_main.setValue_projection_A(result_fusion_out.azimuth_angle);
            can_main.setValue_output_delay(result_fusion_out.delay_time);
            can_main.setValue_state((char)(result_fusion_out.lidar_status));
            can_main.setValue_timestate((char)(result_fusion_out.time_sync_state));
            can_main.setValue_visible((char)(result_fusion_out.Isvisible));
            can_main.setValue_valid((char)(result_fusion_out.valid));
            can_main.setValue_timestamp(result_fusion_out.timestamp * 1e3);
            can_main.setValue_state_lidar((char)(result_fusion_out.each_lidar_status));
            can_main.setValue_transplane((char)(result_fusion_out.TransPlane));
            can_main.setValue_predict((char)(result_fusion_out.IsMeasure));
            can_main.setValue_state_value((char)(result_fusion_out.angle_zero_state));
            can_main.setValue_datasource((char)(result_fusion_out.dataState));
            can_main.setValue_count(testCount);

            can_hpyo.setValue_coor_x(result_fusion_out.bottomCenter.x());
            can_hpyo.setValue_coor_y(result_fusion_out.bottomCenter.y());
            can_hpyo.setValue_coor_z(result_fusion_out.bottomCenter.z());
            can_hpyo.setValue_projection_B(result_fusion_out.elevation_angle);
            can_hpyo.setValue_projection_A(result_fusion_out.azimuth_angle);
            can_hpyo.setValue_output_delay(result_fusion_out.delay_time);
            can_hpyo.setValue_state((char)(result_fusion_out.lidar_status));
            can_hpyo.setValue_timestate((char)(result_fusion_out.time_sync_state));
            can_hpyo.setValue_visible((char)(result_fusion_out.Isvisible));
            can_hpyo.setValue_valid((char)(result_fusion_out.valid));
            can_hpyo.setValue_timestamp(result_fusion_out.timestamp * 1e3);
            can_hpyo.setValue_state_lidar((char)(result_fusion_out.each_lidar_status));
            can_hpyo.setValue_transplane((char)(result_fusion_out.TransPlane));
            can_hpyo.setValue_predict((char)(result_fusion_out.IsMeasure));
            can_hpyo.setValue_state_value((char)(result_fusion_out.angle_zero_state));
            can_hpyo.setValue_datasource((char)(result_fusion_out.dataState));
            can_main.setValue_count(testCount);

            auto now_fusion_lidar = std::chrono::system_clock::now();
            // 转换为自1970年以来的毫秒数
            uint64_t timestamp_ms_fusion = std::chrono::duration_cast<std::chrono::milliseconds>(
                                               now_fusion_lidar.time_since_epoch())
                                               .count();

            // 日志记录
            logger.setValue_result(result_fusion_out.timestamp,
                                   result_fusion_out.bottomCenter.x(), result_fusion_out.bottomCenter.y(), result_fusion_out.bottomCenter.z(),
                                   result_fusion_out.axis.x(), result_fusion_out.axis.y(), result_fusion_out.axis.z(),
                                   result_fusion_out.azimuth_angle, result_fusion_out.elevation_angle,
                                   result_fusion_out.cloudNumber, result_fusion_out.fitResidual, result_fusion_out.TransPlane, result_fusion_out.delay_time,
                                   result_fusion_out.Isvisible, result_fusion_out.IsMeasure, result_fusion_out.lidar_status, (int)(result_fusion_out.each_lidar_status), result_fusion_out.valid, result_fusion_out.dataState, timestamp_ms_fusion);
        }

        // 实时模式下触发 CAN 发送（实时模式不会启动回放线程）
        can_main.triggerSendRealtime();
        can_hpyo.triggerSendRealtime();

        ros::spinOnce();
        rate.sleep();
    }
}

void LidarFusionNode::getRockDirectionAndFlags(double CM_WX_15s[3], double JBottom_WX_15s[3], 
    double* rt_azimuth_deg, double* rt_elevation_deg,double J0_WX_15s[3],
    char*  rt_flag_chuanwang)
{
            double rocks[3];
            double rock[3];

            rocks[0] = CM_WX_15s[0] - JBottom_WX_15s[0];
            rocks[1] = CM_WX_15s[1] - JBottom_WX_15s[1];
            rocks[2] = CM_WX_15s[2] - JBottom_WX_15s[2];

            double rock_norm = sqrt(rocks[0] * rocks[0] + rocks[1] * rocks[1] + rocks[2] * rocks[2]);
            if (fabs(rock_norm) < 1e-9)
            {
                rock[0] = rock[1] = rock[2] = 0.0;
            }
            else
            {
                rock[0] = rocks[0] / rock_norm;
                rock[1] = rocks[1] / rock_norm;
                rock[2] = rocks[2] / rock_norm;
            }

            double azimuth_deg, elevation_deg, theta;
            azimuth_deg = atan2(rock[1], rock[0]) * 180.0f / PI;
            if (azimuth_deg < 0)
            {
                azimuth_deg += 360.0;
            }

            theta = acos(rock[2]) * 180.0f / PI;
            elevation_deg = 90.0 - theta;
            if (elevation_deg < 0)
            {
                elevation_deg = 0.0;
            }

            *rt_azimuth_deg = azimuth_deg;
            *rt_elevation_deg = elevation_deg;

            char flag_chuanwang = 0;
            double J0_WX_15s[3] = {0.0, 0.0, 0.0};

            //double CM_WX_15s[3] = {CM_WX[0], -CM_WX[2], CM_WX[1]};
            //double JBottom_WX_15s[3] = {JBottom_WX[0], -JBottom_WX[2], JBottom_WX[1]};
            double CM_WX[3] = {CM_WX_15s[0], CM_WX_15s[2], -CM_WX_15s[1]};
            double JBottom_WX[3] = {JBottom_WX_15s[0], JBottom_WX_15s[2], -JBottom_WX_15s[1]};


            if (JBottom_WX_15s[2] < 0)
            {
                flag_chuanwang = 1;

                double J0_WX[3];
                double diff[3] = {
                    JBottom_WX[0] - CM_WX[0],
                    JBottom_WX[1] - CM_WX[1],
                    JBottom_WX[2] - CM_WX[2]};

                double denominator = fabs(JBottom_WX[1]) + CM_WX[1];
                if (fabs(denominator) < 1e-9)
                {
                    J0_WX[0] = CM_WX[0];
                    J0_WX[1] = CM_WX[1];
                    J0_WX[2] = CM_WX[2];
                }
                else
                {
                    double scalar = CM_WX[1] / denominator;
                    J0_WX[0] = diff[0] * scalar + CM_WX[0];
                    J0_WX[1] = diff[1] * scalar + CM_WX[1];
                    J0_WX[2] = diff[2] * scalar + CM_WX[2];
                }

                J0_WX_15s[0] = J0_WX[0];
                J0_WX_15s[1] = -J0_WX[2];
                J0_WX_15s[2] = JBottom_WX[1];
            }else
            {
                flag_chuanwang = 0;
                J0_WX_15s[0] = JBottom_WX[0];
                J0_WX_15s[1] = JBottom_WX[1];
                J0_WX_15s[2] = JBottom_WX[2];
            }
            *rt_flag_chuanwang = flag_chuanwang;
}


double LidarFusionNode::deg2rad(double deg)
{
    return deg * PI / 180.0;
}

double LidarFusionNode::cosd(double deg)
{
    return cos(deg2rad(deg));
}

double LidarFusionNode::sind(double deg)
{
    return sin(deg2rad(deg));
}

void LidarFusionNode::mat33_mult(const double A[3][3], const double B[3][3], double C[3][3])
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            C[i][j] = 0.0;
            for (int k = 0; k < 3; k++)
            {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

void LidarFusionNode::mat33_transpose(const double A[3][3], double AT[3][3])
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            AT[i][j] = A[j][i];
        }
    }
}

void LidarFusionNode::mat33_vec3_mult(const double A[3][3], const double a[3], double b[3])
{
    for (int i = 0; i < 3; i++)
    {
        b[i] = 0.0;
        for (int k = 0; k < 3; k++)
        {
            b[i] += A[i][k] * a[k];
        }
    }
}

void LidarFusionNode::vec3_sub(const double a[3], const double b[3], double c[3])
{
    for (int i = 0; i < 3; i++)
    {
        c[i] = a[i] - b[i];
    }
}

void LidarFusionNode::vec3_add(const double a[3], const double b[3], double c[3])
{
    for (int i = 0; i < 3; i++)
    {
        c[i] = a[i] + b[i];
    }
}

void LidarFusionNode::calculate_CM_WX(const double COM_ZL[3], double SheXiang, const double att[3], const double Lever[3], double CM_WX[3])
{
    // 提取姿态角
    double fuyang = att[0];
    double henggun = att[1];
    double heading = att[2];

    // 定义变换矩阵T = [0 1 0; 0 0 1; 1 0 0]
    double T[3][3] = {
        {0.0, 1.0, 0.0},
        {0.0, 0.0, 1.0},
        {1.0, 0.0, 0.0}};

    double T0[3][3] = {0};
    mat33_transpose(T, T0);

    double temp_COM_ZL[3] = {0};
    double temp_Lever[3] = {0};
    mat33_vec3_mult(T0, COM_ZL, temp_COM_ZL);
    mat33_vec3_mult(T0, Lever, temp_Lever);

    // 1. 计算C_ZL2N矩阵 (箭系转网系)
    double C_ZL2N[3][3] = {0};
    C_ZL2N[0][0] = cosd(SheXiang);
    C_ZL2N[0][1] = sind(SheXiang);
    C_ZL2N[0][2] = 0.0;

    C_ZL2N[1][0] = -sind(SheXiang);
    C_ZL2N[1][1] = cosd(SheXiang);
    C_ZL2N[1][2] = 0.0;

    C_ZL2N[2][0] = 0.0;
    C_ZL2N[2][1] = 0.0;
    C_ZL2N[2][2] = 1.0;

    // 2. 计算各姿态旋转矩阵
    // 偏航角(heading)旋转矩阵 C3_heading
    double C3_heading[3][3] = {0};
    C3_heading[0][0] = cosd(heading);
    C3_heading[0][1] = sind(heading);
    C3_heading[0][2] = 0.0;

    C3_heading[1][0] = -sind(heading);
    C3_heading[1][1] = cosd(heading);
    C3_heading[1][2] = 0.0;

    C3_heading[2][0] = 0.0;
    C3_heading[2][1] = 0.0;
    C3_heading[2][2] = 1.0;

    // 俯仰角(fuyang)旋转矩阵 C1_fuyang (修正原MATLAB代码的笔误O)
    double C1_fuyang[3][3] = {0};
    C1_fuyang[0][0] = 1.0;
    C1_fuyang[0][1] = 0.0;
    C1_fuyang[0][2] = 0.0;

    C1_fuyang[1][0] = 0.0;
    C1_fuyang[1][1] = cosd(fuyang);
    C1_fuyang[1][2] = sind(fuyang);

    C1_fuyang[2][0] = 0.0;
    C1_fuyang[2][1] = -sind(fuyang);
    C1_fuyang[2][2] = cosd(fuyang);

    // 横滚角(henggun)旋转矩阵 C2_henggun (修正原MATLAB代码的fuyang笔误)
    double C2_henggun[3][3] = {0};
    C2_henggun[0][0] = cosd(henggun);
    C2_henggun[0][1] = 0.0;
    C2_henggun[0][2] = -sind(henggun);

    C2_henggun[1][0] = 0.0;
    C2_henggun[1][1] = 1.0;
    C2_henggun[1][2] = 0.0;

    C2_henggun[2][0] = sind(henggun); // 原MATLAB代码此处写错为fuyang，已修正
    C2_henggun[2][1] = 0.0;
    C2_henggun[2][2] = cosd(henggun);

    // 计算C_N2WX = C2_henggun * C1_fuyang * C3_heading
    double C1C3[3][3] = {0};
    mat33_mult(C1_fuyang, C3_heading, C1C3);

    double C_N2WX[3][3] = {0};
    mat33_mult(C2_henggun, C1C3, C_N2WX);

    // 计算C_ZL2WX = C_N2WX * C_ZL2N
    double C_ZL2WX[3][3] = {0};
    mat33_mult(C_N2WX, C_ZL2N, C_ZL2WX);

    // 计算C_WX02WX = C2_henggun * C1_fuyang
    double C_WX02WX[3][3] = {0};
    mat33_mult(C2_henggun, C1_fuyang, C_WX02WX);

    // 计算C_WX2WX0 = C_WX02WX的转置
    double C_WX2WX0[3][3] = {0};
    mat33_transpose(C_WX02WX, C_WX2WX0);

    // 计算C_WX2WX0 * Lever
    double temp_vec1[3] = {0};
    mat33_vec3_mult(C_WX2WX0, temp_Lever, temp_vec1);

    // 计算(C_WX2WX0*Lever - Lever)
    double temp_vec2[3] = {0};
    vec3_sub(temp_vec1, temp_Lever, temp_vec2);

    // 计算CCO_WX = C_WX02WX * temp_vec2
    double CCO_WX[3] = {0};
    mat33_vec3_mult(C_WX02WX, temp_vec2, CCO_WX);

    // 计算C_ZL2WX * COM_ZL
    double temp_vec3[3] = {0};
    mat33_vec3_mult(C_ZL2WX, temp_COM_ZL, temp_vec3);

    // 计算C_ZL2WX*COM_ZL + CCO_WX
    double temp_vec4[3] = {0};
    vec3_add(temp_vec3, CCO_WX, temp_vec4);

    // 最终计算CM_WX = T * temp_vec4
    mat33_vec3_mult(T, temp_vec4, CM_WX);
}

void LidarFusionNode::calculate_ZL_WX(const double COM_ZL_processed[3], const double COM_att[3], const double JBottom2JM_JT[3], double JBottom_ZL[3])
{
    // 提取姿态角
    double fuyang = COM_att[0];
    double henggun = COM_att[1];
    double heading = COM_att[2];

    double C_J3[3][3] = {0};
    C_J3[0][0] = cosd(fuyang);
    C_J3[0][1] = sind(fuyang);
    C_J3[0][2] = 0.0;

    C_J3[1][0] = -sind(fuyang);
    C_J3[1][1] = cosd(fuyang);
    C_J3[1][2] = 0.0;

    C_J3[2][0] = 0.0;
    C_J3[2][1] = 0.0;
    C_J3[2][2] = 1.0;

    double C_J2[3][3] = {0};
    C_J2[0][0] = cosd(henggun);
    C_J2[0][1] = 0.0;
    C_J2[0][2] = -sind(henggun);

    C_J2[1][0] = 0.0;
    C_J2[1][1] = 1.0;
    C_J2[1][2] = 0.0;

    C_J2[2][0] = sind(henggun);
    C_J2[2][1] = 0.0;
    C_J2[2][2] = cosd(henggun);

    double C_J1[3][3] = {0};
    C_J1[0][0] = 1.0;
    C_J1[0][1] = 0.0;
    C_J1[0][2] = 0.0;

    C_J1[1][0] = 0.0;
    C_J1[1][1] = cosd(heading);
    C_J1[1][2] = sind(heading);

    C_J1[2][0] = 0.0;
    C_J1[2][1] = -sind(heading);
    C_J1[2][2] = cosd(heading);

    double C_ZL2JT_temp[3][3] = {0};
    mat33_mult(C_J2, C_J3, C_ZL2JT_temp);

    double C_ZL2JT[3][3] = {0};
    mat33_mult(C_J1, C_ZL2JT_temp, C_ZL2JT);

    double C_ZL2JT0[3][3] = {0};
    mat33_transpose(C_ZL2JT, C_ZL2JT0);

    double JBottom2JM_ZL_temp[3] = {0};
    mat33_vec3_mult(C_ZL2JT0, JBottom2JM_JT, JBottom2JM_ZL_temp);

    vec3_sub(COM_ZL_processed, JBottom2JM_ZL_temp, JBottom_ZL);
}

// 姿态角转旋转矩阵 Cnb（载体->导航）
void LidarFusionNode::a2cnb(const double att[3], double Cnb[3][3])
{
    double si = sin(att[0]);
    double ci = cos(att[0]);
    double sj = sin(att[1]);
    double cj = cos(att[1]);
    double sk = sin(att[2]);
    double ck = cos(att[2]);

    Cnb[0][0] = cj * ck - si * sj * sk;
    Cnb[0][1] = -ci * sk;
    Cnb[0][2] = sj * ck + si * cj * sk;

    Cnb[1][0] = cj * sk + si * sj * ck;
    Cnb[1][1] = ci * ck;
    Cnb[1][2] = sj * sk - si * cj * ck;

    Cnb[2][0] = -ci * sj;
    Cnb[2][1] = si;
    Cnb[2][2] = ci * cj;
}

// 3x3矩阵转置
void LidarFusionNode::mat3_transpose(const double m[3][3], double res[3][3])
{
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            res[i][j] = m[j][i];
        }
    }
}

// 3x3矩阵乘法：a * b
void LidarFusionNode::mat3_mult(const double a[3][3], const double b[3][3], double res[3][3])
{
    double tmp[3][3] = {0};
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            tmp[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j];
        }
    }
    memcpy(res, tmp, sizeof(tmp));
}

// 3x3矩阵 * 3x1向量
void LidarFusionNode::mat3_vec_mult(const double m[3][3], const double v[3], double res[3])
{
    res[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2];
    res[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2];
    res[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2];
}

//======================== 函数1：挂钩L坐标 → 箭底N / 挂钩N ========================
// 输入：pos_hook[3], att_hook[3], att_net[3]
// 输出：pos_bottom_N[3], pos_hook_N[3]
void LidarFusionNode::poshookL2posbottomN(
    const double pos_hook[3],
    const double att_hook[3],
    const double att_net[3],
    double pos_bottom_N[3],
    double pos_hook_N[3])
{
    const double glv_deg = M_PI / 180.0;
    const double dir = 130.0 * glv_deg;
    const double yaw_0 = att_net[2];

    double lever_os[3] = {0.36, -(65.2 - 3.56), 0.0};
    double lever_hook[3] = {32.498, 0.0, 0.0};

    // 坐标系变换
    const double c_rfu_fur[3][3] = {{0,0,1}, {1,0,0}, {0,1,0}};
    double c_fur_rfu[3][3];
    mat3_transpose(c_rfu_fur, c_fur_rfu);

    double lever_os_t[3], lever_hook_t[3], pos_hook_t[3];
    mat3_vec_mult(c_rfu_fur, lever_os, lever_os_t);
    mat3_vec_mult(c_rfu_fur, lever_hook, lever_hook_t);
    mat3_vec_mult(c_rfu_fur, pos_hook, pos_hook_t);

    // 计算 cLR
    double att1[3] = {att_hook[0], 0, 0};
    double att2[3] = {0, 0, att_hook[1]};
    double att3[3] = {0, att_hook[2], 0};
    double m1[3][3], m2[3][3], m3[3][3], tmp[3][3], cLR[3][3];
    a2cnb(att1, m1); a2cnb(att2, m2); a2cnb(att3, m3);
    mat3_mult(m1, m2, tmp);
    mat3_mult(tmp, m3, cLR);

    // cGL / cLG
    double attGL[3] = {0, 0, -dir};
    double cGL[3][3], cLG[3][3];
    a2cnb(attGL, cGL);
    mat3_transpose(cGL, cLG);

    // cGN
    double cGN[3][3];
    a2cnb(att_net, cGN);

    // cNL
    double cGN_T[3][3], cNL[3][3];
    mat3_transpose(cGN, cGN_T);
    mat3_mult(cGN_T, cGL, cNL);

    // cGA / cAG
    double attGA[3] = {0, 0, yaw_0};
    double cGA[3][3], cAG[3][3];
    a2cnb(attGA, cGA);
    mat3_transpose(cGA, cAG);

    // cAN / cLA
    double cAN[3][3], cLA[3][3];
    mat3_mult(cAG, cGN, cAN);
    mat3_mult(cLG, cGA, cLA);

    // pos_bottom
    double cLR_lev[3], pos_bottom[3];
    mat3_vec_mult(cLR, lever_hook_t, cLR_lev);
    vec3_sub(pos_hook_t, cLR_lev, pos_bottom);

    // 公共项
    double cAN_lev[3], temp[3], cLA_temp[3];
    mat3_vec_mult(cAN, lever_os_t, cAN_lev);
    vec3_sub(cAN_lev, lever_os_t, temp);
    mat3_vec_mult(cLA, temp, cLA_temp);
    // 计算输出
    double vec1[3], vec2[3];
    vec3_add(pos_bottom, cLA_temp, vec1);
    vec3_add(pos_hook_t, cLA_temp, vec2);

    mat3_vec_mult(cNL, vec1, pos_bottom_N);
    mat3_vec_mult(cNL, vec2, pos_hook_N);

    // 最终坐标转换
    double tmp1[3], tmp2[3];
    mat3_vec_mult(c_fur_rfu, pos_bottom_N, pos_bottom_N);
    mat3_vec_mult(c_fur_rfu, pos_hook_N, pos_hook_N);
}

//======================== 函数2：箭底N坐标 → 挂钩L坐标 ========================
// 输入：pos_bottom_N[3], att_hook[3], att_net[3]
// 输出：pos_hook[3]
void LidarFusionNode::posbottomN2poshookL(
    const double pos_bottom_N[3],
    const double att_hook[3],
    const double att_net[3],
    double pos_hook[3])
{
    const double glv_deg = M_PI / 180.0;
    const double dir = 130.0 * glv_deg;
    const double yaw_0 = att_net[2];

    double lever_os[3] = {0.36, -(65.2 - 3.56), 0.0};
    double lever_hook[3] = {32.498, 0.0, 0.0};

    // 坐标系变换
    const double c_rfu_fur[3][3] = {{0,0,1}, {1,0,0}, {0,1,0}};
    double c_fur_rfu[3][3];
    mat3_transpose(c_rfu_fur, c_fur_rfu);

    double lever_os_t[3], lever_hook_t[3], pos_N_t[3];
    mat3_vec_mult(c_rfu_fur, lever_os, lever_os_t);
    mat3_vec_mult(c_rfu_fur, lever_hook, lever_hook_t);
    mat3_vec_mult(c_rfu_fur, pos_bottom_N, pos_N_t);

    double att1[3] = {att_hook[0], 0, 0};
    double att2[3] = {0, 0, att_hook[1]};
    double att3[3] = {0, att_hook[2], 0};

    double m1[3][3], m2[3][3], m3[3][3], tmp[3][3], cLR[3][3];
    a2cnb(att1, m1);
    a2cnb(att2, m2);
    a2cnb(att3, m3);
    mat3_mult(m1, m2, tmp);
    mat3_mult(tmp, m3, cLR);

    double attGL[3] = {0, 0, -dir};
    double cGL[3][3];
    a2cnb(attGL, cGL);

    double cGN[3][3];
    a2cnb(att_net, cGN);

    double cGN_T[3][3], cNL[3][3], cLN[3][3];
    mat3_transpose(cGN, cGN_T);
    mat3_mult(cGN_T, cGL, cNL);
    mat3_transpose(cNL, cLN);

    double attGA[3] = {0, 0, yaw_0};
    double cGA[3][3], cAG[3][3];
    a2cnb(attGA, cGA);
    mat3_transpose(cGA, cAG);

    double cAN[3][3], cNA[3][3];
    mat3_mult(cAG, cGN, cAN);
    mat3_transpose(cAN, cNA);

    double cNA_lev[3], tmp_sub[3], tmp_vec[3], pos_bottom[3];
    mat3_vec_mult(cNA, lever_os_t, cNA_lev);
    vec3_sub(lever_os_t, cNA_lev, tmp_sub);
    vec3_sub(pos_N_t, tmp_sub, tmp_vec);
    mat3_vec_mult(cLN, tmp_vec, pos_bottom);

    // pos_hook
    double cLR_lev[3], pos_hook_mid[3];
    mat3_vec_mult(cLR, lever_hook_t, cLR_lev);
    vec3_add(pos_bottom, cLR_lev, pos_hook_mid);

    // 最终坐标转换
    mat3_vec_mult(c_fur_rfu, pos_hook_mid, pos_hook);
}