diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98732896..995f5af7 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(catkin REQUIRED generate_dynamic_reconfigure_options( cfg/BulletSolver.cfg + cfg/GimbalBase.cfg ) ################################### diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 8f2b4f4a..44b1b1e1 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,5 +14,13 @@ gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) +gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) +gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) +gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) +gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) +gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) +gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0) +gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) +gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg new file mode 100755 index 00000000..debe59ca --- /dev/null +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python +PACKAGE = "rm_gimbal_controllers" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) +gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) +gen.add("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0) +gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0) +gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) + +exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 5ed6d081..33ab54f9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,13 +46,19 @@ #include #include #include +#include +#include +#include +#include namespace rm_gimbal_controllers { struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, dt, timeout; + resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle, + min_switch_angle, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; + int min_fit_switch_count; }; class BulletSolver @@ -61,7 +67,7 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num); + double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); double getResistanceCoefficient(double bullet_speed) const; @@ -76,25 +82,35 @@ class BulletSolver void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); + void judgeShootBeforehand(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); + void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; + std::shared_ptr> fly_time_pub_; + ros::Subscriber identified_target_change_sub_; + ros::Time switch_armor_time_{}; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; double max_track_target_vel_; - bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; + double fly_time_; + int shoot_beforehand_cmd_{}; int selected_armor_; + int count_; bool track_target_; + bool identified_target_change_ = true; + bool is_in_delay_before_switch_{}; + bool dynamic_reconfig_initialized_{}; geometry_msgs::Point target_pos_{}; - double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; }; 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 1c919927..0a166875 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -37,23 +37,33 @@ #pragma once -#include +#include #include #include -#include #include -#include +#include +#include #include #include #include -#include +#include +#include #include #include #include -#include +#include +#include +#include +#include namespace rm_gimbal_controllers { +struct GimbalConfig +{ + double yaw_k_v_, pitch_k_v_, k_chassis_vel_; + double accel_pitch_{}, accel_yaw_{}; +}; + class ChassisVel { public: @@ -139,27 +149,33 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; // ROS Interface ros::Time last_publish_time_{}; + std::unique_ptr> yaw_pos_state_pub_, pitch_pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; + urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; std::string gimbal_des_frame_id_{}, imu_name_{}; double publish_rate_{}; bool state_changed_{}; + bool pitch_des_in_limit_{}, yaw_des_in_limit_{}; + int loop_count_{}; // Transform geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_; @@ -169,18 +185,16 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; + bool dynamic_reconfig_initialized_{}; + GimbalConfig config_{}; + realtime_tools::RealtimeBuffer config_rt_buffer_; + dynamic_reconfigure::Server* d_srv_{}; + + RampFilter*ramp_rate_pitch_{}, *ramp_rate_yaw_{}; + enum { RATE, diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 94188c8a..15cda288 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,7 +52,15 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), - .timeout = getParam(controller_nh, "timeout", 0.) }; + .timeout = getParam(controller_nh, "timeout", 0.), + .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), + .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), + .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), + .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0), + .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5), + .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), + .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), + .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -80,6 +88,12 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); + fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); + + identified_target_change_sub_ = + controller_nh.subscribe("/change", 10, &BulletSolver::identifiedTargetChangeCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -100,7 +114,7 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num) + double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -115,18 +129,48 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; + double max_switch_angle = config_.max_switch_angle / 180 * M_PI; + double min_switch_angle = config_.min_switch_angle / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = track_target_ ? - acos(r / target_rho) - M_PI / 12 + - (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : - M_PI / 12; - if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) + if (std::abs(chassis_angular_vel_z) >= config_.max_chassis_angular_vel) + track_target_ = 0; + double switch_armor_angle = + track_target_ ? + (acos(r / target_rho) - max_switch_angle + + (-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) * + (1 - std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel) + + min_switch_angle * std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel : + min_switch_angle; + if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && + std::abs(v_yaw) >= 1.0) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = armors_num == 4 ? pos.z + dz : pos.z; + count_++; + if (identified_target_change_) + { + count_ = 0; + identified_target_change_ = false; + } + if (count_ >= config_.min_fit_switch_count) + { + if (count_ == config_.min_fit_switch_count) + switch_armor_time_ = ros::Time::now(); + selected_armor_ = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = armors_num == 4 ? pos.z + dz : pos.z; + } } + is_in_delay_before_switch_ = + (((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) > + output_yaw_ + switch_armor_angle) && + v_yaw > 0.) || + ((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) < + output_yaw_ - switch_armor_angle) && + v_yaw < 0.)) && + track_target_; + yaw += v_yaw * config_.track_rotate_target_delay; + pos.x += vel.x * config_.track_move_target_delay; + pos.y += vel.y * config_.track_move_target_delay; int count{}; double error = 999; if (track_target_) @@ -180,6 +224,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (count >= 20 || std::isnan(error)) return false; } + if (fly_time_pub_->trylock()) + { + fly_time_pub_->msg_.data = fly_time_; + fly_time_pub_->unlockAndPublish(); + } return true; } @@ -301,6 +350,32 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec return error; } +void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) +{ + if (msg->data) + identified_target_change_ = true; +} + +void BulletSolver::judgeShootBeforehand(const ros::Time& time) +{ + if (!track_target_) + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec()) + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec()) + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + else if (is_in_delay_before_switch_) + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + else + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + if (shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/) { ROS_INFO("[Bullet Solver] Dynamic params change"); @@ -316,6 +391,14 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.delay = init_config.delay; config.dt = init_config.dt; config.timeout = init_config.timeout; + config.ban_shoot_duration = init_config.ban_shoot_duration; + config.gimbal_switch_duration = init_config.gimbal_switch_duration; + config.max_switch_angle = init_config.max_switch_angle; + config.min_switch_angle = init_config.min_switch_angle; + config.max_chassis_angular_vel = init_config.max_chassis_angular_vel; + config.track_rotate_target_delay = init_config.track_rotate_target_delay; + config.track_move_target_delay = init_config.track_move_target_delay; + config.min_fit_switch_count = init_config.min_fit_switch_count; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -326,7 +409,15 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .g = config.g, .delay = config.delay, .dt = config.dt, - .timeout = config.timeout }; + .timeout = config.timeout, + .ban_shoot_duration = config.ban_shoot_duration, + .gimbal_switch_duration = config.gimbal_switch_duration, + .max_switch_angle = config.max_switch_angle, + .min_switch_angle = config.min_switch_angle, + .max_chassis_angular_vel = config.max_chassis_angular_vel, + .track_rotate_target_delay = config.track_rotate_target_delay, + .track_move_target_delay = config.track_move_target_delay, + .min_fit_switch_count = config.min_fit_switch_count }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 7208e7a9..770a0dff 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -62,12 +62,6 @@ 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"]; - ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation"); - yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.); - velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.); - effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.); - - k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); 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"); @@ -75,12 +69,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); - yaw_k_v_ = getParam(nh_yaw, "k_v", 0.); - pitch_k_v_ = getParam(nh_pitch, "k_v", 0.); + ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "yaw/pid_pos"); + ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pitch/pid_pos"); + + config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), + .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), + .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), + .accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.) }; + 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); + hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); - if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) + if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch) || + !pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) return false; + robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) has_imu_ = false; @@ -96,21 +104,46 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - gimbal_des_frame_id_ = ctrl_pitch_.joint_urdf_->child_link_name + "_des"; + // Get URDF info about joint + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + pitch_joint_urdf_ = urdf.getJoint(ctrl_pitch_.getJointName()); + yaw_joint_urdf_ = urdf.getJoint(ctrl_yaw_.getJointName()); + if (!pitch_joint_urdf_) + { + ROS_ERROR("Could not find joint pitch in urdf"); + return false; + } + if (!yaw_joint_urdf_) + { + ROS_ERROR("Could not find joint yaw in urdf"); + return false; + } + + gimbal_des_frame_id_ = pitch_joint_urdf_->child_link_name + "_des"; odom2gimbal_des_.header.frame_id = "odom"; odom2gimbal_des_.child_frame_id = gimbal_des_frame_id_; odom2gimbal_des_.transform.rotation.w = 1.; odom2pitch_.header.frame_id = "odom"; - odom2pitch_.child_frame_id = ctrl_pitch_.joint_urdf_->child_link_name; + odom2pitch_.child_frame_id = pitch_joint_urdf_->child_link_name; odom2pitch_.transform.rotation.w = 1.; odom2base_.header.frame_id = "odom"; - odom2base_.child_frame_id = ctrl_yaw_.joint_urdf_->parent_link_name; + odom2base_.child_frame_id = yaw_joint_urdf_->parent_link_name; odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); + yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); + pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); + + ramp_rate_pitch_ = new RampFilter(0, 0.001); + ramp_rate_yaw_ = new RampFilter(0, 0.001); return true; } @@ -125,10 +158,17 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + config_ = *config_rt_buffer_.readFromRT(); + ramp_rate_pitch_->setAcc(config_.accel_pitch_); + ramp_rate_yaw_->setAcc(config_.accel_yaw_); + ramp_rate_pitch_->input(cmd_gimbal_.rate_pitch); + ramp_rate_yaw_->input(cmd_gimbal_.rate_yaw); + cmd_gimbal_.rate_pitch = ramp_rate_pitch_->output(); + cmd_gimbal_.rate_yaw = ramp_rate_yaw_->output(); try { - odom2pitch_ = robot_state_handle_.lookupTransform("odom", ctrl_pitch_.joint_urdf_->child_link_name, time); - odom2base_ = robot_state_handle_.lookupTransform("odom", ctrl_yaw_.joint_urdf_->parent_link_name, time); + odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); + odom2base_ = robot_state_handle_.lookupTransform("odom", yaw_joint_urdf_->parent_link_name, time); } catch (tf2::TransformException& ex) { @@ -167,13 +207,14 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); double pitch_real_des, yaw_real_des; - if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, ctrl_pitch_.joint_urdf_)) + pitch_des_in_limit_ = setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_); + if (!pitch_des_in_limit_) { double yaw_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->upper : 1e16; - lower_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->lower : -1e16; + upper_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->upper : 1e16; + lower_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, lower_limit)) ? @@ -183,13 +224,14 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_temp); } - if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, ctrl_yaw_.joint_urdf_)) + yaw_des_in_limit_ = setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, yaw_joint_urdf_); + if (!yaw_des_in_limit_) { double pitch_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->upper : 1e16; - lower_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->lower : -1e16; + upper_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->upper : 1e16; + lower_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, base2gimbal_current_des_pitch, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, lower_limit)) ? @@ -233,7 +275,9 @@ void Controller::track(const ros::Time& time) double yaw_compute = yaw_real; double pitch_compute = -pitch_real; geometry_msgs::Point target_pos = data_track_.position; - geometry_msgs::Vector3 target_vel = data_track_.velocity; + geometry_msgs::Vector3 target_vel{}; + if (data_track_.id != 12) + target_vel = data_track_.velocity; try { if (!data_track_.header.frame_id.empty()) @@ -248,15 +292,17 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - target_pos.x -= odom2pitch_.transform.translation.x; - target_pos.y -= odom2pitch_.transform.translation.y; - target_pos.z -= odom2pitch_.transform.translation.z; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; + target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); - bool solve_success = - bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, data_track_.v_yaw, - data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); + 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, chassis_vel_->angular_->z()); + bullet_solver_->judgeShootBeforehand(time); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -336,10 +382,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { tf2::doTransform(gyro, angular_vel_pitch, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); tf2::doTransform(gyro, angular_vel_yaw, - robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(yaw_joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); } catch (tf2::TransformException& ex) @@ -353,11 +399,13 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } - geometry_msgs::TransformStamped base_frame2des; - base_frame2des = - robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); - double roll_des, pitch_des, yaw_des; // desired position - quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); + double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; + quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); + quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); + double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); + double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); + pid_pitch_pos_.computeCommand(pitch_angle_error, period); + pid_yaw_pos_.computeCommand(yaw_angle_error, period); double yaw_vel_des = 0., pitch_vel_des = 0.; if (state_ == RATE) @@ -373,19 +421,18 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); tf2::Vector3 target_pos_tf, target_vel_tf; - try { geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + yaw_joint_urdf_->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); yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); + transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->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); @@ -397,36 +444,63 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } + if (!pitch_des_in_limit_) + pitch_vel_des = 0.; + if (!yaw_des_in_limit_) + yaw_vel_des = 0.; + + pid_pitch_pos_.computeCommand(pitch_angle_error, period); + pid_yaw_pos_.computeCommand(yaw_angle_error, period); + + // publish state + if (loop_count_ % 10 == 0) + { + if (yaw_pos_state_pub_ && yaw_pos_state_pub_->trylock()) + { + yaw_pos_state_pub_->msg_.header.stamp = time; + yaw_pos_state_pub_->msg_.set_point = yaw_des; + yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; + yaw_pos_state_pub_->msg_.process_value = yaw_real; + yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); + yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); + yaw_pos_state_pub_->unlockAndPublish(); + } + if (pitch_pos_state_pub_ && pitch_pos_state_pub_->trylock()) + { + pitch_pos_state_pub_->msg_.header.stamp = time; + pitch_pos_state_pub_->msg_.set_point = pitch_des; + pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; + pitch_pos_state_pub_->msg_.process_value = pitch_real; + pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); + pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); + pitch_pos_state_pub_->unlockAndPublish(); + } + } + loop_count_++; + + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - ctrl_yaw_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); - ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - double resistance_compensation = 0.; - if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_) - resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; - else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_) - resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; - else - resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_; - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + - yaw_k_v_ * yaw_vel_des + resistance_compensation); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) { Eigen::Vector3d gravity(0, 0, -gravity_); tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, "odom", time)); + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, "base_link", time)); Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); double feedforward = -mass_origin.cross(gravity).y(); if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); tf2::doTransform(gravity_compensation, gravity_compensation, - robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, - ctrl_pitch_.joint_urdf_->parent_link_name, time)); + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, + pitch_joint_urdf_->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -464,6 +538,27 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } +void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) +{ + ROS_INFO("[Gimbal Base] Dynamic params change"); + if (!dynamic_reconfig_initialized_) + { + 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.k_chassis_vel_ = init_config.k_chassis_vel_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + dynamic_reconfig_initialized_ = true; + } + GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, + .pitch_k_v_ = config.pitch_k_v_, + .k_chassis_vel_ = config.k_chassis_vel_, + .accel_pitch_ = config.accel_pitch_, + .accel_yaw_ = config.accel_yaw_ }; + config_rt_buffer_.writeFromNonRT(config_non_rt); +} + } // namespace rm_gimbal_controllers PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::Controller, controller_interface::ControllerBase)