Update code

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_))
@@ -70,6 +72,8 @@ class HeatLimit
       ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("local_heat_protect_threshold", heat_protect_threshold_))
       ROS_ERROR("Local heat protect threshold no defined (namespace: %s)", nh.getNamespace().c_str());
+    if (!nh.getParam("buff_offset", buff_offset_))
+      ROS_ERROR("Buff offset no defined (namespace: %s)", nh.getNamespace().c_str());
     nh.param("use_local_heat", use_local_heat_, true);
     if (type_ == "ID1_42MM")
       bullet_heat_ = 100.;
@@ -79,6 +83,8 @@ class HeatLimit
     shoot_state_sub_ =
         nh.subscribe<rm_msgs::LocalHeatState>("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this);
     timer_ = nh.createTimer(ros::Duration(0.1), std::bind(&HeatLimit::timerCB, this));
+    fly_time_sub_ = nh.subscribe<std_msgs::Float64>("/controllers/gimbal_controller/bullet_solver/fly_time", 10,
+                                                    &HeatLimit::flyTimeCB, this);
   }
 
   typedef enum
@@ -113,6 +119,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 +144,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,11 +203,16 @@ class HeatLimit
     state_ = mode;
   }
 
-  bool getShootFrequencyMode() const
+  int getShootFrequencyMode() const
   {
     return state_;
   }
 
+  void flyTimeCB(const std_msgs::Float64ConstPtr& msg)
+  {
+    buff_shoot_frequency_ = 1 / (msg->data + buff_offset);
+  }
+
 private:
   void updateExpectShootFrequency()
   {
@@ -217,7 +233,8 @@ class HeatLimit
     }
     else if (state_ == HeatLimit::MINIMAL)
     {
-      shoot_frequency_ = minimal_shoot_frequency_;
+      shoot_frequency_ = buff_shoot_frequency_;
+      // shoot_frequency_ = minimal_shoot_frequency_;
       burst_flag_ = false;
     }
     else
@@ -231,15 +248,18 @@ 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_{};
+  double buff_shoot_frequency_{}, buff_offset_{};
   int once_shoot_num_{};
+  bool referee_status_change_{};
 
   ros::Publisher local_heat_pub_;
   ros::Subscriber shoot_state_sub_;
+  ros::Subscriber fly_time_sub_;
   ros::Timer timer_;
 
   rm_msgs::LocalHeatData local_heat_pub_data_;

rm_common/include/rm_common/traj_gen.h

@@ -199,3 +199,41 @@ class MinTimeTraj
     }
   }
 };
+
+template <typename T>
+class NonlinearTrackingDifferentiator
+{
+public:
+  NonlinearTrackingDifferentiator(T r, T h) : r_(r), h_(h)
+  {
+  }
+  void update(T v, T v_dot)
+  {
+    if ((v > 3. && x1_ < -3.) || (v < -3 && x1_ > 3))
+    {
+      x1_ = v;
+      x2_ = 0;
+    }
+    else
+    {
+      T y = x1_ - v + h_ * x2_;
+      T a0 = sqrt(h_ * h_ * r_ * r_ + 8 * r_ * fabs(y));
+      T a = x2_ + 0.5 * (a0 - h_ * r_) * (y > 0 ? 1 : -1);
+      T u = fabs(a) > h_ * r_ ? -r_ * (a > 0 ? 1 : -1) : -r_ * a / (h_ * r_);
+      x1_ = x1_ + h_ * (x2_ + v_dot);
+      x2_ = x2_ + h_ * u;
+    }
+  }
+  T getX1() const
+  {
+    return x1_;
+  }
+  T getX2() const
+  {
+    return x2_;
+  }
+
+private:
+  T r_{}, h_{};
+  T x1_{}, x2_{};
+};

rm_msgs/CMakeLists.txt

