From 23317d12a13e6b42259ce5562493b7448a2d0ade Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 29 Dec 2025 17:00:31 +0800 Subject: [PATCH 01/11] Feat: add hybrid_joint_interface. --- .../hybrid_joint_interface.h | 116 ++++++++++++++++++ rm_gazebo/include/rm_gazebo/rm_robot_hw_sim.h | 21 ++++ rm_gazebo/src/rm_robot_hw_sim.cpp | 48 ++++++++ 3 files changed, 185 insertions(+) create mode 100644 rm_common/include/rm_common/hardware_interface/hybrid_joint_interface.h diff --git a/rm_common/include/rm_common/hardware_interface/hybrid_joint_interface.h b/rm_common/include/rm_common/hardware_interface/hybrid_joint_interface.h new file mode 100644 index 00000000..90663053 --- /dev/null +++ b/rm_common/include/rm_common/hardware_interface/hybrid_joint_interface.h @@ -0,0 +1,116 @@ +// +// Created by qiayuan on 2021/11/5. +// +#pragma once +#include +#include + +namespace rm_control +{ +class HybridJointHandle : public hardware_interface::JointStateHandle +{ +public: + HybridJointHandle() = default; + + HybridJointHandle(const JointStateHandle& js, double* posDes, double* velDes, double* kp, double* kd, double* ff) + : JointStateHandle(js), posDes_(posDes), velDes_(velDes), kp_(kp), kd_(kd), ff_(ff) + { + if (posDes_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Position desired data pointer is null."); + } + if (velDes_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Velocity desired data pointer is null."); + } + if (kp_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Kp data pointer is null."); + } + if (kd_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Kd data pointer is null."); + } + if (ff_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Feedforward data pointer is null."); + } + } + void setPositionDesired(double cmd) + { + assert(posDes_); + *posDes_ = cmd; + } + void setVelocityDesired(double cmd) + { + assert(velDes_); + *velDes_ = cmd; + } + void setKp(double cmd) + { + assert(kp_); + *kp_ = cmd; + } + void setKd(double cmd) + { + assert(kd_); + *kd_ = cmd; + } + void setFeedforward(double cmd) + { + assert(ff_); + *ff_ = cmd; + } + void setCommand(double pos_des, double vel_des, double kp, double kd, double ff) + { + setPositionDesired(pos_des); + setVelocityDesired(vel_des); + setKp(kp); + setKd(kd); + setFeedforward(ff); + } + double getPositionDesired() + { + assert(posDes_); + return *posDes_; + } + double getVelocityDesired() + { + assert(velDes_); + return *velDes_; + } + double getKp() + { + assert(kp_); + return *kp_; + } + double getKd() + { + assert(kd_); + return *kd_; + } + double getFeedforward() + { + assert(ff_); + return *ff_; + } + +private: + double* posDes_ = { nullptr }; + double* velDes_ = { nullptr }; + double* kp_ = { nullptr }; + double* kd_ = { nullptr }; + double* ff_ = { nullptr }; +}; + +class HybridJointInterface + : public hardware_interface::HardwareResourceManager +{ +}; + +} // namespace rm_control diff --git a/rm_gazebo/include/rm_gazebo/rm_robot_hw_sim.h b/rm_gazebo/include/rm_gazebo/rm_robot_hw_sim.h index 915f0375..c4d6c3c1 100644 --- a/rm_gazebo/include/rm_gazebo/rm_robot_hw_sim.h +++ b/rm_gazebo/include/rm_gazebo/rm_robot_hw_sim.h @@ -40,10 +40,23 @@ #include #include #include +#include #include namespace rm_gazebo { +struct HybridJointData +{ + hardware_interface::JointHandle joint_; + double posDes_{}, velDes_{}, kp_{}, kd_{}, ff_{}; +}; + +struct HybridJointCommand +{ + ros::Time stamp_; + double posDes_{}, velDes_{}, kp_{}, kd_{}, ff_{}; +}; + struct ImuData { gazebo::physics::LinkPtr link_ptr; @@ -63,6 +76,7 @@ class RmRobotHWSim : public gazebo_ros_control::DefaultRobotHWSim const urdf::Model* urdf_model, std::vector transmissions) override; void readSim(ros::Time time, ros::Duration period) override; + void writeSim(ros::Time time, ros::Duration period) override; private: void parseImu(XmlRpc::XmlRpcValue& imu_datas, const gazebo::physics::ModelPtr& parent_model); @@ -75,13 +89,20 @@ class RmRobotHWSim : public gazebo_ros_control::DefaultRobotHWSim res.message = "Imu status: " + imu_status_message; return true; } + + rm_control::HybridJointInterface hybridJointInterface_; rm_control::RobotStateInterface robot_state_interface_; hardware_interface::ImuSensorInterface imu_sensor_interface_; rm_control::RmImuSensorInterface rm_imu_sensor_interface_; gazebo::physics::WorldPtr world_; std::list imu_datas_; ros::ServiceServer switch_imu_service_; + + std::list hybridJointDatas_; + std::unordered_map > cmdBuffer_; + static bool disable_imu_; + double delay_{}; }; } // namespace rm_gazebo diff --git a/rm_gazebo/src/rm_robot_hw_sim.cpp b/rm_gazebo/src/rm_robot_hw_sim.cpp index 8012cb5f..8ba01dc9 100644 --- a/rm_gazebo/src/rm_robot_hw_sim.cpp +++ b/rm_gazebo/src/rm_robot_hw_sim.cpp @@ -46,6 +46,18 @@ bool RmRobotHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle m std::vector transmissions) { bool ret = DefaultRobotHWSim::initSim(robot_namespace, model_nh, parent_model, urdf_model, transmissions); + // Joint interface + registerInterface(&hybridJointInterface_); + std::vector names = ej_interface_.getNames(); + for (const auto& name : names) + { + hybridJointDatas_.push_back(HybridJointData{ .joint_ = ej_interface_.getHandle(name) }); + HybridJointData& back = hybridJointDatas_.back(); + hybridJointInterface_.registerHandle( + rm_control::HybridJointHandle(back.joint_, &back.posDes_, &back.velDes_, &back.kp_, &back.kd_, &back.ff_)); + cmdBuffer_.insert(std::make_pair(name.c_str(), std::deque())); + } + gazebo_ros_control::DefaultRobotHWSim::registerInterface(&robot_state_interface_); gazebo_ros_control::DefaultRobotHWSim::registerInterface(&imu_sensor_interface_); gazebo_ros_control::DefaultRobotHWSim::registerInterface(&rm_imu_sensor_interface_); @@ -92,6 +104,42 @@ void RmRobotHWSim::readSim(ros::Time time, ros::Duration period) cmd = 0; for (auto& cmd : joint_velocity_command_) cmd = 0; + for (auto& joint : hybridJointDatas_) + { + joint.posDes_ = joint.joint_.getPosition(); + joint.velDes_ = joint.joint_.getVelocity(); + joint.kp_ = 0.; + joint.kd_ = 0.; + joint.ff_ = 0.; + } +} + +void RmRobotHWSim::writeSim(ros::Time time, ros::Duration period) +{ + for (auto joint : hybridJointDatas_) + { + auto& buffer = cmdBuffer_.find(joint.joint_.getName())->second; + if (time == ros::Time(period.toSec())) + { // Simulation reset + buffer.clear(); + } + + while (!buffer.empty() && buffer.back().stamp_ + ros::Duration(delay_) < time) + { + buffer.pop_back(); + } + buffer.push_front(HybridJointCommand{ .stamp_ = time, + .posDes_ = joint.posDes_, + .velDes_ = joint.velDes_, + .kp_ = joint.kp_, + .kd_ = joint.kd_, + .ff_ = joint.ff_ }); + + const auto& cmd = buffer.back(); + joint.joint_.setCommand(cmd.kp_ * (cmd.posDes_ - joint.joint_.getPosition()) + + cmd.kd_ * (cmd.velDes_ - joint.joint_.getVelocity()) + cmd.ff_); + } + DefaultRobotHWSim::writeSim(time, period); } void RmRobotHWSim::parseImu(XmlRpc::XmlRpcValue& imu_datas, const gazebo::physics::ModelPtr& parent_model) From 782872c44d20904cefc7b53e2a2b168dfb38d5f8 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Thu, 1 Jan 2026 23:31:46 +0800 Subject: [PATCH 02/11] Fix: fix effort joint interface can't use in gazebo. --- rm_gazebo/src/rm_robot_hw_sim.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rm_gazebo/src/rm_robot_hw_sim.cpp b/rm_gazebo/src/rm_robot_hw_sim.cpp index 8ba01dc9..b8f1c7ee 100644 --- a/rm_gazebo/src/rm_robot_hw_sim.cpp +++ b/rm_gazebo/src/rm_robot_hw_sim.cpp @@ -136,8 +136,9 @@ void RmRobotHWSim::writeSim(ros::Time time, ros::Duration period) .ff_ = joint.ff_ }); const auto& cmd = buffer.back(); - joint.joint_.setCommand(cmd.kp_ * (cmd.posDes_ - joint.joint_.getPosition()) + - cmd.kd_ * (cmd.velDes_ - joint.joint_.getVelocity()) + cmd.ff_); + if (joint.joint_.getCommand() == 0.0f) + joint.joint_.setCommand(cmd.kp_ * (cmd.posDes_ - joint.joint_.getPosition()) + + cmd.kd_ * (cmd.velDes_ - joint.joint_.getVelocity()) + cmd.ff_); } DefaultRobotHWSim::writeSim(time, period); } From 4f896d1a12767cd29c4b365a4e42b0520d649be4 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 3 Jan 2026 12:22:09 +0800 Subject: [PATCH 03/11] Feat: add hybrid_joint_limits. --- .../hybrid_joint_limits_interface.h | 334 ++++++++++++++++++ 1 file changed, 334 insertions(+) create mode 100644 rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h diff --git a/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h b/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h new file mode 100644 index 00000000..ee3ff364 --- /dev/null +++ b/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h @@ -0,0 +1,334 @@ +// +// Created by wk on 2026/1/3. +// + +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +#include + +namespace rm_control +{ +class HybridJointSaturationHandle +{ +public: + HybridJointSaturationHandle(const rm_control::HybridJointHandle& hjh, + const joint_limits_interface::JointLimits& limits) + : hjh_(hjh), limits_(limits) + { + if (!limits.has_velocity_limits) + { + throw joint_limits_interface::JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) + { + throw joint_limits_interface::JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() + + "'. It has no efforts limits specification."); + } + + if (limits_.has_position_limits) + { + min_pos_limit_ = limits_.min_position; + max_pos_limit_ = limits_.max_position; + } + else + { + min_pos_limit_ = -std::numeric_limits::max(); + max_pos_limit_ = std::numeric_limits::max(); + } + } + + /** \return Joint name. */ + std::string getName() const + { + return hjh_.getName(); + } + + /** + * \brief Enforce position, velocity, and effort limits for a joint that is not subject to soft limits. + */ + void enforceLimits(const ros::Duration& period) + { + using joint_limits_interface::internal::saturate; + double min_eff = -limits_.max_effort; + double max_eff = limits_.max_effort; + + if (limits_.has_position_limits) + { + const double pos = hjh_.getPosition(); + if (pos < limits_.min_position) + min_eff = 0.0; + else if (pos > limits_.max_position) + max_eff = 0.0; + } + + const double vel = hjh_.getVelocity(); + if (vel < -limits_.max_velocity) + min_eff = 0.0; + else if (vel > limits_.max_velocity) + max_eff = 0.0; + + if (std::isnan(prev_pos_cmd_)) + prev_pos_cmd_ = hjh_.getPosition(); + + double min_pos, max_pos; + if (limits_.has_velocity_limits) + { + const double delta_pos = limits_.max_velocity * period.toSec(); + min_pos = std::max(prev_pos_cmd_ - delta_pos, min_pos_limit_); + max_pos = std::min(prev_pos_cmd_ + delta_pos, max_pos_limit_); + } + else + { + min_pos = min_pos_limit_; + max_pos = max_pos_limit_; + } + + const double pos_cmd = saturate(hjh_.getPositionDesired(), min_pos, max_pos); + prev_pos_cmd_ = pos_cmd; + + // Velocity bounds + double vel_low{}; + double vel_high{}; + + if (limits_.has_acceleration_limits) + { + assert(period.toSec() > 0.0); + const double dt = period.toSec(); + + vel_low = std::max(prev_vel_cmd_ - limits_.max_acceleration * dt, -limits_.max_velocity); + vel_high = std::min(prev_vel_cmd_ + limits_.max_acceleration * dt, limits_.max_velocity); + } + else + { + vel_low = -limits_.max_velocity; + vel_high = limits_.max_velocity; + } + + // Saturate velocity command according to limits + const double vel_cmd = saturate(hjh_.getVelocityDesired(), vel_low, vel_high); + prev_vel_cmd_ = vel_cmd; + + hjh_.setCommand(pos_cmd, vel_cmd, hjh_.getKp(), hjh_.getKd(), saturate(hjh_.getFeedforward(), min_eff, max_eff)); + } + + /** + * \brief Reset state, in case of mode switch or e-stop + */ + void reset() + { + prev_pos_cmd_ = std::numeric_limits::quiet_NaN(); + } + +private: + rm_control::HybridJointHandle hjh_; + joint_limits_interface::JointLimits limits_; + + double min_pos_limit_, max_pos_limit_; + + double prev_pos_cmd_ = { std::numeric_limits::quiet_NaN() }; + double prev_vel_cmd_ = { std::numeric_limits::quiet_NaN() }; +}; + +class HybridJointSoftLimitsHandle +{ +public: + HybridJointSoftLimitsHandle() = default; + + HybridJointSoftLimitsHandle(const rm_control::HybridJointHandle& hjh, + const joint_limits_interface::JointLimits& limits, + const joint_limits_interface::SoftJointLimits& soft_limits) + : hjh_(hjh), limits_(limits), soft_limits_(soft_limits) + { + if (!limits.has_velocity_limits) + { + throw joint_limits_interface::JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() + + "'. It has no velocity limits specification."); + } + if (!limits.has_effort_limits) + { + throw joint_limits_interface::JointLimitsInterfaceException("Cannot enforce limits for joint '" + getName() + + "'. It has no effort limits specification."); + } + } + + /** \return Joint name. */ + std::string getName() const + { + return hjh_.getName(); + } + + /** + * \brief Enforce position, velocity and effort limits for a joint subject to soft limits. + * + * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits will be enforced. + */ + void enforceLimits(const ros::Duration& period) + { + using joint_limits_interface::internal::saturate; + + // Effort Interface limits + // Current state + double pos = hjh_.getPosition(); + double vel = hjh_.getVelocity(); + + // Velocity bounds + double soft_min_vel{}; + double soft_max_vel{}; + + if (limits_.has_position_limits) + { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, + limits_.max_velocity); + } + else + { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Effort bounds depend on the velocity and effort bounds + const double soft_min_eff = + saturate(-soft_limits_.k_velocity * (vel - soft_min_vel), -limits_.max_effort, limits_.max_effort); + + const double soft_max_eff = + saturate(-soft_limits_.k_velocity * (vel - soft_max_vel), -limits_.max_effort, limits_.max_effort); + + // Saturate effort command according to bounds + const double eff_cmd = saturate(hjh_.getFeedforward(), soft_min_eff, soft_max_eff); + + assert(period.toSec() > 0.0); + // Current position + if (std::isnan(prev_pos_cmd_)) + { + prev_pos_cmd_ = hjh_.getPosition(); + } // Happens only once at initialization + pos = prev_pos_cmd_; + + if (limits_.has_position_limits) + { + // Velocity bounds depend on the velocity limit and the proximity to the position limit + soft_min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, + limits_.max_velocity); + } + else + { + // No position limits, eg. continuous joints + soft_min_vel = -limits_.max_velocity; + soft_max_vel = limits_.max_velocity; + } + + // Position Interface limits + // Position bounds + const double dt = period.toSec(); + double pos_low = pos + soft_min_vel * dt; + double pos_high = pos + soft_max_vel * dt; + + if (limits_.has_position_limits) + { + // This extra measure safeguards against pathological cases, like when the soft limit lies beyond the hard limit + pos_low = std::max(pos_low, limits_.min_position); + pos_high = std::min(pos_high, limits_.max_position); + } + + // Saturate position command according to bounds + const double pos_cmd = saturate(hjh_.getPositionDesired(), pos_low, pos_high); + + // Cache variables + prev_pos_cmd_ = pos_cmd; + + // Velocity Interface limits + double min_vel{}, max_vel{}; + if (limits_.has_position_limits) + { + // Velocity bounds depend on the velocity limit and the proximity to the position limit. + pos = hjh_.getPosition(); + min_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, max_vel_limit_); + max_vel = saturate(-soft_limits_.k_position * (pos - soft_limits_.max_position), -max_vel_limit_, max_vel_limit_); + } + else + { + min_vel = -max_vel_limit_; + max_vel = max_vel_limit_; + } + + if (limits_.has_acceleration_limits) + { + vel = hjh_.getVelocity(); + const double delta_t = period.toSec(); + min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); + max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); + } + + hjh_.setCommand(pos_cmd, saturate(hjh_.getVelocityDesired(), min_vel, max_vel), hjh_.getKp(), hjh_.getKd(), eff_cmd); + } + + /** + * \brief Reset state, in case of mode switch or e-stop + */ + void reset() + { + prev_pos_cmd_ = std::numeric_limits::quiet_NaN(); + } + +private: + rm_control::HybridJointHandle hjh_; + joint_limits_interface::JointLimits limits_; + joint_limits_interface::SoftJointLimits soft_limits_; + + double prev_pos_cmd_ = { std::numeric_limits::quiet_NaN() }; + double max_vel_limit_; +}; + +/** Interface for enforcing limits on an effort-controlled joint through saturation. */ +class HybridJointSaturationInterface : public joint_limits_interface::JointLimitsInterface +{ +public: + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Reset all managed handles. */ + void reset() + { + for (auto&& resource_name_and_handle : this->resource_map_) + { + resource_name_and_handle.second.reset(); + } + } +}; +class HybridJointSoftLimitsInterface : public joint_limits_interface::JointLimitsInterface +{ +public: + /** \name Real-Time Safe Functions + *\{*/ + /** \brief Reset all managed handles. */ + void reset() + { + for (auto&& resource_name_and_handle : this->resource_map_) + { + resource_name_and_handle.second.reset(); + } + } +}; +} // namespace rm_control From 28dc7a0c68c31a8d16ec3d2f8067dc90b7291dbf Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 3 Jan 2026 16:02:37 +0800 Subject: [PATCH 04/11] Style: optimize variable snitialize style. --- .../joint_limits_interface/hybrid_joint_limits_interface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h b/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h index ee3ff364..72f6ac17 100644 --- a/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h +++ b/rm_common/include/rm_common/joint_limits_interface/hybrid_joint_limits_interface.h @@ -299,7 +299,7 @@ class HybridJointSoftLimitsHandle joint_limits_interface::SoftJointLimits soft_limits_; double prev_pos_cmd_ = { std::numeric_limits::quiet_NaN() }; - double max_vel_limit_; + double max_vel_limit_{}; }; /** Interface for enforcing limits on an effort-controlled joint through saturation. */ From 73c089365ad38c1e7edef08927a432bb9d87ffb5 Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Mon, 4 May 2026 16:06:58 +0800 Subject: [PATCH 05/11] Modify power limit logic. --- .../include/rm_common/decision/power_limit.h | 143 ++++++++++-------- 1 file changed, 76 insertions(+), 67 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 16c9c209..7a9e30a6 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -59,32 +59,32 @@ class PowerLimit ROS_ERROR("Safety power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("capacitor_threshold", capacitor_threshold_)) ROS_ERROR("Capacitor threshold no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("disable_cap_gyro_threshold", disable_cap_gyro_threshold_)) - 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("disable_burst_cap_threshold", disable_burst_cap_threshold_)) + ROS_ERROR("Disable burst cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("enable_burst_cap_threshold", enable_burst_cap_threshold_)) + ROS_ERROR("Enable burst cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("disable_normal_cap_threshold", disable_normal_cap_threshold_)) + ROS_ERROR("Disable normal cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("enable_gyro_cap_threshold", enable_gyro_cap_threshold_)) + ROS_ERROR("Enable gyro cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("disable_gyro_cap_threshold", disable_gyro_cap_threshold_)) + ROS_ERROR("Disable gyro cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("extra_power", extra_power_)) ROS_ERROR("Extra power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("burst_power", burst_power_)) ROS_ERROR("Burst power no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("standard_power", standard_power_)) - ROS_ERROR("Standard power no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("gyro_power", gyro_power_)) + 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("buffer_threshold", buffer_threshold_)) - ROS_ERROR("buffer threshold no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("is_new_capacitor", is_new_capacitor_)) - ROS_ERROR("is_new_capacitor 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_; + default_burst_power_ = burst_power_; } typedef enum { @@ -103,14 +103,7 @@ class PowerLimit } void updateState(uint8_t state) { - if (!capacitor_is_on_) - expect_state_ = ALLOFF; - else - expect_state_ = state; - } - void updateCapSwitchState(bool state) - { - capacitor_is_on_ = state; + expect_state_ = state; } void setGameRobotData(const rm_msgs::GameRobotStatus data) { @@ -120,7 +113,6 @@ class PowerLimit void setChassisPowerBuffer(const rm_msgs::PowerHeatData data) { chassis_power_buffer_ = data.chassis_power_buffer; - power_buffer_threshold_ = chassis_power_buffer_ * 0.8; } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { @@ -136,10 +128,6 @@ 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_; @@ -148,35 +136,49 @@ class PowerLimit { return expect_state_; } + + void setUpstairsPower(bool upstairs) + { + if (upstairs) + { + max_power_limit_ = upstairs_power_; + burst_power_ = upstairs_power_; + } + else + { + max_power_limit_ = default_max_power_limit_; + burst_power_ = default_burst_power_; + } + } + void setGyroPower(rm_msgs::ChassisCmd& chassis_cmd) { - if (!allow_gyro_cap_ && cap_energy_ >= enable_cap_gyro_threshold_) + if (!allow_gyro_cap_ && cap_energy_ >= enable_gyro_cap_threshold_) allow_gyro_cap_ = true; - if (allow_gyro_cap_ && cap_energy_ <= disable_cap_gyro_threshold_) + if (allow_gyro_cap_ && cap_energy_ <= disable_gyro_cap_threshold_) allow_gyro_cap_ = false; - if (allow_gyro_cap_ && chassis_power_limit_ < 80) - chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; + if (allow_gyro_cap_) + { + chassis_cmd.power_limit = gyro_power_; + } else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } + void setBurstPower(rm_msgs::ChassisCmd& chassis_cmd) { - if (!allow_use_cap_ && cap_energy_ >= enable_use_cap_threshold_) + if (!allow_use_cap_ && cap_energy_ >= enable_burst_cap_threshold_) allow_use_cap_ = true; - if (allow_use_cap_ && cap_energy_ <= disable_use_cap_threshold_) + if (allow_use_cap_ && cap_energy_ <= disable_burst_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_; + chassis_cmd.power_limit = burst_power_; } else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } + void setLimitPower(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER) @@ -191,7 +193,7 @@ class PowerLimit chassis_cmd.power_limit = burst_power_; else { - switch (is_new_capacitor_ ? expect_state_ : cap_state_) + switch (expect_state_) { case NORMAL: normal(chassis_cmd); @@ -223,39 +225,45 @@ class PowerLimit allow_use_cap_ = false; chassis_cmd.power_limit = chassis_power_limit_ * 0.70; } + void normal(rm_msgs::ChassisCmd& chassis_cmd) { - if (is_new_capacitor_) + 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 { chassis_cmd.power_limit = chassis_power_limit_; - return; } - allow_use_cap_ = false; - double buffer_energy_error = chassis_power_buffer_ - buffer_threshold_; - double plus_power = buffer_energy_error * power_gain_; - chassis_cmd.power_limit = chassis_power_limit_ + plus_power; - // TODO:Add protection when buffer<5 if (chassis_cmd.power_limit > max_power_limit_) + { chassis_cmd.power_limit = max_power_limit_; + } } + void zero(rm_msgs::ChassisCmd& chassis_cmd) { chassis_cmd.power_limit = 0.0; } + void burst(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) { - if (is_new_capacitor_) - chassis_cmd.power_limit = burst_power_; - else if (is_gyro) + if (is_gyro) + { setGyroPower(chassis_cmd); + } else + { setBurstPower(chassis_cmd); + } } else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } void applyPosturePowerScale(rm_msgs::ChassisCmd& chassis_cmd) const @@ -265,21 +273,22 @@ class PowerLimit chassis_cmd.power_limit = std::max(0.0, std::floor(chassis_cmd.power_limit * posture_power_scale_)); } - int chassis_power_buffer_; - int robot_id_, chassis_power_limit_; - int max_power_limit_{ 70 }; - float cap_energy_; + uint8_t expect_state_{}, cap_state_{}; + + int chassis_power_buffer_{}; + int robot_id_{}, chassis_power_limit_{}; + double max_power_limit_{ 70.0 }; + float cap_energy_{}; double safety_power_{}; double capacitor_threshold_{}; double power_buffer_threshold_{ 50.0 }; - double enable_cap_gyro_threshold_{}, disable_cap_gyro_threshold_{}, enable_use_cap_threshold_{}, - disable_use_cap_threshold_{}; - double charge_power_{}, extra_power_{}, burst_power_{}, standard_power_{}; - double buffer_threshold_{}; + double enable_burst_cap_threshold_{}, disable_burst_cap_threshold_{}; + 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_{}; - bool is_new_capacitor_{ false }; - uint8_t expect_state_{}, cap_state_{}; - bool capacitor_is_on_{ true }; + bool allow_gyro_cap_{ false }, allow_use_cap_{ false }; double posture_power_scale_{ 1.0 }; From 47b89ff27937872d291b00eac6521b22cf4b2a53 Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Mon, 4 May 2026 16:13:34 +0800 Subject: [PATCH 06/11] Update msg. --- rm_msgs/msg/ChassisActiveSusCmd.msg | 1 + rm_msgs/msg/ShootCmd.msg | 1 + 2 files changed, 2 insertions(+) diff --git a/rm_msgs/msg/ChassisActiveSusCmd.msg b/rm_msgs/msg/ChassisActiveSusCmd.msg index 95fd2aa0..09e8824c 100644 --- a/rm_msgs/msg/ChassisActiveSusCmd.msg +++ b/rm_msgs/msg/ChassisActiveSusCmd.msg @@ -4,3 +4,4 @@ uint8 UP = 2 time stamp uint8 mode +int32 feedforward_countdown diff --git a/rm_msgs/msg/ShootCmd.msg b/rm_msgs/msg/ShootCmd.msg index a4985032..278af3dd 100644 --- a/rm_msgs/msg/ShootCmd.msg +++ b/rm_msgs/msg/ShootCmd.msg @@ -10,6 +10,7 @@ uint8 SPEED_18M_PER_SECOND = 4 uint8 SPEED_30M_PER_SECOND = 5 uint8 mode +bool allow_deploy_fire float64 wheel_speed float64 hz float64 wheels_speed_offset_front From 632e12d104a0a35a22fcefde880b3a12b471ae2b Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Mon, 4 May 2026 16:18:17 +0800 Subject: [PATCH 07/11] Modify hero chassis and shooter logic in command sender. --- .../rm_common/decision/command_sender.h | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index f3399e71..ddb0ab2f 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -306,11 +306,31 @@ class ChassisActiveSuspensionCommandSender : public TimeStampCommandSenderBase(nh) { + nh.param("leg_feedforward_duration", leg_feedforward_duration_, 20.0); + } + void setLegSwitchTime(ros::Time leg_switch_time) + { + leg_switch_time_ = leg_switch_time; + } + int getLegMode() + { + return msg_.mode; + } + void sendCommand(const ros::Time& time) override + { + msg_.feedforward_countdown = + std::max(0, static_cast(leg_feedforward_duration_ - (ros::Time::now() - leg_switch_time_).toSec())); + TimeStampCommandSenderBase::sendCommand(time); } void setZero() override { msg_.mode = 0; + msg_.feedforward_countdown = 0; } + +private: + double leg_feedforward_duration_{}; + ros::Time leg_switch_time_{}; }; class GimbalCommandSender : public TimeStampCommandSenderBase { @@ -516,6 +536,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Mon, 4 May 2026 16:40:02 +0800 Subject: [PATCH 08/11] Modify UI logic for hero. --- rm_referee/include/rm_referee/common/data.h | 1 + rm_referee/include/rm_referee/ui/flash_ui.h | 4 +- .../include/rm_referee/ui/time_change_ui.h | 43 ++++++++++++++ .../include/rm_referee/ui/trigger_change_ui.h | 19 +++++++ rm_referee/src/referee_base.cpp | 55 ++++++++++++++++-- rm_referee/src/ui/flash_ui.cpp | 34 +++++++++-- rm_referee/src/ui/time_change_ui.cpp | 56 +++++++++++++++++++ rm_referee/src/ui/trigger_change_ui.cpp | 32 +++++++++++ 8 files changed, 232 insertions(+), 12 deletions(-) diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index d1545616..83e162e6 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 4c9fe4c5..a61a592c 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -97,13 +97,15 @@ class DeployFlashUi : public FlashUi explicit DeployFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, std::deque* character_queue) : FlashUi(rpc_value, base, "deploy", graph_queue, character_queue){}; - void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data, const ros::Time& last_get_data_time); + void updateAllowDeployFire(bool flag); + void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data); void updateChassisVelData(const geometry_msgs::Twist::ConstPtr& data); private: void display(const ros::Time& time) override; uint8_t chassis_mode_; double angular_z_{ 0. }; + bool allow_deploy_fire_{ false }; }; class HeroHitFlashUi : public FlashGroupUi diff --git a/rm_referee/include/rm_referee/ui/time_change_ui.h b/rm_referee/include/rm_referee/ui/time_change_ui.h index bc512091..b66d9edf 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -53,6 +53,22 @@ class CapacitorTimeChangeUi : public TimeChangeUi double remain_charge_; }; +class RelocalizeProgressTimeChangeUi : public TimeChangeUi +{ +public: + explicit RelocalizeProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "relocalize", graph_queue, character_queue) + { + } + void add() override; + void updateRelocalizeProgress(const double data, const ros::Time& time); + +private: + void updateConfig() override; + double relocalize_progress_; +}; + class EffortTimeChangeUi : public TimeChangeUi { public: @@ -316,6 +332,33 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi double target_distance_; }; +class DeployDistanceTimeChangeUi : public TimeChangeUi +{ +public: + explicit DeployDistanceTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "deploy_distance", graph_queue, character_queue){}; + void updateDeployDistanceData(const geometry_msgs::PointConstPtr& data); + +private: + void updateConfig() override; + double deploy_distance_{}; +}; + +class HeroLegTimeChangeUi : public TimeChangeUi +{ +public: + explicit HeroLegTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "hero_leg_feedforward_countdown", graph_queue, character_queue) + { + } + void updateFeedforwardCountdown(int feedforward_countdown); + +private: + void updateConfig() override; + int feedforward_countdown_{}; +}; class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi { public: diff --git a/rm_referee/include/rm_referee/ui/trigger_change_ui.h b/rm_referee/include/rm_referee/ui/trigger_change_ui.h index b534acd6..4c7d3342 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -78,6 +78,25 @@ class ChassisTriggerChangeUi : public TriggerChangeUi double mode_change_threshold_; }; +class HeroLegTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit HeroLegTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "hero_leg_mode", graph_queue, character_queue) + { + graph_->setContent("DOWN"); + graph_->setColor(rm_referee::GraphColor::YELLOW); + } + void updateMode(uint8_t mode); + +private: + void update() override; + void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false) override; + std::string getHeroLegState(uint8_t mode); + uint8_t leg_mode_; +}; + class ShooterTriggerChangeUi : public TriggerChangeUi { public: diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index ccbc3a64..7de785a6 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -15,6 +15,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::actuator_state_sub_ = nh.subscribe("/actuator_states", 10, &RefereeBase::actuatorStateCallback, this); RefereeBase::dbus_sub_ = nh.subscribe(dbus_topic_, 10, &RefereeBase::dbusDataCallback, this); + RefereeBase::hero_leg_data_sub_ = + nh.subscribe("/cmd_active_suspension", 10, &RefereeBase::heroLegDataCallback, this); RefereeBase::chassis_cmd_sub_ = nh.subscribe("/cmd_chassis", 10, &RefereeBase::chassisCmdDataCallback, this); RefereeBase::vel2D_cmd_sub_ = @@ -32,6 +34,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::camera_name_sub_ = nh.subscribe("/camera_name", 10, &RefereeBase::cameraNameCallBack, this); RefereeBase::balance_state_sub_ = nh.subscribe("/state", 10, &RefereeBase::balanceStateCallback, this); RefereeBase::track_sub_ = nh.subscribe("/track", 10, &RefereeBase::trackCallBack, this); + RefereeBase::deploy_distance_sub_ = + nh.subscribe("/base2target", 10, &RefereeBase::deployDistanceCallBack, this); RefereeBase::map_sentry_sub_ = nh.subscribe("/map_sentry_data", 10, &RefereeBase::mapSentryCallback, this); RefereeBase::radar_receive_sub_ = @@ -54,6 +58,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/customize_display_ui", 1, &RefereeBase::customizeDisplayCmdCallBack, this); RefereeBase::visualize_state_data_sub_ = nh.subscribe("/visualize_state", 1, &RefereeBase::visualizeStateDataCallBack, this); + RefereeBase::relocalize_progress_sub_ = + nh.subscribe("/shinji/progress", 10, &RefereeBase::relocalizeProgressCallback, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -73,6 +79,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) shooter_trigger_change_ui_ = new ShooterTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gimbal") gimbal_trigger_change_ui_ = new GimbalTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "hero_leg_mode") + hero_leg_trigger_change_ui_ = new HeroLegTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target") target_trigger_change_ui_ = new TargetTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target_view_angle") @@ -102,6 +110,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) { if (rpc_value[i]["name"] == "capacitor") capacitor_time_change_ui_ = new CapacitorTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "relocalize") + relocalize_progress_time_change_ui_ = + new RelocalizeProgressTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "effort") effort_time_change_ui_ = new EffortTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "progress") @@ -135,6 +146,11 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "target_distance") target_distance_time_change_ui_ = new TargetDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "deploy_distance") + deploy_distance_time_change_ui_ = + new DeployDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "hero_leg_feedforward_countdown") + hero_leg_time_change_ui_ = new HeroLegTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "drone_towards") drone_towards_time_change_group_ui_ = new DroneTowardsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); @@ -155,8 +171,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "spin") spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - // if (rpc_value[i]["name"] == "deploy") - // deploy_flash_ui_ = new DeployFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "deploy") + deploy_flash_ui_ = new DeployFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "hero_hit") hero_hit_flash_ui_ = new HeroHitFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "exceed_bullet_speed") @@ -209,6 +225,8 @@ void RefereeBase::addUi() ROS_INFO_THROTTLE(1.0, "Adding ui... %.1f%%", (add_ui_times_ / static_cast(add_ui_max_times_)) * 100); if (chassis_trigger_change_ui_) chassis_trigger_change_ui_->addForQueue(2); + if (hero_leg_trigger_change_ui_) + hero_leg_trigger_change_ui_->addForQueue(2); if (gimbal_trigger_change_ui_) gimbal_trigger_change_ui_->addForQueue(2); if (shooter_trigger_change_ui_) @@ -229,6 +247,8 @@ void RefereeBase::addUi() dart_status_time_change_ui_->addForQueue(); if (capacitor_time_change_ui_) capacitor_time_change_ui_->addForQueue(); + if (relocalize_progress_time_change_ui_) + relocalize_progress_time_change_ui_->addForQueue(); if (rotation_time_change_ui_) rotation_time_change_ui_->addForQueue(); if (lane_line_time_change_ui_) @@ -262,6 +282,10 @@ void RefereeBase::addUi() } if (target_distance_time_change_ui_) target_distance_time_change_ui_->addForQueue(); + if (deploy_distance_time_change_ui_) + deploy_distance_time_change_ui_->addForQueue(); + if (hero_leg_time_change_ui_) + hero_leg_time_change_ui_->addForQueue(); if (friend_bullets_time_change_group_ui_) friend_bullets_time_change_group_ui_->addForQueue(); // if (target_hp_time_change_ui_) @@ -381,6 +405,11 @@ void RefereeBase::capacityDataCallBack(const rm_msgs::PowerManagementSampleAndSt if (chassis_trigger_change_ui_ && !is_adding_) chassis_trigger_change_ui_->updateCapacityResetStatus(); } +void RefereeBase::relocalizeProgressCallback(const std_msgs::Int32ConstPtr& data) +{ + if (relocalize_progress_time_change_ui_ && !is_adding_) + relocalize_progress_time_change_ui_->updateRelocalizeProgress(data->data, ros::Time::now()); +} void RefereeBase::powerHeatDataCallBack(const rm_msgs::PowerHeatData& data, const ros::Time& last_get_data_time) { } @@ -443,21 +472,28 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) if (chassis_trigger_change_ui_) chassis_trigger_change_ui_->updateDbusData(data); } +void RefereeBase::heroLegDataCallback(const rm_msgs::ChassisActiveSusCmd::ConstPtr& data) +{ + if (hero_leg_trigger_change_ui_ && !is_adding_) + hero_leg_trigger_change_ui_->updateMode(data->mode); + if (hero_leg_time_change_ui_ && !is_adding_) + hero_leg_time_change_ui_->updateFeedforwardCountdown(data->feedforward_countdown); +} void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& data) { if (chassis_trigger_change_ui_) chassis_trigger_change_ui_->updateChassisCmdData(data); if (spin_flash_ui_ && !is_adding_) spin_flash_ui_->updateChassisCmdData(data, ros::Time::now()); - // if (deploy_flash_ui_ && !is_adding_) - // deploy_flash_ui_->updateChassisCmdData(data, ros::Time::now()); + if (deploy_flash_ui_ && !is_adding_) + deploy_flash_ui_->updateChassisCmdData(data); if (rotation_time_change_ui_ && !is_adding_) rotation_time_change_ui_->updateChassisCmdData(data); } void RefereeBase::vel2DCmdDataCallback(const geometry_msgs::Twist::ConstPtr& data) { - // if (deploy_flash_ui_ && !is_adding_) - // deploy_flash_ui_->updateChassisVelData(data); + if (deploy_flash_ui_ && !is_adding_) + deploy_flash_ui_->updateChassisVelData(data); } void RefereeBase::shootStateCallback(const rm_msgs::ShootState::ConstPtr& data) { @@ -517,6 +553,11 @@ void RefereeBase::trackCallBack(const rm_msgs::TrackDataConstPtr& data) // if (target_hp_time_change_ui_ && !is_adding_) // target_hp_time_change_ui_->updateTrackID(data->id); } +void RefereeBase::deployDistanceCallBack(const geometry_msgs::PointConstPtr& data) +{ + if (deploy_distance_time_change_ui_ && !is_adding_) + deploy_distance_time_change_ui_->updateDeployDistanceData(data); +} void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data) { if (balance_pitch_time_change_group_ui_) @@ -604,6 +645,8 @@ void RefereeBase::shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data) { if (friction_speed_trigger_change_ui_ && !is_adding_) friction_speed_trigger_change_ui_->updateFrictionSpeedUiData(data); + if (deploy_flash_ui_) + deploy_flash_ui_->updateAllowDeployFire(data->allow_deploy_fire); } void RefereeBase::updateBulletRemainData(const rm_referee::BulletNumData& data) diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index ca77b696..3ebdb597 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -169,14 +169,38 @@ void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, void DeployFlashUi::display(const ros::Time& time) { if (!(chassis_mode_ == rm_msgs::ChassisCmd::RAW && angular_z_ == 0.0)) - graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateFlashUiForQueue(time, (chassis_mode_ == rm_msgs::ChassisCmd::RAW && angular_z_ == 0.0), false); + { + FlashUi::updateFlashUiForQueue(time, false, true); + return; + } + if (allow_deploy_fire_) + { + graph_->setColor(rm_referee::GraphColor ::YELLOW); + } + else + { + graph_->setColor(rm_referee::GraphColor ::PINK); + } + FlashUi::updateFlashUiForQueue(time, false, true); + FlashUi::updateFlashUiForQueue(time, true, true); } -void DeployFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data, const ros::Time& last_get_data_time) +void DeployFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data) { - chassis_mode_ = data->mode; - display(last_get_data_time); + if (data->mode != chassis_mode_) + { + chassis_mode_ = data->mode; + display(ros::Time::now()); + } +} + +void DeployFlashUi::updateAllowDeployFire(bool flag) +{ + if (flag != allow_deploy_fire_) + { + allow_deploy_fire_ = flag; + display(ros::Time::now()); + } } void DeployFlashUi::updateChassisVelData(const geometry_msgs::Twist::ConstPtr& data) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 6256855b..f4b2d2b4 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -78,7 +78,37 @@ void CapacitorTimeChangeUi::updateRemainCharge(const double remain_charge, const remain_charge_ = remain_charge; updateForQueue(); } +void RelocalizeProgressTimeChangeUi::add() +{ + graph_->setOperation(rm_referee::GraphOperation::ADD); + UiBase::display(false); +} +void RelocalizeProgressTimeChangeUi::updateConfig() +{ + if (relocalize_progress_ >= 0.) + { + graph_->setStartX(610); + graph_->setStartY(150); + graph_->setEndX(610 + 600 * relocalize_progress_ / 100.0); + graph_->setEndY(150); + + if (relocalize_progress_ >= 70.) + graph_->setColor(rm_referee::GraphColor::GREEN); + else if (relocalize_progress_ >= 30.) + graph_->setColor(rm_referee::GraphColor::ORANGE); + else + graph_->setColor(rm_referee::GraphColor::PINK); + } + else + return; +} + +void RelocalizeProgressTimeChangeUi::updateRelocalizeProgress(const double data, const ros::Time& time) +{ + relocalize_progress_ = data; + updateForQueue(); +} void EffortTimeChangeUi::updateConfig() { char data_str[30] = { ' ' }; @@ -427,6 +457,32 @@ void TargetDistanceTimeChangeUi::updateConfig() graph_->setFloatNum(target_distance_); } +void DeployDistanceTimeChangeUi::updateDeployDistanceData(const geometry_msgs::PointConstPtr& data) +{ + deploy_distance_ = sqrt(data->x * data->x + data->y * data->y); + updateForQueue(); +} + +void DeployDistanceTimeChangeUi::updateConfig() +{ + char buffer[32]; + snprintf(buffer, sizeof(buffer), "%.3f", deploy_distance_); + std::string deploy_dis(buffer); + graph_->setContent(deploy_dis); + graph_->setColor(rm_referee::GraphColor::PINK); +} + +void HeroLegTimeChangeUi::updateFeedforwardCountdown(int feedforward_countdown) +{ + feedforward_countdown_ = feedforward_countdown; + updateForQueue(); +} + +void HeroLegTimeChangeUi::updateConfig() +{ + graph_->setIntNum(feedforward_countdown_); + graph_->setColor(rm_referee::GraphColor::CYAN); +} void DroneTowardsTimeChangeGroupUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data) { angle_ = yawFromQuat(data->pose.orientation) - M_PI / 2; diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 81b88660..2e350296 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -229,6 +229,38 @@ void ChassisTriggerChangeUi::updateCapacityResetStatus() displayInCapacity(); } +void HeroLegTriggerChangeUi::updateMode(uint8_t mode) +{ + leg_mode_ = mode; + update(); +} + +void HeroLegTriggerChangeUi::update() +{ + updateConfig(leg_mode_, false); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateTwiceForQueue(true); +} + +void HeroLegTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) +{ + std::string state = getHeroLegState(main_mode); + graph_->setContent(state); + graph_->setColor(rm_referee::GraphColor::YELLOW); +} + +std::string HeroLegTriggerChangeUi::getHeroLegState(uint8_t mode) +{ + if (mode == rm_msgs::ChassisActiveSusCmd::DOWN) + return "down"; + else if (mode == rm_msgs::ChassisActiveSusCmd::MID) + return "mid"; + else if (mode == rm_msgs::ChassisActiveSusCmd::UP) + return "up"; + else + return "error"; +} + void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); From 7d3a844ae480e4e132db531b043274174a534407 Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Mon, 4 May 2026 16:52:02 +0800 Subject: [PATCH 09/11] Fix power limit. --- 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 7a9e30a6..a50f4aa0 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -132,6 +132,10 @@ class PowerLimit { return start_burst_time_; } + inline void setBurstPowerLimit(const double& burst_power_limit) + { + burst_power_ = burst_power_limit; + } uint8_t getState() { return expect_state_; From 3edfc8ef7adbbfed8e837abb519526b5fe466779 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sat, 6 Jun 2026 08:13:14 +0000 Subject: [PATCH 10/11] Initial plan From f8320f613436dbdec17b1bd6def3b7b2af4edf14 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sat, 6 Jun 2026 08:18:00 +0000 Subject: [PATCH 11/11] ci: pin clang-format in pre-commit hook --- .github/workflows/format.yml | 4 ---- .pre-commit-config.yaml | 10 +++------- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 7ee6d89b..91a16e70 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -15,8 +15,4 @@ jobs: steps: - uses: actions/checkout@v3 - uses: actions/setup-python@v3 - - name: Install clang-format - run: | - sudo apt update - sudo apt install clang-format - uses: pre-commit/action@v3.0.0 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 904f0457..b20b79bd 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,12 +39,8 @@ repos: - id: rst-linter exclude: .*/doc/.* - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 hooks: - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: [ "-fallback-style=none", "-i" ] + args: [ "-fallback-style=none" ]