|
|
@@ -0,0 +1,3715 @@
|
|
|
+#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);
|
|
|
+}
|
