From a10dc3a5d07cc2592fefbaf4a5ccf14a3cdf622f Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Sat, 3 Oct 2026 11:37:18 +0000 Subject: [PATCH] Add dual yaw controller and update gimbal_base. --- rm_gimbal_controllers/CMakeLists.txt | 4 +- .../dual_yaw_controller.h | 38 ++ .../rm_gimbal_controllers/gimbal_base.h | 82 ++- .../src/dual_yaw_controller.cpp | 154 ++++++ rm_gimbal_controllers/src/gimbal_base.cpp | 493 ++++++++++++------ 5 files changed, 598 insertions(+), 173 deletions(-) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h create mode 100644 rm_gimbal_controllers/src/dual_yaw_controller.cpp diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98e8736b..9a3a0e1c 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -14,6 +14,7 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp + std_msgs rm_common controller_interface effort_controllers @@ -45,6 +46,7 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp + std_msgs rm_common effort_controllers tf2_eigen @@ -65,7 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h new file mode 100644 index 00000000..85dccb45 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * BSD 3-Clause License + ******************************************************************************/ + +#pragma once + +#include + +namespace rm_gimbal_controllers +{ +class DualYawController : public Controller +{ +public: + DualYawController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf, + int axis) const override; + bool shouldApplyJointLimit(int axis) const override; + std::string getBaseFrameID(const std::unordered_map& joint_urdfs) override; + void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, + const double pos_real[3], const double pos_des[3], const double pos_des_temp[3], + const double vel_des[3], const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]) override; + +private: + bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh); + bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point); + void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error); + + urdf::JointConstSharedPtr base_yaw_joint_; + std::unique_ptr base_yaw_ctrl_; + std::unique_ptr base_yaw_pid_pos_; + std::unique_ptr> base_yaw_pos_state_pub_; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 50c3eff2..42b5ea17 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,12 +44,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -57,22 +57,24 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel { public: - explicit ChassisVel(const ros::NodeHandle& nh) + ChassisVel(const ros::NodeHandle& nh) { double num_data; nh.param("num_data", num_data, 20.0); @@ -141,26 +143,46 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); + void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); + bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - void updateChassisVel(); - void updateBallisticSolution(const ros::Time& time); double gravityFeedForward(const ros::Time& time); + void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); + void publishShootBeforehand(const ros::Time& time, uint8_t cmd); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); - void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg); + void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); - std::string getGimbalFrameID(std::unordered_map joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); + bool TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res); + void publishTargetIsArmor(); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; @@ -169,34 +191,43 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs_; std::unordered_map pos_des_in_limit_; bool has_imu_ = true; - double ballistic_yaw_{}, ballistic_pitch_{}; std::shared_ptr bullet_solver_; - std::shared_ptr ballistic_solver_; + double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; - ros::Time last_ballistic_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; - std::shared_ptr> ballistic_solution_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; - ros::Subscriber ballistic_solver_request_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; + ros::ServiceServer TrackSolver_type_srv_; + ros::Publisher target_is_armor_pub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_, - odom2gimbal_traject_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; @@ -221,9 +252,12 @@ class Controller : public controller_interface::MultiInterfaceController +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + if (!initBaseYaw(robot_hw, controller_nh)) + return false; + return Controller::init(robot_hw, root_nh, controller_nh); +} + +bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) +{ + ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw"); + ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos"); + + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint"))); + if (!base_yaw_joint_) + { + ROS_ERROR("Could not find base_yaw joint in urdf"); + return false; + } + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); + base_yaw_ctrl_ = std::make_unique(); + base_yaw_pid_pos_ = std::make_unique(); + base_yaw_pos_state_pub_ = + std::make_unique>(base_yaw_nh, "pos_state", 1); + return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh); +} + +bool DualYawController::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& joint_urdf, int axis) const +{ + return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name); +} + +bool DualYawController::shouldApplyJointLimit(int axis) const +{ + return axis != 2; +} + +std::string DualYawController::getBaseFrameID(const std::unordered_map& joint_urdfs) +{ + if (base_yaw_joint_) + return base_yaw_joint_->parent_link_name.c_str(); + return Controller::getBaseFrameID(joint_urdfs); +} + +void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double /*pos_des_temp*/[3], + const double vel_des[3], const double angle_error[3], + const double traject_pos_des[3], const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); + } + + if (!base_yaw_ctrl_ || !base_yaw_pid_pos_) + return; + + double base_yaw_pos_real = 0.; + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + double roll{}, pitch{}; + quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + double armor_set_point{}; + double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; + double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); + base_yaw_pid_pos_->computeCommand(base_yaw_error, period); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z()); + base_yaw_ctrl_->update(time, period); + publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); +} + +bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point) +{ + if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero()) + return false; + + geometry_msgs::Point target_pos = data_track_.position; + if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z)) + return false; + + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + geometry_msgs::Point base_yaw_pos_odom; + base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x; + base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y; + base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z; + + armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x); + if (!std::isfinite(armor_set_point)) + return false; + + return true; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } +} + +void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error) +{ + if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock()) + { + base_yaw_pos_state_pub_->msg_.header.stamp = time; + base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2]; + base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real; + base_yaw_pos_state_pub_->msg_.error = base_yaw_error; + base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd(); + base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum(); + base_yaw_pos_state_pub_->unlockAndPublish(); + } +} + +} // namespace rm_gimbal_controllers + +PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e7c67730..0c5d5b75 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -48,27 +49,6 @@ namespace rm_gimbal_controllers { bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); - chassis_vel_ = std::make_shared(chassis_vel_nh); - ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); - ballistic_solver_ = std::make_shared(nh_ballistic_solver); - - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - XmlRpc::XmlRpcValue xml_rpc_value; const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) @@ -82,7 +62,57 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - auto* effort_joint_interface = robot_hw->get(); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); + bullet_solver_ = std::make_shared(nh_bullet_solver); + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); + + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "moment_of_inertia", 0.), + }; + + config_rt_buffer_.initRT(config_); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + std::string target_is_armor_topic; + controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); + controller_nh.param("initial_target_is_armor", target_is_armor_, true); + target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); + publishTargetIsArmor(); + TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { // Get URDF info about joint @@ -103,12 +133,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro return false; } const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.emplace(axis, std::move(joint_urdf)); - ctrls_.emplace(axis, std::make_unique()); - pid_pos_.emplace(axis, std::make_unique()); - pos_des_in_limit_.emplace(axis, true); - pos_state_pub_.emplace( - axis, std::make_unique>(nh, "pos_state", 1)); + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + ctrls_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } @@ -119,8 +152,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); - auto* imu_sensor_interface = robot_hw->get(); + hardware_interface::ImuSensorInterface* imu_sensor_interface = + robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { @@ -137,18 +172,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.header.frame_id = "odom"; odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; + odom2gimbal_traject_des_.header.frame_id = "odom"; odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); - ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, - &Controller::ballisticSolverRequestCB, this); + data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - ballistic_solution_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); return true; } @@ -164,19 +207,18 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); - base2gimbal_ = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, odom2gimbal_.child_frame_id, time); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(5, "%s\n", ex.what()); return; } - updateBallisticSolution(time); updateChassisVel(); if (state_ != cmd_gimbal_.mode) { @@ -189,7 +231,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + externalAimTrack(time); + else + track(time, period); break; case DIRECT: direct(time); @@ -201,24 +246,40 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { - tf2::Quaternion odom2base, odom2gimbal_des, base2gimbal_des, base2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des); + tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q); double current_rpy[3], des_rpy[3], traject_rpy[3]; - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), current_rpy[0], current_rpy[1], current_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), des_rpy[0], des_rpy[1], des_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, traject_yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + for (const auto& it : joint_urdfs_) { - bool update = (it.first != 1 && it.first != 2) || (it.first == 1 && update_pitch) || (it.first == 2 && update_yaw); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } + base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); @@ -229,6 +290,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -254,8 +316,16 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -273,7 +343,7 @@ void Controller::track(const ros::Time& time) { if (!data_track_.header.frame_id.empty()) { - auto transform = + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); @@ -296,8 +366,8 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -324,6 +394,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -335,11 +457,9 @@ void Controller::direct(const ros::Time& time) try { if (!cmd_gimbal_.target_pos.header.frame_id.empty()) - { - auto transform = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, - cmd_gimbal_.target_pos.header.stamp); - tf2::doTransform(aim_point_odom, aim_point_odom, transform); - } + tf2::doTransform(aim_point_odom, aim_point_odom, + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, + cmd_gimbal_.target_pos.header.stamp)); } catch (tf2::TransformException& ex) { @@ -419,9 +539,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - auto transform = - robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), time); - tf2::doTransform(gyro, angular_vel, transform); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -438,6 +558,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -453,79 +574,83 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - auto transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + else { - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + target_pos = data_track_.position; + target_vel = data_track_.velocity; + } + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - pid_pos_.at(2)->computeCommand(bullet_solver_->getUsingtraject() ? traject_angle_error[2] : angle_error[2], period); - double cmd = pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z; - if (state_ == TRACK && bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->setCommand(cmd); - ctrls_.at(2)->update(time, period); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -561,9 +686,9 @@ double Controller::gravityFeedForward(const ros::Time& time) if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, - joint_urdfs_.at(1)->parent_link_name, time); - tf2::doTransform(gravity_compensation, gravity_compensation, transform); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -591,42 +716,97 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -void Controller::updateBallisticSolution(const ros::Time& time) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) +{ + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) { - if (publish_rate_ > 0.0 && last_ballistic_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) { - if (ballistic_solution_pub_->trylock()) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - std_msgs::Float32MultiArray data; - data.data.emplace_back(ballistic_yaw_); - data.data.emplace_back(ballistic_pitch_); - ballistic_solution_pub_->msg_.data = data.data; - ballistic_solution_pub_->unlockAndPublish(); + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); } - last_ballistic_publish_time_ = time; + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); } } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->child_link_name; + return joint_urdfs.at(1)->child_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->child_link_name; + return joint_urdfs.at(0)->child_link_name.c_str(); if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->child_link_name; - return {}; + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); } -std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->parent_link_name; + return joint_urdfs.at(2)->parent_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->parent_link_name; + return joint_urdfs.at(0)->parent_link_name.c_str(); if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->parent_link_name; - return {}; + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); } double Controller::updateCompensation(double chassis_vel_angular_z) @@ -649,12 +829,27 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) +{ + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); +} + +bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) +{ + const bool previous = target_is_armor_; + target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; + publishTargetIsArmor(); + if (previous != target_is_armor_) + ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); + res.switch_is_success = true; + return true; +} + +void Controller::publishTargetIsArmor() { - ballistic_track_rt_buffer_.writeFromNonRT(*msg); - bool ballistic_solver_request = msg->data; - if (ballistic_solver_request) - ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); + std_msgs::Bool msg; + msg.data = target_is_armor_; + target_is_armor_pub_.publish(msg); } void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) @@ -665,12 +860,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; - config.accel_pitch_ = init_config.accel_pitch_; - config.accel_yaw_ = init_config.accel_yaw_; config.chassis_comp_a_ = init_config.chassis_comp_a_; config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; config.chassis_comp_d_ = init_config.chassis_comp_d_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -680,7 +876,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, - .chassis_comp_d_ = config.chassis_comp_d_ }; + .chassis_comp_d_ = config.chassis_comp_d_, + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); }