dzran1210 · Sep 10, 2026
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -551,10 +551,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double getWheelSpeedDes()
   {
     setSpeedDesAndWheelSpeedDes();
+    if (deploy_flag_)
+      return deploy_wheel_speed_;
     if (hero_flag_)
     {
-      if (deploy_flag_)
-        return deploy_wheel_speed_;
       return wheel_speed_des_;
     }
     return wheel_speed_des_ + total_extra_wheel_speed_;
@@ -805,7 +805,9 @@ class JointPositionBinaryCommandSender : public CommandSenderBase<std_msgs::Floa
 public:
   explicit JointPositionBinaryCommandSender(ros::NodeHandle& nh) : CommandSenderBase<std_msgs::Float64>(nh)
   {
-    ROS_ASSERT(nh.getParam("on_pos", on_pos_) && nh.getParam("off_pos", off_pos_));
+    // ROS_ASSERT(nh.getParam("on_pos", on_pos_) && nh.getParam("off_pos", off_pos_));
+    nh.getParam("on_pos", on_pos_);
+    nh.getParam("off_pos", off_pos_);
   }
   void on()
   {
@@ -821,6 +823,12 @@ class JointPositionBinaryCommandSender : public CommandSenderBase<std_msgs::Floa
   {
     current_position_ = msg_.data;
     change_position_ = current_position_ + scale * per_change_position_;
+    const double min_position = on_pos_ < off_pos_ ? on_pos_ : off_pos_;
+    const double max_position = on_pos_ > off_pos_ ? on_pos_ : off_pos_;
+    if (change_position_ < min_position)
+      change_position_ = min_position;
+    else if (change_position_ > max_position)
+      change_position_ = max_position;
     msg_.data = change_position_;
   }
   bool getState() const
@@ -835,7 +843,7 @@ class JointPositionBinaryCommandSender : public CommandSenderBase<std_msgs::Floa
 
 private:
   bool state{};
-  double on_pos_{}, off_pos_{}, current_position_{}, change_position_{}, per_change_position_{ 0.05 };
+  double on_pos_{}, off_pos_{}, current_position_{}, change_position_{}, per_change_position_{ 0.03 };
 };
 
 class CardCommandSender : public CommandSenderBase<std_msgs::Float64>

diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h
@@ -62,6 +62,8 @@ class HeatLimit
       ROS_ERROR("Burst shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("minimal_shoot_frequency", minimal_shoot_frequency_))
       ROS_ERROR("Minimal shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
+    if (!nh.getParam("outpost_shoot_frequency", outpost_shoot_frequency_))
+      ROS_ERROR("Outpost shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("safe_shoot_frequency", safe_shoot_frequency_))
       ROS_ERROR("Safe shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("heat_coeff", heat_coeff_))
@@ -113,6 +115,7 @@ class HeatLimit
     }
     local_heat_pub_data_.once_shoot_num = once_shoot_num_;
     local_heat_pub_data_.local_shooter_cooling_heat = local_shooter_cooling_heat_;
+    local_heat_pub_data_.desire_shoot_frequency = shoot_frequency_;
     local_heat_pub_.publish(local_heat_pub_data_);
   }
 
@@ -137,20 +140,24 @@ class HeatLimit
     {
       shooter_cooling_heat_ = data.shooter_id_1_42_mm_cooling_heat;
     }
+    referee_status_change_ = true;
   }
 
   void setRefereeStatus(bool status)
   {
     referee_is_online_ = status;
   }
 
-  double getShootFrequency() const
+  double getShootFrequency()
   {
     std::lock_guard<std::mutex> lock(heat_mutex_);
     if (!referee_is_online_)
       return 5.0;
-    if (state_ == BURST)
-      return shoot_frequency_;
+    if (referee_status_change_ && abs(local_shooter_cooling_heat_ - shooter_cooling_heat_) > 10)
+    {
+      local_shooter_cooling_heat_ = shooter_cooling_heat_;
+    }
+    referee_status_change_ = false;
     double shooter_cooling_heat =
         (use_local_heat_ || !referee_is_online_) ? local_shooter_cooling_heat_ : shooter_cooling_heat_;
     if (shooter_cooling_limit_ - shooter_cooling_heat < bullet_heat_)
@@ -192,7 +199,7 @@ class HeatLimit
     state_ = mode;
   }
 
-  bool getShootFrequencyMode() const
+  int getShootFrequencyMode() const
   {
     return state_;
   }
@@ -231,12 +238,13 @@ 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_{}, outpost_shoot_frequency_{};
 
   bool referee_is_online_, use_local_heat_, last_shoot_state_{};
   int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_;
   double local_shooter_cooling_heat_{}, heat_protect_threshold_{};
   int once_shoot_num_{};
+  bool referee_status_change_{};
 
   ros::Publisher local_heat_pub_;
   ros::Subscriber shoot_state_sub_;

diff --git a/rm_common/include/rm_common/decision/invincible_detect.h b/rm_common/include/rm_common/decision/invincible_detect.h
@@ -0,0 +1,54 @@
+//
+// Created by ray on 25-1-11.
+//
+
+#pragma once
+
+#include <ros/ros.h>
+#include <rm_msgs/GameStatus.h>
+#include <rm_msgs/GameRobotStatus.h>
+
+namespace rm_common
+{
+class InvincibleDetect
+{
+public:
+  InvincibleDetect(ros::NodeHandle& nh)
+  {
+    if (!nh.getParam("invincible_check", invincible_check_))
+      ROS_ERROR("Invincible_check no defined (namespace: %s)", nh.getNamespace().c_str());
+  }
+  typedef enum
+  {
+    INJURABLE = 0,
+    IN_SUPPLY_BASE = 1,
+    NORMAL_FRESHLY_RESURRECTED = 2,
+    MONEY_FRESHLY_RESURRECTED = 3,
+  } InvincibleState;
+
+  //  void updateEnemyStatus(int robot_id)
+  // 调用上面的update函数，需要输入敌方id，然后函数中直接更新对应id机器人的无敌状态
+  //  InvincibleState getInvincibleState(int robot_id)
+  // 用这个函数来get对应机器人的无敌状态，然后用这个状态做判断，如果不是INJURABLE，就把shooter设为READY
+
+private:
+  //  updateState()
+  void updateRobotInfo(int track_id, rm_msgs::GameRobotStatus& data)
+  {
+    robot_id = data.robot_id;
+    if (robot_id < 100)
+      track_id = track_id + 100;
+  }
+
+  void updateInvincibleState(int track_id, bool is_red)
+  {
+  }
+
+  int last_hp;
+  float last_revive_time;
+  float last_heal_time;
+  int robot_id;
+  InvincibleState invincible_state = INJURABLE;
+  XmlRpc::XmlRpcValue invincible_check_;
+};
+}  // namespace rm_common
diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h
@@ -39,6 +39,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <string>
 
 #include <ros/ros.h>
 #include <rm_msgs/ChassisCmd.h>
@@ -77,13 +78,8 @@ class PowerLimit
       ROS_ERROR("Gyro power no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("upstairs_power", upstairs_power_))
       ROS_ERROR("Upstairs power no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("max_power_limit", max_power_limit_))
-      ROS_ERROR("max power limit no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("power_gain", power_gain_))
-      ROS_ERROR("power gain no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("total_burst_time", total_burst_time_))
-      ROS_ERROR("total burst time no defined (namespace: %s)", nh.getNamespace().c_str());
-    default_max_power_limit_ = max_power_limit_;
+    if (!ros::param::get("/rm_manual/robot_type", robot_type_))
+      ROS_WARN("Only standard and hero robot types are supported (namespace: %s)", nh.getNamespace().c_str());
     default_burst_power_ = burst_power_;
   }
   typedef enum
@@ -97,45 +93,60 @@ class PowerLimit
 
   void updateSafetyPower(int safety_power)
   {
-    if (safety_power > 0)
+    if (robot_type_ == "standard")
+      safety_power_ = 40 + robot_id_ * 5;
+    else if (robot_type_ == "hero")
+      safety_power_ = 45 + robot_id_ * 5;
+    else
       safety_power_ = safety_power;
-    ROS_INFO("update safety power: %d", safety_power);
+    ROS_WARN_THROTTLE(2.0, "update safety power: %.0f", safety_power_);
+  }
+
+  void updateBurstPower(const double& burst_power_limit)
+  {
+    burst_power_ = burst_power_limit;
+    ROS_WARN("update burst power: %.0f", burst_power_);
   }
+
   void updateState(uint8_t state)
   {
     expect_state_ = state;
   }
+
   void setGameRobotData(const rm_msgs::GameRobotStatus data)
   {
+    robot_level_ = data.robot_level;
     robot_id_ = data.robot_id;
     chassis_power_limit_ = data.chassis_power_limit;
   }
+
   void setChassisPowerBuffer(const rm_msgs::PowerHeatData data)
   {
     chassis_power_buffer_ = data.chassis_power_buffer;
   }
+
   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(5);
     cap_energy_ = data.capacity_remain_charge;
     cap_state_ = data.state_machine_running_state;
   }
+
   void setRefereeStatus(bool status)
   {
     referee_is_online_ = status;
   }
+
   void setStartBurstTime(const ros::Time start_burst_time)
   {
     start_burst_time_ = start_burst_time;
   }
+
   ros::Time getStartBurstTime() const
   {
     return start_burst_time_;
   }
-  inline void setBurstPowerLimit(const double& burst_power_limit)
-  {
-    burst_power_ = burst_power_limit;
-  }
+
   uint8_t getState()
   {
     return expect_state_;
@@ -145,12 +156,10 @@ class PowerLimit
   {
     if (upstairs)
     {
-      max_power_limit_ = upstairs_power_;
       burst_power_ = upstairs_power_;
     }
     else
     {
-      max_power_limit_ = default_max_power_limit_;
       burst_power_ = default_burst_power_;
     }
   }
@@ -188,7 +197,7 @@ class PowerLimit
     if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER)
       chassis_cmd.power_limit = 400;
     else
-    {  // standard and hero
+    {
       if (referee_is_online_)
       {
         if (capacity_is_online_ && expect_state_ != ALLOFF)
@@ -218,7 +227,10 @@ class PowerLimit
           normal(chassis_cmd);
       }
       else
+      {
+        updateSafetyPower(safety_power_);
         chassis_cmd.power_limit = safety_power_;
+      }
     }
     applyPosturePowerScale(chassis_cmd);
   }
@@ -227,24 +239,19 @@ class PowerLimit
   void charge(rm_msgs::ChassisCmd& chassis_cmd)
   {
     allow_use_cap_ = false;
-    chassis_cmd.power_limit = chassis_power_limit_ * 0.70;
   }
 
   void normal(rm_msgs::ChassisCmd& chassis_cmd)
   {
     allow_use_cap_ = false;
-    if (cap_state_ != ALLOFF && cap_energy_ > disable_normal_cap_threshold_ &&
-        chassis_power_buffer_ > power_buffer_threshold_)
-    {
-      chassis_cmd.power_limit = chassis_power_limit_ + extra_power_;
-    }
-    else
+    if (chassis_power_limit_ == 65535)
     {
-      chassis_cmd.power_limit = chassis_power_limit_;
+      expect_state_ = BURST;  // Unlimit power limit in referee
+      return;
     }
-    if (chassis_cmd.power_limit > max_power_limit_)
+    if (cap_state_ != ALLOFF)
     {
-      chassis_cmd.power_limit = max_power_limit_;
+      chassis_cmd.power_limit = chassis_power_limit_ - 5;
     }
   }
 
@@ -255,16 +262,17 @@ class PowerLimit
 
   void burst(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro)
   {
+    if (chassis_power_limit_ != 65535 && (chassis_power_limit_ > burst_power_ || chassis_power_limit_ > gyro_power_))
+    {
+      chassis_cmd.power_limit = chassis_power_limit_;
+      return;
+    }
     if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_)
     {
       if (is_gyro)
-      {
         setGyroPower(chassis_cmd);
-      }
       else
-      {
         setBurstPower(chassis_cmd);
-      }
     }
     else
       expect_state_ = NORMAL;
@@ -279,9 +287,10 @@ class PowerLimit
 
   uint8_t expect_state_{}, cap_state_{};
 
+  std::string robot_type_{};
   int chassis_power_buffer_{};
-  int robot_id_{}, chassis_power_limit_{};
-  double max_power_limit_{ 70.0 };
+  int robot_id_{}, robot_level_{};
+  int chassis_power_limit_{};
   float cap_energy_{};
   double safety_power_{};
   double capacitor_threshold_{};
@@ -290,14 +299,12 @@ class PowerLimit
   double enable_gyro_cap_threshold_{}, disable_gyro_cap_threshold_{};
   double disable_normal_cap_threshold_{};
   double extra_power_{}, burst_power_{}, gyro_power_{}, upstairs_power_{};
-  double default_max_power_limit_{}, default_burst_power_{};
-  double power_gain_{};
+  double default_burst_power_{};
 
   bool allow_gyro_cap_{ false }, allow_use_cap_{ false };
   double posture_power_scale_{ 1.0 };
 
   ros::Time start_burst_time_{};
-  int total_burst_time_{};
 
   bool referee_is_online_{ false };
   bool capacity_is_online_{ false };