From 032ada0782edfa7d98d8dccfd35c9560ffc8cebe Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Sat, 4 May 2024 23:16:01 +0800 Subject: [PATCH 01/11] Claculate heat with trigger. --- .../rm_common/decision/command_sender.h | 13 +--- .../include/rm_common/decision/heat_limit.h | 78 +++++++++++++++---- rm_msgs/msg/ShootState.msg | 1 + 3 files changed, 67 insertions(+), 25 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index a761fa5c..a29254d0 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -359,10 +359,6 @@ class ShooterCommandSender : public TimeStampCommandSenderBase gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || - (track_data_.accel > target_acceleration_tolerance_)) || - (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) || - (allow_shoot_.error == 0. && time - allow_shoot_.stamp < ros::Duration(0.1))) + if (((gimbal_des_error_.error > gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || + (track_data_.accel > target_acceleration_tolerance_)) || + (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) if (msg_.mode == rm_msgs::ShootCmd::PUSH) setMode(rm_msgs::ShootCmd::READY); } @@ -464,7 +459,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase #include #include +#include +#include namespace rm_common { @@ -64,11 +66,14 @@ class HeatLimit ROS_ERROR("Heat coeff no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("type", type_)) ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str()); - // nh.param("safe_speed_limit", shooter_speed_limit_, 15); + nh.param("is_local", is_local_, true); if (type_ == "ID1_42MM") bullet_heat_ = 100.; else bullet_heat_ = 10.; + heat_pub_ = nh.advertise("/heat", 1); + heat_sub_ = + nh.subscribe("/controllers/shooter_controller/state", 50, &HeatLimit::heatCB, this); } typedef enum @@ -79,6 +84,36 @@ class HeatLimit MINIMAL = 3 } ShootHz; + void heatCB(const rm_msgs::ShootState msg) + { + rm_msgs::TrackData ms; + if (msg.has_shoot == true) + { + shooter_local_cooling_heat_ += bullet_heat_; + ms.v_yaw = 1; + } + if (shooter_local_cooling_heat_ >= shooter_cooling_limit_) + { + local_frequency_ = 0.0; + ms.dz = 1; + } + else + { + local_frequency_ = shoot_frequency_; + ms.dz = 2; + } + if ((ros::Time::now() - last_time_).toSec() > 1. && shooter_local_cooling_heat_ > 0) + { + last_time_ = ros::Time::now(); + shooter_local_cooling_heat_ -= shooter_cooling_rate_; + if (shooter_local_cooling_heat_ < 0) + shooter_local_cooling_heat_ = 0; + } + ms.radius_1 = shooter_local_cooling_heat_; + ms.radius_2 = local_frequency_; + heat_pub_.publish(ms); + } + void setStatusOfShooter(const rm_msgs::GameRobotStatus data) { shooter_cooling_limit_ = data.shooter_cooling_limit; @@ -110,19 +145,26 @@ class HeatLimit { if (state_ == BURST) return shoot_frequency_; - if (!referee_is_online_) - return safe_shoot_frequency_; - - if (shooter_cooling_limit_ - shooter_cooling_heat_ < bullet_heat_) - return 0.0; - else if (shooter_cooling_limit_ - shooter_cooling_heat_ == bullet_heat_) - return shooter_cooling_rate_ / bullet_heat_; - else if (shooter_cooling_limit_ - shooter_cooling_heat_ <= bullet_heat_ * heat_coeff_) - return (shooter_cooling_limit_ - shooter_cooling_heat_) / (bullet_heat_ * heat_coeff_) * - (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) + - shooter_cooling_rate_ / bullet_heat_; + + if (!is_local_) + { + if (!referee_is_online_) + return safe_shoot_frequency_; + if (shooter_cooling_limit_ - shooter_cooling_heat_ < bullet_heat_) + return 0.0; + else if (shooter_cooling_limit_ - shooter_cooling_heat_ == bullet_heat_) + return shooter_cooling_rate_ / bullet_heat_; + else if (shooter_cooling_limit_ - shooter_cooling_heat_ <= bullet_heat_ * heat_coeff_) + return (shooter_cooling_limit_ - shooter_cooling_heat_) / (bullet_heat_ * heat_coeff_) * + (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) + + shooter_cooling_rate_ / bullet_heat_; + else + return shoot_frequency_; + } else - return shoot_frequency_; + { + return local_frequency_; + } } int getSpeedLimit() @@ -191,10 +233,14 @@ class HeatLimit std::string type_{}; bool burst_flag_ = false; double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{}, - high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}; + high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}, local_frequency_{}; + + bool referee_is_online_, is_local_; + int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_, shooter_local_cooling_heat_{}; - bool referee_is_online_; - int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_; + ros::Publisher heat_pub_; + ros::Subscriber heat_sub_; + ros::Time last_time_{}; }; } // namespace rm_common diff --git a/rm_msgs/msg/ShootState.msg b/rm_msgs/msg/ShootState.msg index bdb19932..340964d8 100644 --- a/rm_msgs/msg/ShootState.msg +++ b/rm_msgs/msg/ShootState.msg @@ -3,5 +3,6 @@ uint8 READY = 1 uint8 PUSH = 2 uint8 BLOCK = 3 +bool has_shoot uint8 state time stamp From cfabc3c8969f1e52e24c3a81f4a754f7524d7d33 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Sun, 5 May 2024 17:30:27 +0800 Subject: [PATCH 02/11] Reduce check time. --- rm_common/include/rm_common/decision/heat_limit.h | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index c8a4d2b4..fe63495d 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -42,7 +42,6 @@ #include #include #include -#include namespace rm_common { @@ -90,28 +89,22 @@ class HeatLimit if (msg.has_shoot == true) { shooter_local_cooling_heat_ += bullet_heat_; - ms.v_yaw = 1; } if (shooter_local_cooling_heat_ >= shooter_cooling_limit_) { local_frequency_ = 0.0; - ms.dz = 1; } else { local_frequency_ = shoot_frequency_; - ms.dz = 2; } - if ((ros::Time::now() - last_time_).toSec() > 1. && shooter_local_cooling_heat_ > 0) + if ((ros::Time::now() - last_time_).toSec() > 0.1 && shooter_local_cooling_heat_ > 0) { last_time_ = ros::Time::now(); - shooter_local_cooling_heat_ -= shooter_cooling_rate_; + shooter_local_cooling_heat_ -= shooter_cooling_rate_ / 10; if (shooter_local_cooling_heat_ < 0) shooter_local_cooling_heat_ = 0; } - ms.radius_1 = shooter_local_cooling_heat_; - ms.radius_2 = local_frequency_; - heat_pub_.publish(ms); } void setStatusOfShooter(const rm_msgs::GameRobotStatus data) From 8bc50c1520b31b8c35a89336e1557a39bd1f039e Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Wed, 8 May 2024 19:08:41 +0800 Subject: [PATCH 03/11] Add LocalHeatState.msg and gradient calculation of heat. --- .../include/rm_common/decision/heat_limit.h | 18 ++++++++++-------- rm_msgs/CMakeLists.txt | 3 +++ rm_msgs/msg/LocalHeatState.msg | 4 ++++ 3 files changed, 17 insertions(+), 8 deletions(-) create mode 100644 rm_msgs/msg/LocalHeatState.msg diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index fe63495d..4abfb1e9 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include namespace rm_common { @@ -70,9 +70,7 @@ class HeatLimit bullet_heat_ = 100.; else bullet_heat_ = 10.; - heat_pub_ = nh.advertise("/heat", 1); - heat_sub_ = - nh.subscribe("/controllers/shooter_controller/state", 50, &HeatLimit::heatCB, this); + heat_sub_ = nh.subscribe("/local_heat_state", 50, &HeatLimit::heatCB, this); } typedef enum @@ -83,17 +81,22 @@ class HeatLimit MINIMAL = 3 } ShootHz; - void heatCB(const rm_msgs::ShootState msg) + void heatCB(const rm_msgs::LocalHeatState msg) { - rm_msgs::TrackData ms; if (msg.has_shoot == true) { shooter_local_cooling_heat_ += bullet_heat_; } - if (shooter_local_cooling_heat_ >= shooter_cooling_limit_) + if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= 2 * bullet_heat_) { local_frequency_ = 0.0; } + if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= bullet_heat_ * (heat_coeff_ + 2)) + { + local_frequency_ = (shooter_cooling_limit_ - shooter_local_cooling_heat_) / (bullet_heat_ * heat_coeff_) * + (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) + + shooter_cooling_rate_ / bullet_heat_; + } else { local_frequency_ = shoot_frequency_; @@ -231,7 +234,6 @@ class HeatLimit bool referee_is_online_, is_local_; int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_, shooter_local_cooling_heat_{}; - ros::Publisher heat_pub_; ros::Subscriber heat_sub_; ros::Time last_time_{}; }; diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index e0b80be3..089486bd 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -17,9 +17,12 @@ add_message_files( ChassisCmd.msg ShootCmd.msg ShootState.msg + ShootBeforehandCmd.msg GimbalCmd.msg GimbalDesError.msg + GimbalPosState.msg LpData.msg + LocalHeatState.msg KalmanData.msg MovingAverageData.msg GpioData.msg diff --git a/rm_msgs/msg/LocalHeatState.msg b/rm_msgs/msg/LocalHeatState.msg new file mode 100644 index 00000000..e844cd42 --- /dev/null +++ b/rm_msgs/msg/LocalHeatState.msg @@ -0,0 +1,4 @@ +float64 friction_change_vel +bool has_shoot + +time stamp \ No newline at end of file From 8f6073a7a50b66d8c6304be624739adc7fb20b50 Mon Sep 17 00:00:00 2001 From: BlanchardLj <116863539+BlanchardLj@users.noreply.github.com> Date: Wed, 8 May 2024 19:56:48 +0800 Subject: [PATCH 04/11] Update command_sender.h --- .../rm_common/decision/command_sender.h | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index cf5a92a6..8c27bf40 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -360,7 +360,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || (track_data_.accel > target_acceleration_tolerance_)) || (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) @@ -462,6 +478,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Wed, 8 May 2024 19:59:01 +0800 Subject: [PATCH 05/11] Update command_sender.h MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 把错误删除的代码改回去 --- rm_common/include/rm_common/decision/command_sender.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 8c27bf40..29c24e61 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -375,13 +375,13 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Sat, 8 Jun 2024 17:51:45 +0800 Subject: [PATCH 06/11] Update the formula. --- .../include/rm_common/decision/heat_limit.h | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 4abfb1e9..0cd611eb 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -70,7 +70,8 @@ class HeatLimit bullet_heat_ = 100.; else bullet_heat_ = 10.; - heat_sub_ = nh.subscribe("/local_heat_state", 50, &HeatLimit::heatCB, this); + heat_pub_ = nh.advertise("/local_heat_state/local_cooling_heat", 10); + heat_sub_ = nh.subscribe("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this); } typedef enum @@ -87,27 +88,30 @@ class HeatLimit { shooter_local_cooling_heat_ += bullet_heat_; } - if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= 2 * bullet_heat_) + if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= bullet_heat_) { local_frequency_ = 0.0; } - if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= bullet_heat_ * (heat_coeff_ + 2)) + else if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= bullet_heat_ * heat_coeff_) { - local_frequency_ = (shooter_cooling_limit_ - shooter_local_cooling_heat_) / (bullet_heat_ * heat_coeff_) * + local_frequency_ = (shooter_cooling_limit_ - shooter_local_cooling_heat_) / shooter_cooling_limit_ * + (shooter_cooling_limit_ - shooter_local_cooling_heat_) / shooter_cooling_limit_ * (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) + - shooter_cooling_rate_ / bullet_heat_; + shooter_cooling_rate_ / bullet_heat_ + 1.0; } else { local_frequency_ = shoot_frequency_; } - if ((ros::Time::now() - last_time_).toSec() > 0.1 && shooter_local_cooling_heat_ > 0) + if ((ros::Time::now() - last_time_).toSec() > 0.1 && shooter_local_cooling_heat_ > 0.0) { last_time_ = ros::Time::now(); - shooter_local_cooling_heat_ -= shooter_cooling_rate_ / 10; - if (shooter_local_cooling_heat_ < 0) - shooter_local_cooling_heat_ = 0; + shooter_local_cooling_heat_ -= shooter_cooling_rate_ / 10.0; + if (shooter_local_cooling_heat_ < 0.0) + shooter_local_cooling_heat_ = 0.0; } + local_heat_.data = shooter_local_cooling_heat_; + heat_pub_.publish(local_heat_); } void setStatusOfShooter(const rm_msgs::GameRobotStatus data) @@ -227,13 +231,16 @@ class HeatLimit uint8_t state_{}; std::string type_{}; + std_msgs::Float64 local_heat_; bool burst_flag_ = false; double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{}, high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}, local_frequency_{}; bool referee_is_online_, is_local_; - int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_, shooter_local_cooling_heat_{}; + int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_; + double shooter_local_cooling_heat_{}; + ros::Publisher heat_pub_; ros::Subscriber heat_sub_; ros::Time last_time_{}; }; From 5c1a5b026fecbe8c7ad8cc56d24811850a68b93b Mon Sep 17 00:00:00 2001 From: BlanchardLj <116863539+BlanchardLj@users.noreply.github.com> Date: Sat, 8 Jun 2024 17:56:40 +0800 Subject: [PATCH 07/11] Update ShootState.msg --- rm_msgs/msg/ShootState.msg | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_msgs/msg/ShootState.msg b/rm_msgs/msg/ShootState.msg index 340964d8..bdb19932 100644 --- a/rm_msgs/msg/ShootState.msg +++ b/rm_msgs/msg/ShootState.msg @@ -3,6 +3,5 @@ uint8 READY = 1 uint8 PUSH = 2 uint8 BLOCK = 3 -bool has_shoot uint8 state time stamp From 295f0c0cd490a4105f4514551830b82b780335d9 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Tue, 9 Jul 2024 09:37:46 +0800 Subject: [PATCH 08/11] Add a header file in heat_limit.h and add auto set frictions' speed. --- .../rm_common/decision/command_sender.h | 56 +++++++++++++------ .../include/rm_common/decision/heat_limit.h | 3 +- 2 files changed, 40 insertions(+), 19 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 46fd1468..57d66650 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -161,7 +162,7 @@ class Vel2DCommandSender : public CommandSenderBase max_angular_z_.init(xml_rpc_value); std::string topic; nh.getParam("power_limit_topic", topic); - target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 3.); + target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.); chassis_power_limit_subscriber_ = nh.subscribe(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this); } @@ -186,15 +187,6 @@ class Vel2DCommandSender : public CommandSenderBase vel_direction_ = 1.; msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_; }; - void setAngularZVel(double scale, double limit) - { - if (track_data_.v_yaw > target_vel_yaw_threshold_) - vel_direction_ = -1.; - if (track_data_.v_yaw < -target_vel_yaw_threshold_) - vel_direction_ = 1.; - double angular_z = max_angular_z_.output(power_limit_) > limit ? limit : max_angular_z_.output(power_limit_); - msg_.angular.z = scale * angular_z * vel_direction_; - }; void set2DVel(double scale_x, double scale_y, double scale_z) { setLinearXVel(scale_x); @@ -354,6 +346,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase 1.0 || shoot_data_.bullet_speed > speed_limit_) + { + total_extra_wheel_speed_ -= 5.0; + } + else if (speed_des_ - last_bullet_speed_ > 1.0) + { + total_extra_wheel_speed_ += 5.0; + } + } + if (shoot_data_.bullet_speed != 0.0) + last_bullet_speed_ = shoot_data_.bullet_speed; + } + } void checkError(const ros::Time& time) { - if (msg_.mode == rm_msgs::ShootCmd::PUSH && time - -shoot_beforehand_cmd_.stamp < ros::Duration(0.1)) + if (msg_.mode == rm_msgs::ShootCmd::PUSH && time - shoot_beforehand_cmd_.stamp < ros::Duration(0.1)) { - if (shoot_beforehand_cmd_.cmd == -rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT) + if (shoot_beforehand_cmd_.cmd == rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT) return; - if (shoot_beforehand_cmd_.cmd == -rm_msgs::ShootBeforehandCmd::BAN_SHOOT) + if (shoot_beforehand_cmd_.cmd == rm_msgs::ShootBeforehandCmd::BAN_SHOOT) { setMode(rm_msgs::ShootCmd::READY); return; @@ -436,30 +449,35 @@ rm_msgs::ShootBeforehandCmd::BAN_SHOOT) { case rm_msgs::ShootCmd::SPEED_10M_PER_SECOND: { + speed_limit_ = 10.0; speed_des_ = speed_10_; wheel_speed_des_ = wheel_speed_10_; break; } case rm_msgs::ShootCmd::SPEED_15M_PER_SECOND: { + speed_limit_ = 15.0; speed_des_ = speed_15_; wheel_speed_des_ = wheel_speed_15_; break; } case rm_msgs::ShootCmd::SPEED_16M_PER_SECOND: { + speed_limit_ = 16.0; speed_des_ = speed_16_; wheel_speed_des_ = wheel_speed_16_; break; } case rm_msgs::ShootCmd::SPEED_18M_PER_SECOND: { + speed_limit_ = 18.0; speed_des_ = speed_18_; wheel_speed_des_ = wheel_speed_18_; break; } case rm_msgs::ShootCmd::SPEED_30M_PER_SECOND: { + speed_limit_ = 30.0; speed_des_ = speed_30_; wheel_speed_des_ = wheel_speed_30_; break; @@ -490,16 +508,18 @@ rm_msgs::ShootBeforehandCmd::BAN_SHOOT) HeatLimit* heat_limit_{}; private: - double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{}; + double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{}, speed_limit_{}; double wheel_speed_10_{}, wheel_speed_15_{}, wheel_speed_16_{}, wheel_speed_18_{}, wheel_speed_30_{}, - wheel_speed_des_{}; + wheel_speed_des_{}, last_bullet_speed_{}; double gimbal_error_tolerance_{}; double target_acceleration_tolerance_{}; double extra_wheel_speed_once_{}; double total_extra_wheel_speed_{}; + bool auto_wheel_speed_ = false; rm_msgs::TrackData track_data_; rm_msgs::GimbalDesError gimbal_des_error_; rm_msgs::ShootBeforehandCmd shoot_beforehand_cmd_; + rm_msgs::ShootData shoot_data_; std_msgs::Bool suggest_fire_; uint8_t armor_type_{}; }; diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 0cd611eb..035df395 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace rm_common { @@ -105,10 +106,10 @@ class HeatLimit } if ((ros::Time::now() - last_time_).toSec() > 0.1 && shooter_local_cooling_heat_ > 0.0) { - last_time_ = ros::Time::now(); shooter_local_cooling_heat_ -= shooter_cooling_rate_ / 10.0; if (shooter_local_cooling_heat_ < 0.0) shooter_local_cooling_heat_ = 0.0; + last_time_ = ros::Time::now(); } local_heat_.data = shooter_local_cooling_heat_; heat_pub_.publish(local_heat_); From 7d72dd388e89d3fe3df0075f3491524f0a1e2537 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Mon, 15 Jul 2024 14:59:56 +0800 Subject: [PATCH 09/11] Merge branch 'master' of github.com:rm-controls/rm_control into calculate_heat_trigger --- rm_common/include/rm_common/decision/command_sender.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index aeb6d481..827e9b8b 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -162,7 +162,7 @@ class Vel2DCommandSender : public CommandSenderBase max_angular_z_.init(xml_rpc_value); std::string topic; nh.getParam("power_limit_topic", topic); - target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.); + target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 3.); chassis_power_limit_subscriber_ = nh.subscribe(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this); } @@ -187,6 +187,15 @@ class Vel2DCommandSender : public CommandSenderBase vel_direction_ = 1.; msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_; }; + void setAngularZVel(double scale, double limit) + { + if (track_data_.v_yaw > target_vel_yaw_threshold_) + vel_direction_ = -1.; + if (track_data_.v_yaw < -target_vel_yaw_threshold_) + vel_direction_ = 1.; + double angular_z = max_angular_z_.output(power_limit_) > limit ? limit : max_angular_z_.output(power_limit_); + msg_.angular.z = scale * angular_z * vel_direction_; + }; void set2DVel(double scale_x, double scale_y, double scale_z) { setLinearXVel(scale_x); From 6e09a40b4935b1320c9a25ca143988215fb65c63 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Wed, 17 Jul 2024 14:40:47 +0800 Subject: [PATCH 10/11] Add param of speed oscillation. --- rm_common/include/rm_common/decision/command_sender.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 827e9b8b..d07533dc 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -352,6 +352,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase 1.0 || shoot_data_.bullet_speed > speed_limit_) + if (last_bullet_speed_ - speed_des_ >= speed_oscillation_ || shoot_data_.bullet_speed > speed_limit_) { total_extra_wheel_speed_ -= 5.0; } - else if (speed_des_ - last_bullet_speed_ > 1.0) + else if (speed_des_ - last_bullet_speed_ > speed_oscillation_) { total_extra_wheel_speed_ += 5.0; } @@ -519,7 +520,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Thu, 25 Jul 2024 20:04:26 +0800 Subject: [PATCH 11/11] Fix merge problem. --- rm_common/include/rm_common/decision/command_sender.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 2974a3c6..ea2557bd 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -358,6 +358,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase