AI辅助解决弹道复现BUG #pragma once #include // ROS相关头文件 #include “ros/ros.h” #include “rm_msgs/Armor.h” #include “rm_msgs/ArmorArray.h” #include “rm_msgs/RmSerial.h” #include <std_msgs/Float64.h> #include <angles/angles.h> #include <tf2/LinearMath/Transform.h> #include <tf2/LinearMath/Vector3.h> #include <tf2/LinearMath/Quaternion.h> // OpenCV相关头文件 #include <opencv2/opencv.hpp> #include <cv_bridge/cv_bridge.h> // 标准库头文件 #include #include // 时间相关头文件 #include #include #include #include <yaml-cpp/yaml.h> // 自定义头文件 #include “Calculater.hpp” #include “GimbalPos.hpp” #include “TargetModel.hpp” #include “visual.hpp” #include “CoorConverter.hpp” #include “math.hpp” #include “MPC.hpp” #include “trajectory_visualizer.hpp” using namespace std; using namespace cv; /* 自瞄文档 标准模型状态向量 X(0) -> 机器人中心的x坐标 X(1) -> 机器人中心x方向速度 X(2) -> 机器人中心的y坐标 X(3) -> 机器人中心y方向速度 X(4) -> 左侧装甲板的固定高度 X(5) -> 右侧装甲板的固定高度 X(6) -> 小装甲板旋转半径（左侧） X(7) -> 大装甲板旋转半径（右侧） X(8) -> 机器人整体偏航角yaw X(9) -> 偏航角速度palstance 前哨站状态向量 X(0): 机器人中心的x坐标 X(1): 机器人中心的y坐标 X(2): 第一块装甲板高度h1 X(3): 第二块装甲板高度h2 X(4): 第三块装甲板高度h3 X(5): 偏航角yaw X(6): 偏航角速度palstance / /* @brief 将弧度约束在[-pi, pi]范围内，大于n，减去2n；小于-n，加上2n @param angle 角度 / #ifndef _std_radian #define std_radian(angle) ((angle) + round((0 - (angle)) / (2 * PI)) * (2 * PI)) #endif //追踪类 template class Tracker{ private: double COMMAND_TIMESPAN; //电控延迟 double local_gravity; //重力加速度 double eTime; //曝光时间 / ======================== 系统参数 ======================== / //ROS相关 ros::NodeHandle nh; //ROS节点句柄 ros::Publisher debugpub; //debug发布者 ros::Publisher debugpub1; //debug1发布者 std_msgs::Float64 debugdate; //debug数据 std_msgs::Float64 debugdate1; //debug1数据 rm_msgs::RmSerial RmSerialData; //接收串口数据 tf2_ros::Buffer tfBuffer_; // TF 缓冲区 tf2_ros::TransformListener tfListener; // 声明一个tf2_ros::TransformListener对象，并传入tfBuffer rm_msgs::ArmorArrayConstPtr m_armors; // 接收装甲板数据 //图像处理相关 cv::Mat frame; //接收的原始图像 cv::Mat frame_; //被处理的图像副本(保护原图像) cv::Mat camera_matrix_; //相机内参(构造函数中) cv::Mat dist_coeffs_; //畸变系数(构造函数中) Image img; //图像工具 //坐标变换工具 CAL::Calculater cal; //计算工具 CoordinateTransformer* coorConverter; //装甲板评分参数 std::vector col; // 数列中的每个数代表矩阵的每一列 std::vector row; // 数列中的每个数代表矩阵的每一行 std::vector tmp_v; // 存储C(n,k)的中间结果 std::vector<std::vector> result; // 存储C(n,k)的结果 std::vector<std::vector> nAfour; // 存储A(n,4)的结果 std::vector<std::vector> fourAfour; // 存储A(4,4)的结果 std::map<int, int> row_col; // 存储最终结果，row_col[i]=j表示矩阵的第i行第j列是要选取的数 //数据关联参数 double min = 0, tmp = 0; /* ======================== 跟踪控制参数 ======================== / //基本跟踪参数 char TrackingID; //跟踪中的装甲板ID bool Switch_Armor; //装甲板切换标识符 double trackTime; //目标丢失判定时间(秒) //角度补偿参数 double pitch_compensation; //pitch补偿 double yaw_compensation; //yaw补偿 //火力控制参数 bool all_fire = false; //完全火力模式(不停止射击) / ======================== 装甲板预测参数 ======================== / //空间参考点参数 Eigen::Vector3d pegPos; //空间标准点(用于装甲板跳变判断) double peg_point_pixel_now; //当前装甲板在像素坐标系的位置 double peg_point_pixel_last; //位于像素标准坐标系的上一个装甲板位置 //子弹参数 double BulletVector = 22; //子弹速度(初值给24m/s) / ======================== 时间戳管理 ======================== / //装甲板跟踪时间 long long Now_Time_armor = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count();//当前时间戳(用于计算丢失时间) long long Track_Time_armor = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count();//丢失时间戳(用于计算丢失时间) //帧率计算 long long begin_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count();//首时间 long long end_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count();//尾时间 // C++工具时间戳 long long tool_begin_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count();//首时间 long long tool_end_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count();//尾时间 // ros工具时间戳 ros::Time tool_begin_Time_ros = ros::Time::now(); ros::Time tool_end_Time_ros = ros::Time::now(); // 获取装甲板tf时间(判断是否是相同帧) ros::Time last_tf_time = ros::Time::now(); / ======================== 标志位 ======================== / bool functional = true; // 射击模式 int m_center_tracked; // 锁中心状态标志位 bool m_track_center; // 是否跟随中心 bool m_fix_on; // 重力补偿开关 std::string config_path_; // 存储配置路径 unique_ptr targetModel; // 目标模型 ros::Publisher AngPub; // 角度话题发布者 std::shared_ptr m_MPC; // 模型预测控制器 // =========================== 阈值 ==================================== double m_score_tolerance; //装甲板匹配得分最大值 double m_switch_threshold; // 更新装甲板切换的角度阈值，角度制 double m_force_aim_palstance_threshold; // 强制允许发射的目标旋转速度最大值，弧度制 double m_aim_angle_tolerance; // 自动击发时目标装甲板相对偏角最大值，角度制 double m_aim_pose_tolerance; // 自动击发位姿偏差最大值，弧度制 double m_aim_center_angle_tolerance; // 跟随圆心自动击发目标偏角判断，角度制 double m_switch_trackmode_threshold; // 更换锁中心模式角速度阈值，弧度制 double m_aim_center_palstance_threshold;// 跟随圆心转跟随装甲板的目标旋转速度最大值，弧度制 // =========================== 可视化 =================================== std::vector<std::vectorEigen::Vector3d> visual_armor_position_pose_temp; // 用于可视化存储观测装甲板的全局变量 GimbalPose m_cur_pose; // 当前位姿 GimbalPose m_target_pose; // 目标位姿 GimbalPose m_target_pose_debug; // 用于debug,比较mpc和传统模式 // =========================== 串口补偿 ======================================= double m_rollOffset = 0; double m_pitchOffset = 0; double m_yawOffset = 0; public: Tracker(ros::NodeHandle& nh,const std::string& config_path) : config_path_(config_path), tfListener(tfBuffer_) { this->nh = nh; AngPub = nh.advertise<geometry_msgs::Vector3>("/auto_angle", 1000); debugpub = nh.advertise<std_msgs::Float64>("/debugpub", 1000); debugpub1 = nh.advertise<std_msgs::Float64>("/debugpub1", 1000); cal = new CAL::Calculater(this->nh,this->img); tfBuffer_.setUsingDedicatedThread(true); m_armors.reset(); // 显式初始化为空 // 初始化时创建 TargetModel targetModel = std::make_unique(config_path_); coorConverter = new CoordinateTransformer(config_path_); m_MPC = std::make_shared(config_path_); // 控制器初始化 // 检查配置文件路径是否有效 if (!config_path_.empty()) { try { setParam(config_path_); ROS_INFO(“Successfully loaded parameters from: %s”, config_path_.c_str()); } catch (const std::exception& e) { ROS_ERROR(“Failed to load parameters: %s”, e.what()); } } else { ROS_WARN(“No configuration file path provided. Using default parameters.”); } } ~Tracker() { delete cal; delete coorConverter; } /*************** * @brief 回调函数: 接受串口信息，并更新内部变量、TF、坐标系 * @param serial ROS 消息的智能指针，包含子弹速度、射击标志、补偿角度等 / void SetSerial(const rm_msgs::RmSerialConstPtr _serial){ if (_serial) { RmSerialData = _serial; // 目前注释掉了(到时候测试一下) // if(_serial->BulletVec > 10){ // BulletVector = _serial->BulletVec; // } if(_serial->ShootFlag == ‘f’){ functional = true; }else if(_serial->ShootFlag == ‘a’){ functional = true; }else{ functional = true; } // 从串口获取当前装甲板的姿态 m_cur_pose.roll = RmSerialData.Roll; m_cur_pose.pitch = RmSerialData.Pitch; m_cur_pose.yaw = RmSerialData.Yaw; // debug // ROS_INFO(“cur: pitch: %lf” , RmSerialData.Pitch); // ROS_INFO(“BulletVector: %lf” , BulletVector); // std::vectorEigen::Vector3d imuabsPos = cal->GetPos(“map”,“imu”,1); // cal->TFUpdata(“map”,“imuabs”,{0.0, 0.0, 0.0},{0.0, 0.0, imuabsPos[1].z()},0); // ROS_WARN(“have serial 3333333333333333333333333333”); } } /*************** * @brief 回调函数: 接收相机节点发送的图像,用于debug * @param img 图像信息 * @param frame 供算法线程直接使用的原始图 * @param frame_ 额外再 clone 一份，用于可视化 / void doimage(const sensor_msgs::ImageConstPtr img_) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(img_, sensor_msgs::image_encodings::BGR8); } catch(cv_bridge::Exception& e) { ROS_ERROR(“cv_bridge exception: %s”, e.what()); return; } frame = cv_ptr->image.clone(); frame_ = frame.clone(); // ROS_WARN(“have image 111111111111111111”); } // 回调函数: 接收识别发送的装甲板序列 void doArmors(rm_msgs::ArmorArrayConstPtr armors) { m_armors = armors; // ROS_WARN(“have armor 222222222222222222”); } /* * @brief 主跟踪循环：每帧调用一次，完成“决策 → 预测 → 补偿 → 发布”全链路 / void Track() { if (!m_armors) { cout « “消息队列为空” « endl; return; } try { //begin_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count(); if(!functional){ return; } TargetModel targetModel_temp = nullptr; targetModel_temp = armorUpdate(m_armors); if(targetModel_temp != nullptr){ cv::Point2d finAngle = {0.0, 0.0}; GimbalPose target_pose_temp = reconstruction_choose_compensation(); // 调试输出(打印云台需要转动到的角度) // cout « “pitch: " « target_pose_temp.pitch « " " « “yaw: " « target_pose_temp.yaw « endl; // 打印需要移动的相对角 // finAngle.x = target_pose_temp.pitch - m_cur_pose.pitch; // 计算需要移动的pitch角度 // finAngle.y = target_pose_temp.yaw - m_cur_pose.yaw; // 计算需要移动的yaw角度 // // // 陀螺仪的绝对角 finAngle.x = target_pose_temp.pitch; // 计算需要移动到的pitch角度 finAngle.y = target_pose_temp.yaw; // 计算需要移动到的yaw角度 // debug: 云台跟随效果rqt_plot打印 // yaw角 // debugdate.data = m_cur_pose.yaw; // 当前云台yaw角度 // debugdate1.data = target_pose_temp.yaw; // 计算出云台需要转动的yaw角度 // // pitch角 // debugdate.data = m_cur_pose.pitch; // 当前云台pitch角度 // debugdate1.data = target_pose_temp.pitch; // 计算出云台需要转动的pitch角度 // debug: 自动打弹打印 // cout « “finAngle.x: " « finAngle.x « " " « “finAngle.y: " « finAngle.y « endl; // cout « “是否允许打弹: " « targetModel_temp->auto_fire « endl; // if (targetModel_temp->auto_fire) { // debugdate.data = 1; // } // else { // debugdate.data = 0; // } Pub_Aangle(true, targetModel_temp->auto_fire, finAngle); }else{ Pub_Aangle(false); } // 帧率控制 // double time_line = 30.0; // end_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count(); // double frame_time = (end_Time - begin_Time)/1000.0; // if ((end_Time - begin_Time) - time_line < 0.0)// 帧率控制 // { // int sleep_time_ms = static_cast(time_line - (end_Time - begin_Time)); // std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time_ms)); // //frame_time = 0.03; // end_Time = chrono::time_point_castchrono::milliseconds(chrono::system_clock::now()).time_since_epoch().count(); // frame_time = (end_Time - begin_Time)/1000.0; // // cout « “sleep_time_ms:” « sleep_time_ms « “ms” « endl; // // cout « “frame_time:” « frame_time 1000 « “ms” « endl; // } // begin_Time = end_Time; // debugdate1.data = frame_time 1000; debugpub.publish(debugdate); debugpub1.publish(debugdate1); } catch (const std::exception& e) { ROS_ERROR("[Exception] In Track function: %s”, e.what()); } } // 重构选板 GimbalPose reconstruction_choose_compensation() { // 目标装甲板 Armor abs_facing_armor; Armor abs_target_armor; double hit_time = 0; double center_hit_time = 0; double m_time_off = COMMAND_TIMESPAN + eTime + 0.025; /** * 严格意义上来说，如果要准确预测击中时刻的装甲板位置的话，需要解一个非线性方程。此处采用一种近似的解法 * 根据当前最近装甲板距离计算击中时间，用于预测目标装甲板出现的位置 * 事实上相当于一步牛顿迭代法，或者说一阶的线性化
...