/** * @brief 传感器外参标定库 * @author ZhengfengGu. any question please send mail to *[email protected] * @date 2025-5-22 * @version V1.0.0 * @copyright Copyright (c) 2003-无固定期限 杭州士腾科技有限公司 ********************************************************************* * @attention * 重要信息： * @par 修改日志: * <table> * <tr><th>Date <th>Version <th>Author <th>Description * <tr><td>2025/5/22 <td>1.0.0 <td>ZhengfengGu <td>创建初始版本 * </table> * ********************************************************************** */ #pragma once #include <common/sys_logger.h> #include <nav_msgs/msg/odometry.hpp> #include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/imu.hpp> #include <sensor_msgs/msg/laser_scan.hpp> #include <string> #include "calib_cfg/calib_cfg.h" #include "calib_common/time.h" #include "calibration_component/calibration_def.h" #include "geometry_msgs/msg/twist.hpp" #include "sensor/sensor_dispatch.h" // #include "mrp_interfaces/msg/motor_data.hpp" #include "mrp_interfaces/msg/motor_status_data.hpp" #include "mrp_interfaces/msg/motor_control_data.hpp" namespace mrp { namespace calib { // template <typename NodeT = rclcpp::Node::SharedPtr> class CalibrationComponent { public: /** * @brief 传感器外参标定库. * @param[in] i_lg [mrp::common::LoggerPtr] 日志库指针 * @param[in] i_init_param [InitExternalParam] * i_init_param.laser: 激光初始外参 * i_init_param.laser.x: 激光x轴偏移 * ... * i_init_param.imu: IMU初始外参 * i_init_param.imu.x: IMU x轴偏移 * ... * 初始外参(机械安装值),不标定的传感器初始外参可以不赋值 * @param[in] i_robot_param 机器人参数 * i_robot_param.model 0:差速, 1:阿克曼, 2:单舵轮 * i_robot_param.l:轴距, * i_robot_param.d:舵轮距离中心偏移, * i_robot_param.steer_offset:舵轮零偏(一般给0) */ CalibrationComponent( mrp::common::LoggerPtr i_lg, mrp_interfaces::calibration_component::InitExternalParam i_init_param, mrp_interfaces::calibration_component::RobotParam i_robot_param); ~CalibrationComponent(); /* * @brief * 单舵轮模型添加里程计数据,在标定雷达和里程计时，需要实时添加车辆控制信息， * 时间戳必须准确添加 * @param[in] header [std_msgs::msg::Header] * @param[in] msg [mrp_interfaces::calibration_component::SingleSteerControl] * 里程计数据 * @return void */ void AddSensorData( const std_msgs::msg::Header& header, const std::vector<mrp_interfaces::msg::MotorStatusData>& motor_datas); /* * @brief * 添加雷达数据 * @param[in] msg [sensor_msgs::msg::LaserScan::SharedPt] 雷达数据 * @return void */ void AddSensorData(const sensor_msgs::msg::LaserScan::SharedPtr msg); /* * @brief * 添加IMU数据 * @param[in] msg [sensor_msgs::msg::Imu::SharedPtr msg] IMU数据 * @return void */ void AddSensorData(const sensor_msgs::msg::Imu::SharedPtr msg); /** * @breif 复位状态，开始标定 * @param[in] i_calib_mode [CalibMode] * i_calib_mode.mode:标定模式,0:激光+里程计+IMU, * 1:激光+里程计,2:IMU * i_calib_mode.move_dist:移动距离 (单位m) * @return error_code [int] -1:模式错误 -2:机器人模型错误 * @retval int. */ int StartCalibration( mrp_interfaces::calibration_component::CalibMode i_calib_mode); /** * @brief 停止标定 */ void StopCalibration(); /* * @brief 获取单舵轮模型标定运动指令 * @param[out] cmd_vel * [mrp_interfaces::calibration_component::SingleSteerControl] 标定运动指令 * @retval void */ void GetCalibrationMoveCommand( std::vector<mrp_interfaces::msg::MotorControlData>& motor_datas); /** * @brief 获取标定状态和结果 * @param[out] calib_result [std::map<std::string, * mrp_interfaces::calibration_component::ExternalParam>] 标定结果   * first: 外设类型（可选值: "lidar", "odom", "imu", "camera"）     * second: 外参结构体(其中舵角零偏在second.yaw中) * @return calib_status [int] 0:未开始 3:进行中 5:完成 6:失败 * 状态为 NONE = 0, * WAITING, * INITIALIZING, * RUNNING, * PAUSED, * FINISHED, * FAILED * @retval int */ int GetCalibrationStatusAndResult( std::map<std::string, mrp_interfaces::calibration_component::ExternalParam>& calib_result); void DebugPrint(); private: void CalcuteOdom(const sensor::MotorData& msg); enum class ActionStatus { NONE = 0, WAITING, INITIALIZING, RUNNING, PAUSED, FINISHED, FAILED }; std::shared_ptr<CalibCfg> config_; mrp::common::LoggerPtr logger_; std::shared_ptr<sensor::SensorDispatch> sensor_dispatch_; std::deque<sensor::MotorData> odom_buffer_; struct { double x = 0.0; double y = 0.0; double yaw = 0.0; } current_odom_; bool is_forward_phase_ = true, is_left_phase_ = true; double origin_x = 0, origin_y = 0, origin_yaw = 0; bool move_initialized_ = false; std::deque<common::Time> control_times_; }; } // namespace calib } // namespace mrp各函数的作用分别是什么？