@@ -39,6 +39,7 @@ add_message_files(
         LocalHeatState.msg
         KalmanData.msg
         MovingAverageData.msg
+        MPCGimbalAimCmd.msg
         GpioData.msg
         TofRadarData.msg
         MultiDofCmd.msg

rm_msgs/msg/BulletSolverData.msg

@@ -1,14 +1,10 @@
-
 std_msgs/Header header
-float64 yaw_subtract_
-float64 output_yaw_central
-float64 test_number
-float64 after_fly_yaw_subtract_
-float64 switch_armor_angle
-float64 switche_time_yaw_
-float64 center_this_armor_
-float64 center_next_armor_
-float64 center_diagonal_armor_
-float64 center_triple_next_armor_
+int16[150] selected_armor_
+int16[150] count_number_
+float64 start_vel
+float64 switchtime
+float64 traject_output_yaw_
 float64 traject_acc_
-int16 selected_armor_
+float64 traject_vel_
+int16 using_traject_
+float64[100] test_number

rm_msgs/msg/MPCGimbalAimCmd.msg

@@ -0,0 +1,14 @@
+std_msgs/Header header
+bool valid
+uint8 target_id
+uint8 shoot_cmd
+float64 yaw
+float64 pitch
+float64 yaw_rate
+float64 pitch_rate
+float64 yaw_acc
+float64 pitch_acc
+float64 target_yaw
+float64 target_pitch
+float64 error
+geometry_msgs/Point selected_armor_pos

rm_msgs/msg/detection/TrackData.msg

@@ -1,5 +1,6 @@
 std_msgs/Header header
 uint8 id
+uint8 outpost_id
 bool tracking
 uint8 armors_num
 geometry_msgs/Point position
@@ -10,4 +11,6 @@ float64 v_yaw
 float64 radius_1
 float64 radius_2
 float64 dz
+float64 za
+float64 next_armor_z_diff
 float64 accel

rm_referee/include/rm_referee/ui/time_change_ui.h

@@ -3,7 +3,7 @@
 //
 
 #pragma once
-
+#include <std_msgs/Bool.h>
 #include "rm_referee/ui/ui_base.h"
 
 #include <rm_msgs/LeggedChassisStatus.h>
@@ -335,7 +335,18 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi
   void updateConfig() override;
   double target_distance_;
 };
+class EnemyColorTimeChangeUi : public TimeChangeUi
+{
+public:
+  explicit EnemyColorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                  std::deque<Graph>* character_queue)
+    : TimeChangeUi(rpc_value, base, "enemy_color", graph_queue, character_queue){};
+  void updateEnemyColorData(const std_msgs::Bool::ConstPtr& data);
 
+private:
+  void updateConfig() override;
+  int enemy_color_{};
+};
 class DeployDistanceTimeChangeUi : public TimeChangeUi
 {
 public:

rm_referee/src/ui/time_change_ui.cpp

@@ -599,6 +599,25 @@ void TargetDistanceTimeChangeUi::updateConfig()
 {
   graph_->setFloatNum(target_distance_);
 }
+void EnemyColorTimeChangeUi::updateEnemyColorData(const std_msgs::Bool::ConstPtr& data)
+{
+  enemy_color_ = data->data;
+  updateForQueue();
+}
+void EnemyColorTimeChangeUi::updateConfig()
+{
+  char temp_content[32];
+  snprintf(temp_content, sizeof(temp_content), "%s", enemy_color_ ? "BLUE" : "RED");
+  std::string enemy_color_content(temp_content);
+  graph_->setContent(enemy_color_content);
+  graph_->setColor(enemy_color_ ? rm_referee::GraphColor::CYAN : rm_referee::GraphColor::PINK);
+}
+
+void DeployDistanceTimeChangeUi::updateDeployDistanceData(const geometry_msgs::PointConstPtr& data)
+{
+  deploy_distance_ = sqrt(data->x * data->x + data->y * data->y);
+  updateForQueue();
+}
 
 void DeployDistanceTimeChangeUi::updateDeployDistanceData(const geometry_msgs::PointConstPtr& data)
 {