Enhance UI and communication with recovery mode and debugging features

rm_common/include/rm_common/DebugDataPublisher.h

@@ -0,0 +1,112 @@
+//
+// Created by wk on 2026/3/2.
+//
+#pragma once
+#include <ros/ros.h>
+#include <rm_msgs/DebugData.h>
+#include <unordered_map>
+#include <string>
+
+/**
+ * @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 5.
+   */
+  explicit DebugDataPublisher(ros::NodeHandle& nh, const std::string& topic = "debug_data", int queue_size = 5)
+  {
+    debug_data_pub_ = nh.advertise<rm_msgs::DebugData>(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<std::string, size_t> index_map_;  ///< Name-to-index map for O(1) update.
+};

rm_common/include/rm_common/decision/heat_limit.h

@@ -147,6 +147,8 @@ class HeatLimit
   double getShootFrequency() const
   {
     std::lock_guard<std::mutex> lock(heat_mutex_);
+    if (!referee_is_online_)
+      return 5.0;
     if (state_ == BURST)
       return shoot_frequency_;
     double shooter_cooling_heat =

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_;

rm_common/include/rm_common/filters/kalman_filter.h

@@ -66,14 +66,24 @@ class KalmanFilter
 
   ~KalmanFilter() = default;
 
+  template <typename T1, typename T2>
+  void clear(const Eigen::MatrixBase<T1>& x, const Eigen::MatrixBase<T2>& P0)
+  {
+    x_ = x;
+    inited = true;
+    K_ = DMat<T>::Zero(n_, m_);
+    P_ = P0;
+    P_new_ = P0;
+  }
+
   template <typename T1>
   void clear(const Eigen::MatrixBase<T1>& x)
   {
     x_ = x;
     inited = true;
     K_ = DMat<T>::Zero(n_, m_);
-    P_ = DMat<T>::Zero(n_, m_);
-    P_new_ = DMat<T>::Zero(n_, n_);
+    P_.setIdentity();
+    P_new_.setIdentity();
   }
 
   template <typename T1>

rm_gazebo/launch/step_down.launch

@@ -0,0 +1,33 @@
+<launch>
+    <arg name="robot_type" default="$(env ROBOT_TYPE)" doc="Robot type [standard, auto, hero, engineer]"/>
+
+    <arg name="x_pos" default="0.0"/>
+    <arg name="y_pos" default="0.0"/>
+    <arg name="z_pos" default="0.0"/>
+
+    <arg name="load_chassis" default="true"/>
+    <arg name="load_gimbal" default="true"/>
+    <arg name="load_shooter" default="true"/>
+    <arg name="load_arm" default="true"/>
+    <arg name="paused" default="false"/>
+
+    <rosparam file="$(find rm_gazebo)/config/imus.yaml" command="load" if="$(arg load_gimbal)"/>
+    <param name="robot_description" command="$(find xacro)/xacro $(find rm_description)/urdf/$(arg robot_type)/$(arg robot_type).urdf.xacro
+         load_chassis:=$(arg load_chassis) load_gimbal:=$(arg load_gimbal) load_shooter:=$(arg load_shooter)
+         load_arm:=$(arg load_arm)
+         use_simulation:=true roller_type:=simple
+"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find rm_gazebo)/worlds/step_down.world"/>
+        <arg name="paused" value="false"/>
+        <arg name="use_sim_time" value="true"/>r
+        <arg name="gui" value="true"/>
+    </include>
+    23
+
+    <!-- push robot_description to factory an222322d spawn robot in gazebo -->
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" clear_params="true"
+          args="-x 3.5 -z 1.5 -param robot_description -urdf -model $(arg robot_type)" output="screen"/>
+
+</launch>