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 1/5] 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 2/5] 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 3/5] 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 4/5] 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 5/5] 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_;