diff --git a/rm_common/include/rm_common/DebugDataPublisher.h b/rm_common/include/rm_common/DebugDataPublisher.h new file mode 100644 index 00000000..58282b2f --- /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 5. + */ + explicit DebugDataPublisher(ros::NodeHandle& nh, const std::string& topic = "debug_data", int queue_size = 5) + { + 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 376c9158..f42b55fe 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -147,6 +147,8 @@ 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 = diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 9d99796f..16c9c209 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_; 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 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 + + + + diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 3fb5b660..d41435cf 100755 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -20,6 +20,7 @@ add_message_files( ExchangerMsg.msg ChassisActiveSusCmd.msg ChassisCmd.msg + DebugData.msg ShootCmd.msg ShootState.msg ShootBeforehandCmd.msg 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 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 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"; }