From 2a571e682b6c336b63da257030ebdfba52c01710 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 21 Mar 2026 00:52:59 +0800 Subject: [PATCH 01/13] Update trigger_change_ui to rename 'fallen' to 'sit_down' and add 'recovery' mode --- rm_referee/src/ui/trigger_change_ui.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index bea8e352..81b88660 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -197,7 +197,9 @@ std::string ChassisTriggerChangeUi::getChassisState(uint8_t mode) else if (mode == rm_msgs::ChassisCmd::UP_SLOPE) return "up_slope"; else if (mode == rm_msgs::ChassisCmd::FALLEN) - return "fallen"; + return "sit_down"; + else if (mode == rm_msgs::ChassisCmd::RECOVERY) + return "recovery"; else return "error"; } From 986df678f47d1cf38cf09289a5858699c2ea860b Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 21 Mar 2026 01:04:07 +0800 Subject: [PATCH 02/13] Add reconnect method to Referee class for serial communication --- rm_referee/include/rm_referee/referee.h | 1 + rm_referee/src/referee.cpp | 20 +++++++++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index a75045d9..7d060327 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -98,6 +98,7 @@ class Referee rx_buffer_.clear(); rx_len_ = 0; } + void reconnect(); ros::Publisher game_robot_status_pub_; ros::Publisher game_status_pub_; diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index ffab44ff..1981f848 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -74,6 +74,25 @@ void Referee::read() clearRxBuffer(); } +void Referee::reconnect() +{ + try + { + if (base_.serial_.isOpen()) + base_.serial_.close(); + + base_.serial_.open(); + clearRxBuffer(); + + ROS_WARN("Referee serial reconnected"); + } + catch (...) + { + ROS_ERROR("Referee reconnect failed"); + ros::Duration(1.0).sleep(); + } +} + int Referee::unpack(uint8_t* rx_data) { uint16_t cmd_id; @@ -278,7 +297,6 @@ int Referee::unpack(uint8_t* rx_data) power_heat_data.chassis_power_buffer = power_heat_ref.chassis_power_buffer; power_heat_data.shooter_id_1_17_mm_cooling_heat = power_heat_ref.shooter_id_1_17_mm_cooling_heat; - power_heat_data.shooter_id_2_17_mm_cooling_heat = power_heat_ref.shooter_id_2_17_mm_cooling_heat; power_heat_data.shooter_id_1_42_mm_cooling_heat = power_heat_ref.shooter_id_1_42_mm_cooling_heat; power_heat_data.stamp = last_get_data_time_; From 94ce042bbc4b4fa390d44ec7749e1906a94792ad Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 23 Mar 2026 11:43:02 +0800 Subject: [PATCH 03/13] Add RECOVERY mode to ChassisCmd message --- rm_msgs/msg/ChassisCmd.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/rm_msgs/msg/ChassisCmd.msg b/rm_msgs/msg/ChassisCmd.msg index 8d5bb0b1..5e30d6de 100644 --- a/rm_msgs/msg/ChassisCmd.msg +++ b/rm_msgs/msg/ChassisCmd.msg @@ -4,6 +4,7 @@ uint8 TWIST = 2 uint8 UP_SLOPE = 3 uint8 FALLEN = 4 uint8 DEPLOY = 5 +uint8 RECOVERY = 6 uint8 mode geometry_msgs/Accel accel From 17deea3fbca13b158789f1946496d905ba644792 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 28 Mar 2026 15:41:26 +0800 Subject: [PATCH 04/13] Add DebugData message and publisher for enhanced debugging capabilities --- .../include/rm_common/DebugDataPublisher.h | 112 ++++++++++++++++++ .../include/rm_common/decision/heat_limit.h | 2 + rm_msgs/CMakeLists.txt | 3 + rm_msgs/msg/DebugData.msg | 8 ++ 4 files changed, 125 insertions(+) create mode 100644 rm_common/include/rm_common/DebugDataPublisher.h create mode 100644 rm_msgs/msg/DebugData.msg diff --git a/rm_common/include/rm_common/DebugDataPublisher.h b/rm_common/include/rm_common/DebugDataPublisher.h new file mode 100644 index 00000000..51412e0c --- /dev/null +++ b/rm_common/include/rm_common/DebugDataPublisher.h @@ -0,0 +1,112 @@ +// +// Created by wk on 2026/3/2. +// +#pragma once +#include +#include +#include +#include + +/** + * @brief Debug data publisher. Supports adding/updating named variables and batch publishing. + */ +class DebugDataPublisher +{ +public: + /** + * @brief Construct and initialize the publisher. + * @param nh ROS NodeHandle used for advertise. + * @param topic Topic name, defaults to "debug_data". + * @param queue_size Publish queue size, defaults to 10. + */ + explicit DebugDataPublisher(ros::NodeHandle& nh, const std::string& topic = "debug_data", int queue_size = 10) + { + debug_data_pub_ = nh.advertise(topic, queue_size); + } + + /** + * @brief Add or update a debug variable, stamped at current time. + * @param name Variable name. Duplicates update the existing entry. + * @param value Variable value. + */ + void add(const std::string& name, double value) + { + auto it = index_map_.find(name); + if (it != index_map_.end()) + { + // Update existing entry + debug_data_.stamp[it->second] = ros::Time::now(); + debug_data_.value[it->second] = value; + } + else + { + // Insert new entry + index_map_[name] = debug_data_.name.size(); + debug_data_.stamp.push_back(ros::Time::now()); + debug_data_.name.push_back(name); + debug_data_.value.push_back(value); + } + } + + /** + * @brief Add or update a debug variable with a custom timestamp. + * @param name Variable name. Duplicates update the existing entry. + * @param value Variable value. + * @param stamp Custom timestamp. + */ + void add(const std::string& name, double value, const ros::Time& stamp) + { + auto it = index_map_.find(name); + if (it != index_map_.end()) + { + debug_data_.stamp[it->second] = stamp; + debug_data_.value[it->second] = value; + } + else + { + index_map_[name] = debug_data_.name.size(); + debug_data_.stamp.push_back(stamp); + debug_data_.name.push_back(name); + debug_data_.value.push_back(value); + } + } + + /** + * @brief Publish accumulated debug data and clear. + */ + void publish() + { + if (!debug_data_.name.empty()) + { + debug_data_pub_.publish(debug_data_); + } + clear(); + } + + /** + * @brief Publish accumulated debug data without clearing. Useful for persistent variables. + */ + void publishAndKeep() + { + if (!debug_data_.name.empty()) + { + debug_data_pub_.publish(debug_data_); + } + } + + /** + * @brief Clear all stored debug data. + */ + void clear() + { + debug_data_.stamp.clear(); + debug_data_.name.clear(); + debug_data_.value.clear(); + index_map_.clear(); + } + +private: + ros::Publisher debug_data_pub_; ///< ROS publisher. + rm_msgs::DebugData debug_data_; ///< Debug data to be published. + std::unordered_map index_map_; ///< Name-to-index map for O(1) update. +}; diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 752e2708..dcd5228c 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -142,6 +142,8 @@ class HeatLimit return shoot_frequency_; double shooter_cooling_heat = (use_local_heat_ || !referee_is_online_) ? local_shooter_cooling_heat_ : shooter_cooling_heat_; + if (!referee_is_online_) + return 5.0; if (shooter_cooling_limit_ - shooter_cooling_heat < bullet_heat_) return 0.0; else if (shooter_cooling_limit_ - shooter_cooling_heat == bullet_heat_) diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index fec55dad..c49a9d4a 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -13,10 +13,12 @@ add_message_files( FILES ActuatorState.msg BalanceState.msg + BulletSolverData.msg BusState.msg DbusData.msg ExchangerMsg.msg ChassisCmd.msg + DebugData.msg ShootCmd.msg ShootState.msg ShootBeforehandCmd.msg @@ -30,6 +32,7 @@ add_message_files( LeggedLQRStatus.msg LQRkMatrix.msg LpData.msg + LocalHeatData.msg LocalHeatState.msg KalmanData.msg MovingAverageData.msg diff --git a/rm_msgs/msg/DebugData.msg b/rm_msgs/msg/DebugData.msg new file mode 100644 index 00000000..13aad0c4 --- /dev/null +++ b/rm_msgs/msg/DebugData.msg @@ -0,0 +1,8 @@ +# The time at which this actuator state was measured +time[] stamp + +# The name of variable +string[] name + +# The value of variable +float64[] value From b341051b779e3fd36fe0d473b8009cdab06dd29f Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 28 Mar 2026 15:41:39 +0800 Subject: [PATCH 05/13] Add error handling for serial connection and enhance power limit parameters --- .../include/rm_common/decision/power_limit.h | 30 +++++++++++++++++-- .../include/rm_referee/common/protocol.h | 1 - rm_referee/src/main.cpp | 17 +++++++---- 3 files changed, 39 insertions(+), 9 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 0957be7b..f53cab18 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -60,6 +60,10 @@ class PowerLimit ROS_ERROR("Disable cap gyro threshold no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("enable_cap_gyro_threshold", enable_cap_gyro_threshold_)) ROS_ERROR("Enable cap gyro threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("disable_use_cap_threshold", disable_use_cap_threshold_)) + ROS_ERROR("Disable use cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("enable_use_cap_threshold", enable_use_cap_threshold_)) + ROS_ERROR("Enable use cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("charge_power", charge_power_)) ROS_ERROR("Charge power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("extra_power", extra_power_)) @@ -117,7 +121,7 @@ class PowerLimit } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(10); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } @@ -146,7 +150,25 @@ class PowerLimit if (allow_gyro_cap_ && chassis_power_limit_ < 80) chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; else - expect_state_ = NORMAL; + normal(chassis_cmd); + // expect_state_ = NORMAL; + } + void setBurstPower(rm_msgs::ChassisCmd& chassis_cmd) + { + if (!allow_use_cap_ && cap_energy_ >= enable_use_cap_threshold_) + allow_use_cap_ = true; + if (allow_use_cap_ && cap_energy_ <= disable_use_cap_threshold_) + allow_use_cap_ = false; + if (allow_use_cap_) + { + if (ros::Time::now() - start_burst_time_ < ros::Duration(total_burst_time_)) + chassis_cmd.power_limit = burst_power_; + else + chassis_cmd.power_limit = standard_power_; + } + else + normal(chassis_cmd); + // expect_state_ = NORMAL; } void setLimitPower(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { @@ -211,7 +233,7 @@ class PowerLimit { if (is_gyro) setGyroPower(chassis_cmd); - else if (ros::Time::now() - start_burst_time_ < ros::Duration(total_burst_time_)) + else if (ros::Time::now() - start_burst_time_ > ros::Duration(total_burst_time_)) chassis_cmd.power_limit = burst_power_; else chassis_cmd.power_limit = standard_power_; @@ -228,12 +250,14 @@ class PowerLimit double capacitor_threshold_{}; double power_buffer_threshold_{ 50.0 }; double enable_cap_gyro_threshold_{}, disable_cap_gyro_threshold_{}; + double enable_use_cap_threshold_{}, disable_use_cap_threshold_{}; double charge_power_{}, extra_power_{}, burst_power_{}, standard_power_{}; double buffer_threshold_{}; double power_gain_{}; bool is_new_capacitor_{ false }; uint8_t expect_state_{}, cap_state_{}; bool capacitor_is_on_{ true }; + bool allow_use_cap_{ false }; bool allow_gyro_cap_{ false }; ros::Time start_burst_time_{}; diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index d933c523..72754795 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -344,7 +344,6 @@ typedef struct float reserved_3; uint16_t chassis_power_buffer; uint16_t shooter_id_1_17_mm_cooling_heat; - uint16_t shooter_id_2_17_mm_cooling_heat; uint16_t shooter_id_1_42_mm_cooling_heat; } __packed PowerHeatData; diff --git a/rm_referee/src/main.cpp b/rm_referee/src/main.cpp index ae2022d6..19933b29 100644 --- a/rm_referee/src/main.cpp +++ b/rm_referee/src/main.cpp @@ -11,12 +11,19 @@ int main(int argc, char** argv) ros::NodeHandle nh("~"); rm_referee::Referee referee(nh); ros::Rate loop_rate(80); - while (ros::ok()) + try { - ros::spinOnce(); - referee.read(); - loop_rate.sleep(); + while (ros::ok()) + { + ros::spinOnce(); + referee.read(); + loop_rate.sleep(); + } + } + catch (const serial::SerialException& e) + { + ROS_ERROR_STREAM("Serial lost: " << e.what()); + referee.reconnect(); } - return 0; } From f41953163e3fc3d263c370ffce64ecf8fc0ec0f1 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 12 Apr 2026 20:23:34 +0800 Subject: [PATCH 06/13] Add setter for burst power limit in power_limit.h --- rm_common/include/rm_common/decision/power_limit.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 48c41537..3f8ec533 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -136,6 +136,10 @@ class PowerLimit { start_burst_time_ = start_burst_time; } + inline void setBurstPowerLimit(const double& burst_power_limit) + { + burst_power_ = burst_power_limit; + } ros::Time getStartBurstTime() const { return start_burst_time_; From 2064fc17ab5f4b88e82d24d167311b4f4e84dc8f Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 12 Apr 2026 20:24:25 +0800 Subject: [PATCH 07/13] add step_down launch and world files for simulation setup --- rm_gazebo/launch/step_down.launch | 33 ++++ rm_gazebo/worlds/step_down.world | 284 ++++++++++++++++++++++++++++++ 2 files changed, 317 insertions(+) create mode 100644 rm_gazebo/launch/step_down.launch create mode 100644 rm_gazebo/worlds/step_down.world diff --git a/rm_gazebo/launch/step_down.launch b/rm_gazebo/launch/step_down.launch new file mode 100644 index 00000000..a15ab5aa --- /dev/null +++ b/rm_gazebo/launch/step_down.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + r + + + 23 + + + + + diff --git a/rm_gazebo/worlds/step_down.world b/rm_gazebo/worlds/step_down.world new file mode 100644 index 00000000..f650da9a --- /dev/null +++ b/rm_gazebo/worlds/step_down.world @@ -0,0 +1,284 @@ + + + + 0.001 + 1000 + 1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + 1 + + 2 0.8 0 0 -0 3.14 + + + + model://rm_gazebo/worlds/place/step.stl + 0.001 0.001 0.001 + + + + + + + model://rm_gazebo/worlds/place/step.stl + 0.001 0.001 0.001 + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 1 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1 + 1 + + 2 0.8 0 0 -0 3.14 + + + + model://rm_gazebo/worlds/place/step.stl + 0.001 0.001 0.001 + + + + + + + model://rm_gazebo/worlds/place/step.stl + 0.001 0.001 0.001 + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 1 + 1.63818 -0.176815 0 0 -0 0 + + + 1 + 1 + + 2 0.8 0 0 -0 3.14 + + + + model://rm_gazebo/worlds/place/step.stl + 0.001 0.001 0.001 + + + + + + + model://rm_gazebo/worlds/place/step.stl + 0.001 0.001 0.001 + + + 10 + + + + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + 0 + + 1 + 3.5612 -0.272009 0 0 -0 0 + + + 544 417000000 + 9 297544683 + 1775969403 727393628 + 9121 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 2 0.8 0 0 -0 3.14 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 6.03223 -0.024041 0 0 -0 3.13521 + 1 1 1 + + 4.02716 -0.81125 0 0 -0 -0.00798 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1.03078 0.000587 0.300149 0 -0 0 + 1 1 1 + + 3.03078 0.800587 0.300149 0 -0 3.14 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + -1.17337 -9.03322 2.85913 0 0.275642 1.31538 + orbit + perspective + + + + From 4d6ca17e0eadbbf8cc615ec248e616e2108effd2 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 1 May 2026 12:03:58 +0800 Subject: [PATCH 08/13] update default queue size in DebugDataPublisher to 5 --- rm_common/include/rm_common/DebugDataPublisher.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_common/include/rm_common/DebugDataPublisher.h b/rm_common/include/rm_common/DebugDataPublisher.h index 51412e0c..58282b2f 100644 --- a/rm_common/include/rm_common/DebugDataPublisher.h +++ b/rm_common/include/rm_common/DebugDataPublisher.h @@ -17,9 +17,9 @@ class DebugDataPublisher * @brief Construct and initialize the publisher. * @param nh ROS NodeHandle used for advertise. * @param topic Topic name, defaults to "debug_data". - * @param queue_size Publish queue size, defaults to 10. + * @param queue_size Publish queue size, defaults to 5. */ - explicit DebugDataPublisher(ros::NodeHandle& nh, const std::string& topic = "debug_data", int queue_size = 10) + explicit DebugDataPublisher(ros::NodeHandle& nh, const std::string& topic = "debug_data", int queue_size = 5) { debug_data_pub_ = nh.advertise(topic, queue_size); } From b60d28846fb5a0d2e26fc983d6a78215848a0f55 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 1 May 2026 12:08:06 +0800 Subject: [PATCH 09/13] Add overloaded clear method to KalmanFilter for custom covariance initialization --- .../include/rm_common/filters/kalman_filter.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/rm_common/include/rm_common/filters/kalman_filter.h b/rm_common/include/rm_common/filters/kalman_filter.h index 834fb232..8e01f02b 100644 --- a/rm_common/include/rm_common/filters/kalman_filter.h +++ b/rm_common/include/rm_common/filters/kalman_filter.h @@ -66,14 +66,24 @@ class KalmanFilter ~KalmanFilter() = default; + template + void clear(const Eigen::MatrixBase& x, const Eigen::MatrixBase& P0) + { + x_ = x; + inited = true; + K_ = DMat::Zero(n_, m_); + P_ = P0; + P_new_ = P0; + } + template void clear(const Eigen::MatrixBase& x) { x_ = x; inited = true; K_ = DMat::Zero(n_, m_); - P_ = DMat::Zero(n_, m_); - P_new_ = DMat::Zero(n_, n_); + P_.setIdentity(); + P_new_.setIdentity(); } template From 08dace3f604d7b7c651439b1b57f21034208786b Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 1 May 2026 12:30:30 +0800 Subject: [PATCH 10/13] fix: adjust shoot frequency return value when referee is offline --- rm_common/include/rm_common/decision/heat_limit.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 74738c8e..f42b55fe 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -147,12 +147,12 @@ class HeatLimit double getShootFrequency() const { std::lock_guard lock(heat_mutex_); + if (!referee_is_online_) + return 5.0; if (state_ == BURST) return shoot_frequency_; double shooter_cooling_heat = (use_local_heat_ || !referee_is_online_) ? local_shooter_cooling_heat_ : shooter_cooling_heat_; - if (!referee_is_online_) - return 5.0; if (shooter_cooling_limit_ - shooter_cooling_heat < bullet_heat_) return 0.0; else if (shooter_cooling_limit_ - shooter_cooling_heat == bullet_heat_) From 15228d78f109c92c069f13dd471490d65ba2aa3d Mon Sep 17 00:00:00 2001 From: CH <148587897+2194555@users.noreply.github.com> Date: Fri, 1 May 2026 16:00:42 +0800 Subject: [PATCH 11/13] Update capacity online check duration to 0.3 seconds --- rm_common/include/rm_common/decision/power_limit.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 3f8ec533..16c9c209 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -124,7 +124,7 @@ class PowerLimit } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(10); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } From ef22a072e42eaf026025aaa67b3022139474effb Mon Sep 17 00:00:00 2001 From: CH <148587897+2194555@users.noreply.github.com> Date: Fri, 1 May 2026 16:05:08 +0800 Subject: [PATCH 12/13] Simplify main loop and remove try-catch block Refactor main loop to simplify structure and remove exception handling. --- rm_referee/src/main.cpp | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/rm_referee/src/main.cpp b/rm_referee/src/main.cpp index 19933b29..b0d740f7 100644 --- a/rm_referee/src/main.cpp +++ b/rm_referee/src/main.cpp @@ -11,19 +11,12 @@ int main(int argc, char** argv) ros::NodeHandle nh("~"); rm_referee::Referee referee(nh); ros::Rate loop_rate(80); - try + while (ros::ok()) { - while (ros::ok()) - { - ros::spinOnce(); - referee.read(); - loop_rate.sleep(); - } - } - catch (const serial::SerialException& e) - { - ROS_ERROR_STREAM("Serial lost: " << e.what()); - referee.reconnect(); + ros::spinOnce(); + referee.read(); + loop_rate.sleep(); } + return 0; } From 58343580f22b9a44692cb260bdd2446e0cdc2378 Mon Sep 17 00:00:00 2001 From: CH <148587897+2194555@users.noreply.github.com> Date: Fri, 1 May 2026 16:14:36 +0800 Subject: [PATCH 13/13] Fix format. --- rm_referee/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_referee/src/main.cpp b/rm_referee/src/main.cpp index b0d740f7..ae2022d6 100644 --- a/rm_referee/src/main.cpp +++ b/rm_referee/src/main.cpp @@ -17,6 +17,6 @@ int main(int argc, char** argv) referee.read(); loop_rate.sleep(); } - + return 0; }