From 9e2b077ce44d14cbd25d41a2c255a065d74bac68 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 27 Mar 2026 20:35:17 +0800 Subject: [PATCH 1/8] Optimize the advanced planning problem , add the debug msgs. --- rm_msgs/msg/BulletSolverData.msg | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rm_msgs/msg/BulletSolverData.msg b/rm_msgs/msg/BulletSolverData.msg index a4defc57..0237a189 100644 --- a/rm_msgs/msg/BulletSolverData.msg +++ b/rm_msgs/msg/BulletSolverData.msg @@ -1,4 +1,3 @@ - std_msgs/Header header float64 yaw_subtract_ float64 output_yaw_central @@ -11,4 +10,7 @@ float64 center_next_armor_ float64 center_diagonal_armor_ float64 center_triple_next_armor_ float64 traject_acc_ +float64 pos_distance_ +float64 traject_switch_time_ int16 selected_armor_ +int16 Advanced_planning_ From 7bb5d1b4bd9f5c5200f9808bb9e1948dc6883c0e Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 17 Jul 2026 16:32:17 +0800 Subject: [PATCH 2/8] Add some new msg. --- rm_msgs/CMakeLists.txt | 16 ++++++++++++++++ rm_msgs/msg/AssemblyErrorData.msg | 4 ++++ rm_msgs/msg/BulletSolverData.msg | 22 ++++++++-------------- rm_msgs/msg/MPCGimbalAimCmd.msg | 14 ++++++++++++++ rm_msgs/msg/detection/TrackData.msg | 3 +++ 5 files changed, 45 insertions(+), 14 deletions(-) create mode 100644 rm_msgs/msg/AssemblyErrorData.msg create mode 100644 rm_msgs/msg/MPCGimbalAimCmd.msg diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 8482dc50..27442435 100755 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ add_message_files( ShootCmd.msg ShootState.msg ShootBeforehandCmd.msg + GimbalAimCmd.msg GimbalCmd.msg GimbalDesError.msg GimbalPosState.msg @@ -37,6 +38,7 @@ add_message_files( LocalHeatState.msg KalmanData.msg MovingAverageData.msg + MPCGimbalAimCmd.msg GpioData.msg TofRadarData.msg MultiDofCmd.msg @@ -56,6 +58,8 @@ add_service_files( CameraStatus.srv EnableImuTrigger.srv EnableGyro.srv + ExtraFrictionWheelSpeed.srv + ShooterSpeed.srv ) add_message_files( @@ -88,19 +92,31 @@ add_message_files( BulletAllowance.msg RfidStatus.msg DartClientCmd.msg + DartRemainingTime.msg ManualToReferee.msg RadarData.msg + RadarCmd.msg RadarMarkData.msg RobotsPositionData.msg ClientMapSendData.msg ClientMapReceiveData.msg MapSentryData.msg GameRobotPosData.msg + EnemyHpInfo.msg SentryInfo.msg RadarInfo.msg Buff.msg RadarToSentry.msg SentryCmd.msg + RadarWirelessEnemyCallSign.msg + RadarWirelessEnemyCoinAndFieldStatus.msg + RadarWirelessEnemyProjectileAllowance.msg + RadarWirelessEnemyRobotBuff.msg + RadarWirelessEnemyRobotHp.msg + RadarWirelessEnemyRobotPos.msg + CustomClientCmdData.msg + RobotCustomData.msg + RobotCustomData2.msg VisualizeStateData.msg PowerManagementSampleAndStatusData.msg PowerManagementInitializationExceptionData.msg diff --git a/rm_msgs/msg/AssemblyErrorData.msg b/rm_msgs/msg/AssemblyErrorData.msg new file mode 100644 index 00000000..cf011813 --- /dev/null +++ b/rm_msgs/msg/AssemblyErrorData.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +float64 roll_error +float64 pitch_error +float64 yaw_error \ No newline at end of file diff --git a/rm_msgs/msg/BulletSolverData.msg b/rm_msgs/msg/BulletSolverData.msg index 0237a189..9f19ac47 100644 --- a/rm_msgs/msg/BulletSolverData.msg +++ b/rm_msgs/msg/BulletSolverData.msg @@ -1,16 +1,10 @@ std_msgs/Header header -float64 yaw_subtract_ -float64 output_yaw_central -float64 test_number -float64 after_fly_yaw_subtract_ -float64 switch_armor_angle -float64 switche_time_yaw_ -float64 center_this_armor_ -float64 center_next_armor_ -float64 center_diagonal_armor_ -float64 center_triple_next_armor_ +int16[150] selected_armor_ +int16[150] count_number_ +float64 start_vel +float64 switchtime +float64 traject_output_yaw_ float64 traject_acc_ -float64 pos_distance_ -float64 traject_switch_time_ -int16 selected_armor_ -int16 Advanced_planning_ +float64 traject_vel_ +int16 using_traject_ +float64[100] test_number diff --git a/rm_msgs/msg/MPCGimbalAimCmd.msg b/rm_msgs/msg/MPCGimbalAimCmd.msg new file mode 100644 index 00000000..547bce41 --- /dev/null +++ b/rm_msgs/msg/MPCGimbalAimCmd.msg @@ -0,0 +1,14 @@ +std_msgs/Header header +bool valid +uint8 target_id +uint8 shoot_cmd +float64 yaw +float64 pitch +float64 yaw_rate +float64 pitch_rate +float64 yaw_acc +float64 pitch_acc +float64 target_yaw +float64 target_pitch +float64 error +geometry_msgs/Point selected_armor_pos diff --git a/rm_msgs/msg/detection/TrackData.msg b/rm_msgs/msg/detection/TrackData.msg index 276929d6..1cc15d3e 100644 --- a/rm_msgs/msg/detection/TrackData.msg +++ b/rm_msgs/msg/detection/TrackData.msg @@ -1,5 +1,6 @@ std_msgs/Header header uint8 id +uint8 outpost_id bool tracking uint8 armors_num geometry_msgs/Point position @@ -10,4 +11,6 @@ float64 v_yaw float64 radius_1 float64 radius_2 float64 dz +float64 za +float64 next_armor_z_diff float64 accel From 6be94bf8a0b06430abde7aed98125a5c935cad9a Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 2 Aug 2026 03:27:07 +0800 Subject: [PATCH 3/8] Add dynamicx nimimal hz . --- .../include/rm_common/decision/heat_limit.h | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index d4c3365b..a16f6678 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -62,6 +62,8 @@ class HeatLimit ROS_ERROR("Burst shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("minimal_shoot_frequency", minimal_shoot_frequency_)) ROS_ERROR("Minimal shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("outpost_shoot_frequency", outpost_shoot_frequency_)) + ROS_ERROR("Outpost shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("safe_shoot_frequency", safe_shoot_frequency_)) ROS_ERROR("Safe shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("heat_coeff", heat_coeff_)) @@ -70,6 +72,8 @@ class HeatLimit ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("local_heat_protect_threshold", heat_protect_threshold_)) ROS_ERROR("Local heat protect threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("buff_offset", buff_offset_)) + ROS_ERROR("Buff offset no defined (namespace: %s)", nh.getNamespace().c_str()); nh.param("use_local_heat", use_local_heat_, true); if (type_ == "ID1_42MM") bullet_heat_ = 100.; @@ -79,6 +83,8 @@ class HeatLimit shoot_state_sub_ = nh.subscribe("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this); timer_ = nh.createTimer(ros::Duration(0.1), std::bind(&HeatLimit::timerCB, this)); + fly_time_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/fly_time", 10, + &HeatLimit::flyTimeCB, this); } typedef enum @@ -113,6 +119,7 @@ class HeatLimit } local_heat_pub_data_.once_shoot_num = once_shoot_num_; local_heat_pub_data_.local_shooter_cooling_heat = local_shooter_cooling_heat_; + local_heat_pub_data_.desire_shoot_frequency = shoot_frequency_; local_heat_pub_.publish(local_heat_pub_data_); } @@ -130,12 +137,14 @@ class HeatLimit } else if (type_ == "ID2_17MM") { - shooter_cooling_heat_ = data.shooter_id_2_17_mm_cooling_heat; + // RoboMaster 2026 protocol only reports one 17mm barrel heat field. + shooter_cooling_heat_ = data.shooter_id_1_17_mm_cooling_heat; } else if (type_ == "ID1_42MM") { shooter_cooling_heat_ = data.shooter_id_1_42_mm_cooling_heat; } + referee_status_change_ = true; } void setRefereeStatus(bool status) @@ -143,11 +152,16 @@ class HeatLimit referee_is_online_ = status; } - double getShootFrequency() const + double getShootFrequency() { std::lock_guard lock(heat_mutex_); - if (state_ == BURST) - return shoot_frequency_; + if (!referee_is_online_) + return 5.0; + if(referee_status_change_ && abs(local_shooter_cooling_heat_ - shooter_cooling_heat_) > 10) + { + local_shooter_cooling_heat_ = shooter_cooling_heat_; + } + referee_status_change_ = false; double shooter_cooling_heat = (use_local_heat_ || !referee_is_online_) ? local_shooter_cooling_heat_ : shooter_cooling_heat_; if (shooter_cooling_limit_ - shooter_cooling_heat < bullet_heat_) @@ -189,11 +203,16 @@ class HeatLimit state_ = mode; } - bool getShootFrequencyMode() const + int getShootFrequencyMode() const { return state_; } + void flyTimeCB(const std_msgs::Float64ConstPtr& msg) + { + buff_shoot_frequency_ = 1/(msg->data + buff_offset); + } + private: void updateExpectShootFrequency() { @@ -214,7 +233,8 @@ class HeatLimit } else if (state_ == HeatLimit::MINIMAL) { - shoot_frequency_ = minimal_shoot_frequency_; + shoot_frequency_ = buff_shoot_frequency_; + //shoot_frequency_ = minimal_shoot_frequency_; burst_flag_ = false; } else @@ -228,15 +248,18 @@ class HeatLimit std::string type_{}; bool burst_flag_ = false; double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{}, - high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}; + high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}, outpost_shoot_frequency_{}; bool referee_is_online_, use_local_heat_, last_shoot_state_{}; int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_; double local_shooter_cooling_heat_{}, heat_protect_threshold_{}; + double buff_shoot_frequency_{},buff_offset_{0.0}; int once_shoot_num_{}; + bool referee_status_change_{}; ros::Publisher local_heat_pub_; ros::Subscriber shoot_state_sub_; + ros::Subscriber fly_time_sub_; ros::Timer timer_; rm_msgs::LocalHeatData local_heat_pub_data_; From 7c96b4f8de9da2305e559ee574f6e9da9c9c42eb Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 16 Oct 2026 22:32:23 +0800 Subject: [PATCH 4/8] Add color change service and ui,recover nonlinear track differentiator class. --- .../rm_common/decision/service_caller.h | 33 ++++++- rm_common/include/rm_common/traj_gen.h | 38 +++++++ .../include/rm_referee/ui/time_change_ui.h | 59 ++++++++++- rm_referee/src/ui/time_change_ui.cpp | 98 ++++++++++++++----- 4 files changed, 203 insertions(+), 25 deletions(-) diff --git a/rm_common/include/rm_common/decision/service_caller.h b/rm_common/include/rm_common/decision/service_caller.h index 457a2089..76beca7d 100644 --- a/rm_common/include/rm_common/decision/service_caller.h +++ b/rm_common/include/rm_common/decision/service_caller.h @@ -46,6 +46,9 @@ #include #include #include +#include +#include +#include namespace rm_common { @@ -132,7 +135,35 @@ class ServiceCallerBase std::mutex mutex_; int fail_count_, fail_limit_; }; +class ColorChangeServiceCaller : public ServiceCallerBase +{ +public: + explicit ColorChangeServiceCaller(ros::NodeHandle& nh) + : ServiceCallerBase(nh, "/superpower_track_vision/change_color") + { + service_.request.data = false; + callService(); + } + void changeColor() + { + service_.request.data = !service_.request.data; + callService(); + } +}; +class TrackerResetServiceCaller : public ServiceCallerBase +{ +public: + explicit TrackerResetServiceCaller(ros::NodeHandle& nh) + : ServiceCallerBase(nh, "/superpower_track/reset") + { + callService(); + } + void reset() + { + callService(); + } +}; class SwitchControllersServiceCaller : public ServiceCallerBase { public: @@ -185,7 +216,7 @@ class SwitchDetectionCaller : public ServiceCallerBase { public: explicit SwitchDetectionCaller(ros::NodeHandle& nh) - : ServiceCallerBase(nh, "/detection_nodelet/status_switch") + : ServiceCallerBase(nh, "/Processor/status_change") { service_.request.target = rm_msgs::StatusChangeRequest::ARMOR; service_.request.exposure = rm_msgs::StatusChangeRequest::EXPOSURE_LEVEL_0; diff --git a/rm_common/include/rm_common/traj_gen.h b/rm_common/include/rm_common/traj_gen.h index 6d00c022..0b54b2f1 100644 --- a/rm_common/include/rm_common/traj_gen.h +++ b/rm_common/include/rm_common/traj_gen.h @@ -199,3 +199,41 @@ class MinTimeTraj } } }; + +template +class NonlinearTrackingDifferentiator +{ +public: + NonlinearTrackingDifferentiator(T r, T h) : r_(r), h_(h) + { + } + void update(T v, T v_dot) + { + if ((v > 3. && x1_ < -3.) || (v < -3 && x1_ > 3)) + { + x1_ = v; + x2_ = 0; + } + else + { + T y = x1_ - v + h_ * x2_; + T a0 = sqrt(h_ * h_ * r_ * r_ + 8 * r_ * fabs(y)); + T a = x2_ + 0.5 * (a0 - h_ * r_) * (y > 0 ? 1 : -1); + T u = fabs(a) > h_ * r_ ? -r_ * (a > 0 ? 1 : -1) : -r_ * a / (h_ * r_); + x1_ = x1_ + h_ * (x2_ + v_dot); + x2_ = x2_ + h_ * u; + } + } + T getX1() const + { + return x1_; + } + T getX2() const + { + return x2_; + } + +private: + T r_{}, h_{}; + T x1_{}, x2_{}; +}; 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..f462b143 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -3,7 +3,7 @@ // #pragma once - +#include #include "rm_referee/ui/ui_base.h" namespace rm_referee @@ -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: @@ -143,6 +159,8 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi robot_height_ = data["height"]; camera_range_ = data["camera_range"]; surface_coefficient_ = data["surface_coefficient"]; + if (data.hasMember("x_offset")) + x_offset_ = data["x_offset"]; } else ROS_WARN("LaneLineTimeChangeGroupUi config 's member 'data' not defined."); @@ -167,6 +185,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi protected: std::string reference_frame_; double robot_radius_, robot_height_, camera_range_, surface_coefficient_ = 0.5; + double x_offset_ = 0.0; double pitch_angle_ = 0., screen_x_ = 1920, screen_y_ = 1080; double end_point_a_angle_, end_point_b_angle_; @@ -315,7 +334,45 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi void updateConfig() override; double target_distance_; }; +class EnemyColorTimeChangeUi : public TimeChangeUi +{ +public: + explicit EnemyColorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "enemy_color", graph_queue, character_queue){}; + void updateEnemyColorData(const std_msgs::Bool::ConstPtr& data); + +private: + void updateConfig() override; + int enemy_color_{}; +}; +class DeployDistanceTimeChangeUi : public TimeChangeUi +{ +public: + 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/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 48ee5647..d54d077e 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] = { ' ' }; @@ -208,16 +238,16 @@ void LaneLineTimeChangeGroupUi::updateConfig() { if (it.first == "lane_line_left") { - it.second->setStartX(screen_x_ / 2 - spacing_x_a); + it.second->setStartX(screen_x_ / 2 + x_offset_ - spacing_x_a); it.second->setStartY(screen_y_ / 2 - spacing_y_a); - it.second->setEndX(screen_x_ / 2 - spacing_x_b * surface_coefficient_); + it.second->setEndX(screen_x_ / 2 + x_offset_ - spacing_x_b * surface_coefficient_); it.second->setEndY(screen_y_ / 2 - spacing_y_b); } else if (it.first == "lane_line_right") { - it.second->setStartX(screen_x_ / 2 + spacing_x_a); + it.second->setStartX(screen_x_ / 2 + x_offset_ + spacing_x_a); it.second->setStartY(screen_y_ / 2 - spacing_y_a); - it.second->setEndX(screen_x_ / 2 + spacing_x_b * surface_coefficient_); + it.second->setEndX(screen_x_ / 2 + x_offset_ + spacing_x_b * surface_coefficient_); it.second->setEndY(screen_y_ / 2 - spacing_y_b); } } @@ -426,7 +456,46 @@ void TargetDistanceTimeChangeUi::updateConfig() { graph_->setFloatNum(target_distance_); } +void EnemyColorTimeChangeUi::updateEnemyColorData(const std_msgs::Bool::ConstPtr& data) +{ + enemy_color_ = data->data; + updateForQueue(); +} +void EnemyColorTimeChangeUi::updateConfig() +{ + char temp_content[32]; + snprintf(temp_content, sizeof(temp_content), "%s", enemy_color_ ? "BLUE" : "RED" ); + std::string enemy_color_content(temp_content); + graph_->setContent(enemy_color_content); + graph_->setColor(enemy_color_ ? rm_referee::GraphColor::CYAN : rm_referee::GraphColor::PINK); +} + +void DeployDistanceTimeChangeUi::updateDeployDistanceData(const geometry_msgs::PointConstPtr& data) +{ + deploy_distance_ = sqrt(data->x * data->x + data->y * data->y); + updateForQueue(); +} +void DeployDistanceTimeChangeUi::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; @@ -503,23 +572,7 @@ void FriendBulletsTimeChangeGroupUi::updateConfig() void TargetHpTimeChangeUi::setEnemyHp(const rm_msgs::GameRobotHp& data) { - bool is_enemy_red = base_.robot_id_ > 100; - if (is_enemy_red) - { - enemy_robot_hp_[1] = data.red_1_robot_hp; - enemy_robot_hp_[2] = data.red_2_robot_hp; - enemy_robot_hp_[3] = data.red_3_robot_hp; - enemy_robot_hp_[4] = data.red_4_robot_hp; - enemy_robot_hp_[7] = data.red_7_robot_hp; - } - else - { - enemy_robot_hp_[1] = data.blue_1_robot_hp; - enemy_robot_hp_[2] = data.blue_2_robot_hp; - enemy_robot_hp_[3] = data.blue_3_robot_hp; - enemy_robot_hp_[4] = data.blue_4_robot_hp; - enemy_robot_hp_[7] = data.blue_7_robot_hp; - } + (void)data; } void TargetHpTimeChangeUi::updateTrackID(int id) @@ -530,8 +583,7 @@ void TargetHpTimeChangeUi::updateTrackID(int id) void TargetHpTimeChangeUi::updateTargeHptData() { - auto it = enemy_robot_hp_.find(target_id_); - target_hp_ = it == enemy_robot_hp_.end() ? 0 : it->second; + target_hp_ = 0; updateForQueue(); } From 8cdac46437d73945e0bb9fff1d2c639f62779e8d Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 16 Oct 2026 22:50:35 +0800 Subject: [PATCH 5/8] Fix bug. --- .../include/rm_common/DebugDataPublisher.h | 112 ++++++++++++++++++ rm_msgs/CMakeLists.txt | 1 - rm_msgs/msg/ChassisCmd.msg | 1 + rm_msgs/msg/DebugData.msg | 8 ++ rm_msgs/msg/referee/CustomClientCmdData.msg | 3 + rm_msgs/msg/referee/DartRemainingTime.msg | 4 + rm_msgs/msg/referee/EnemyHpInfo.msg | 6 + rm_msgs/msg/referee/RadarCmd.msg | 8 ++ .../referee/RadarWirelessEnemyCallSign.msg | 3 + .../RadarWirelessEnemyCoinAndFieldStatus.msg | 5 + .../RadarWirelessEnemyProjectileAllowance.msg | 7 ++ .../referee/RadarWirelessEnemyRobotBuff.msg | 38 ++++++ .../msg/referee/RadarWirelessEnemyRobotHp.msg | 8 ++ .../referee/RadarWirelessEnemyRobotPos.msg | 14 +++ rm_msgs/msg/referee/RobotCustomData.msg | 3 + rm_msgs/msg/referee/RobotCustomData2.msg | 3 + 16 files changed, 223 insertions(+), 1 deletion(-) create mode 100644 rm_common/include/rm_common/DebugDataPublisher.h create mode 100644 rm_msgs/msg/DebugData.msg create mode 100644 rm_msgs/msg/referee/CustomClientCmdData.msg create mode 100644 rm_msgs/msg/referee/DartRemainingTime.msg create mode 100644 rm_msgs/msg/referee/EnemyHpInfo.msg create mode 100644 rm_msgs/msg/referee/RadarCmd.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg create mode 100644 rm_msgs/msg/referee/RobotCustomData.msg create mode 100644 rm_msgs/msg/referee/RobotCustomData2.msg 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_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 27442435..2ea6e105 100755 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -23,7 +23,6 @@ add_message_files( ShootCmd.msg ShootState.msg ShootBeforehandCmd.msg - GimbalAimCmd.msg GimbalCmd.msg GimbalDesError.msg GimbalPosState.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_msgs/msg/referee/CustomClientCmdData.msg b/rm_msgs/msg/referee/CustomClientCmdData.msg new file mode 100644 index 00000000..61f3297d --- /dev/null +++ b/rm_msgs/msg/referee/CustomClientCmdData.msg @@ -0,0 +1,3 @@ +uint8[30] data + +time stamp diff --git a/rm_msgs/msg/referee/DartRemainingTime.msg b/rm_msgs/msg/referee/DartRemainingTime.msg new file mode 100644 index 00000000..77fe568f --- /dev/null +++ b/rm_msgs/msg/referee/DartRemainingTime.msg @@ -0,0 +1,4 @@ +uint8 dart_remaining_time +uint8 dart_aim_state + +time stamp diff --git a/rm_msgs/msg/referee/EnemyHpInfo.msg b/rm_msgs/msg/referee/EnemyHpInfo.msg new file mode 100644 index 00000000..2911e35f --- /dev/null +++ b/rm_msgs/msg/referee/EnemyHpInfo.msg @@ -0,0 +1,6 @@ +uint16 enemy_1_HP +uint16 enemy_2_HP +uint16 enemy_3_HP +uint16 enemy_4_HP +uint16 enemy_5_HP +uint16 enemy_7_HP diff --git a/rm_msgs/msg/referee/RadarCmd.msg b/rm_msgs/msg/referee/RadarCmd.msg new file mode 100644 index 00000000..67ffab06 --- /dev/null +++ b/rm_msgs/msg/referee/RadarCmd.msg @@ -0,0 +1,8 @@ +uint8 radar_cmd +uint8 password_cmd +uint8 password_1 +uint8 password_2 +uint8 password_3 +uint8 password_4 +uint8 password_5 +uint8 password_6 diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg b/rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg new file mode 100644 index 00000000..1c99ed1c --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg @@ -0,0 +1,3 @@ +uint8[6] ascii_data + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg b/rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg new file mode 100644 index 00000000..5ae7f573 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg @@ -0,0 +1,5 @@ +uint16 remaining_coin +uint16 total_coin +uint32 field_status + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg b/rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg new file mode 100644 index 00000000..26b429b1 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg @@ -0,0 +1,7 @@ +uint16 hero_projectile_allowance +uint16 infantry_3_projectile_allowance +uint16 infantry_4_projectile_allowance +uint16 aerial_projectile_allowance +uint16 sentry_projectile_allowance + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg b/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg new file mode 100644 index 00000000..b657cbb8 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg @@ -0,0 +1,38 @@ +uint8 hero_recovery_buff +uint16 hero_cooling_buff +uint8 hero_defense_buff +uint8 hero_negative_defense_buff +uint16 hero_attack_buff + +uint8 engineer_recovery_buff +uint16 engineer_cooling_buff +uint8 engineer_defense_buff +uint8 engineer_negative_defense_buff +uint16 engineer_attack_buff + +uint8 infantry_3_recovery_buff +uint16 infantry_3_cooling_buff +uint8 infantry_3_defense_buff +uint8 infantry_3_negative_defense_buff +uint16 infantry_3_attack_buff + +uint8 infantry_4_recovery_buff +uint16 infantry_4_cooling_buff +uint8 infantry_4_defense_buff +uint8 infantry_4_negative_defense_buff +uint16 infantry_4_attack_buff + +uint8 sentry_recovery_buff +uint16 sentry_cooling_buff +uint8 sentry_defense_buff +uint8 sentry_negative_defense_buff +uint16 sentry_attack_buff +uint8 sentry_posture + +uint8 hero_main_status +uint8 engineer_main_status +uint8 infantry_3_main_status +uint8 infantry_4_main_status +uint8 sentry_main_status + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg b/rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg new file mode 100644 index 00000000..55413df6 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg @@ -0,0 +1,8 @@ +uint16 hero_hp +uint16 engineer_hp +uint16 infantry_3_hp +uint16 infantry_4_hp +uint16 reserved +uint16 sentry_hp + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg b/rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg new file mode 100644 index 00000000..fa01ba1b --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg @@ -0,0 +1,14 @@ +uint16 hero_position_x +uint16 hero_position_y +uint16 engineer_position_x +uint16 engineer_position_y +uint16 infantry_3_position_x +uint16 infantry_3_position_y +uint16 infantry_4_position_x +uint16 infantry_4_position_y +uint16 aerial_position_x +uint16 aerial_position_y +uint16 sentry_position_x +uint16 sentry_position_y + +time stamp diff --git a/rm_msgs/msg/referee/RobotCustomData.msg b/rm_msgs/msg/referee/RobotCustomData.msg new file mode 100644 index 00000000..61f3297d --- /dev/null +++ b/rm_msgs/msg/referee/RobotCustomData.msg @@ -0,0 +1,3 @@ +uint8[30] data + +time stamp diff --git a/rm_msgs/msg/referee/RobotCustomData2.msg b/rm_msgs/msg/referee/RobotCustomData2.msg new file mode 100644 index 00000000..070cdda6 --- /dev/null +++ b/rm_msgs/msg/referee/RobotCustomData2.msg @@ -0,0 +1,3 @@ +uint8[300] data + +time stamp From 0fa2beb3335733f2d99674aa9dd8520d0d7a56b0 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 16 Oct 2026 23:34:37 +0800 Subject: [PATCH 6/8] Fix format. --- .../include/rm_common/decision/heat_limit.h | 10 +- .../include/rm_referee/ui/time_change_ui.h | 420 +++++++++--------- rm_referee/src/ui/time_change_ui.cpp | 2 +- 3 files changed, 216 insertions(+), 216 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 99771885..99bc1bcb 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -84,7 +84,7 @@ class HeatLimit nh.subscribe("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this); timer_ = nh.createTimer(ros::Duration(0.1), std::bind(&HeatLimit::timerCB, this)); fly_time_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/fly_time", 10, - &HeatLimit::flyTimeCB, this); + &HeatLimit::flyTimeCB, this); } typedef enum @@ -157,7 +157,7 @@ class HeatLimit std::lock_guard lock(heat_mutex_); if (!referee_is_online_) return 5.0; - if(referee_status_change_ && abs(local_shooter_cooling_heat_ - shooter_cooling_heat_) > 10) + if (referee_status_change_ && abs(local_shooter_cooling_heat_ - shooter_cooling_heat_) > 10) { local_shooter_cooling_heat_ = shooter_cooling_heat_; } @@ -210,7 +210,7 @@ class HeatLimit void flyTimeCB(const std_msgs::Float64ConstPtr& msg) { - buff_shoot_frequency_ = 1/(msg->data + buff_offset); + buff_shoot_frequency_ = 1 / (msg->data + buff_offset); } private: @@ -234,7 +234,7 @@ class HeatLimit else if (state_ == HeatLimit::MINIMAL) { shoot_frequency_ = buff_shoot_frequency_; - //shoot_frequency_ = minimal_shoot_frequency_; + // shoot_frequency_ = minimal_shoot_frequency_; burst_flag_ = false; } else @@ -253,7 +253,7 @@ class HeatLimit bool referee_is_online_, use_local_heat_, last_shoot_state_{}; int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_; double local_shooter_cooling_heat_{}, heat_protect_threshold_{}; - double buff_shoot_frequency_{},buff_offset_{}; + double buff_shoot_frequency_{}, buff_offset_{}; int once_shoot_num_{}; bool referee_status_change_{}; 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 001b7671..acd9873e 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -355,256 +355,256 @@ class DeployDistanceTimeChangeUi : public TimeChangeUi : TimeChangeUi(rpc_value, base, "deploy_distance", graph_queue, character_queue){}; void updateDeployDistanceData(const geometry_msgs::PointConstPtr& data); -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) + class DeployDistanceTimeChangeUi : public TimeChangeUi { - } - void updateFeedforwardCountdown(int feedforward_countdown); + 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_{}; + }; -private: - void updateConfig() override; - int feedforward_countdown_{}; -}; -class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi -{ -public: - explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) + class HeroLegTimeChangeUi : public TimeChangeUi { - if (rpc_value.hasMember("data")) + 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) { - XmlRpc::XmlRpcValue& data = rpc_value["data"]; - ori_x_ = static_cast(data["ori_x"]); - ori_y_ = static_cast(data["ori_y"]); } - else - ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined."); + void updateFeedforwardCountdown(int feedforward_countdown); - graph_vector_.insert( - std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert( - std::pair(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert( - std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); + private: + void updateConfig() override; + int feedforward_countdown_{}; }; - void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data); - -private: - void updateConfig() override; - int ori_x_, ori_y_; - double angle_; - int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, - right_line_y2_; -}; - -class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi -{ -public: - explicit FriendBulletsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeGroupUi(rpc_value, base, "friend_bullets", graph_queue, character_queue) + class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi { - graph_vector_.insert(std::pair("hero", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert(std::pair("standard3", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert(std::pair("standard4", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert(std::pair("standard5", new Graph(rpc_value["config"], base_, id_++))); - int ui_start_y = 0; - for (auto it = graph_vector_.begin(); it != graph_vector_.end(); ++it) + public: + explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) { - if (it == graph_vector_.begin()) - ui_start_y = it->second->getConfig().start_y; - else + if (rpc_value.hasMember("data")) { - ui_start_y -= 40; - it->second->setStartY(ui_start_y); + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + ori_x_ = static_cast(data["ori_x"]); + ori_y_ = static_cast(data["ori_y"]); } - } + else + ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined."); + + graph_vector_.insert( + std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); + }; + void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data); + + private: + void updateConfig() override; + int ori_x_, ori_y_; + double angle_; + int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, + right_line_y2_; }; - void updateBulletsData(const rm_referee::BulletNumData& data); -private: - void updateConfig() override; - int hero_bullets_{ 1 }, standard3_bullets_{ 3 }, standard4_bullets_{ 4 }, standard5_bullets_{ 5 }; -}; - -class TargetHpTimeChangeUi : public TimeChangeUi -{ -public: - explicit TargetHpTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeUi(rpc_value, base, "target_hp", graph_queue, character_queue) + class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi { - if (rpc_value.hasMember("enemy_id")) + public: + explicit FriendBulletsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "friend_bullets", graph_queue, character_queue) { - XmlRpc::XmlRpcValue& enemy_id = rpc_value["enemy_id"]; - for (int i = 0; i < enemy_id.size(); i++) + graph_vector_.insert(std::pair("hero", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard3", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard4", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard5", new Graph(rpc_value["config"], base_, id_++))); + int ui_start_y = 0; + for (auto it = graph_vector_.begin(); it != graph_vector_.end(); ++it) { - int id = static_cast(enemy_id[i]); - enemy_robot_hp_[id] = 0; + if (it == graph_vector_.begin()) + ui_start_y = it->second->getConfig().start_y; + else + { + ui_start_y -= 40; + it->second->setStartY(ui_start_y); + } } - } - } - void setEnemyHp(const rm_msgs::GameRobotHp& data); - void updateTrackID(int id); - void updateTargeHptData(); + }; + void updateBulletsData(const rm_referee::BulletNumData& data); -private: - void updateConfig() override; - std::map enemy_robot_hp_; - int target_hp_{}, target_id_{}; -}; + private: + void updateConfig() override; + int hero_bullets_{ 1 }, standard3_bullets_{ 3 }, standard4_bullets_{ 4 }, standard5_bullets_{ 5 }; + }; -class LegThetaTimeChangeGroupUi : public TimeChangeGroupUi -{ -public: - explicit LegThetaTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeGroupUi(rpc_value, base, "leg_theta", graph_queue, character_queue) + class TargetHpTimeChangeUi : public TimeChangeUi { - XmlRpc::XmlRpcValue line_config; - line_config["type"] = "line"; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("color")) - line_config["color"] = rpc_value["config"]["color"]; - else - line_config["color"] = "cyan"; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("width")) - line_config["width"] = rpc_value["config"]["width"]; - else - line_config["width"] = 2; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("delay")) - line_config["delay"] = rpc_value["config"]["delay"]; - else - line_config["delay"] = 0.2; - - // Virtual rod is rendered as multiple short segments (dash effect). - XmlRpc::XmlRpcValue virtual_rod_config = line_config; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_color")) - virtual_rod_config["color"] = rpc_value["config"]["virtual_rod_color"]; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_width")) - virtual_rod_config["width"] = rpc_value["config"]["virtual_rod_width"]; - else - virtual_rod_config["width"] = 1; - - XmlRpc::XmlRpcValue rect_config; - rect_config["type"] = "rectangle"; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("chassis_color")) - rect_config["color"] = rpc_value["config"]["chassis_color"]; - else - rect_config["color"] = line_config["color"]; - rect_config["width"] = line_config["width"]; - rect_config["delay"] = line_config["delay"]; - - if (rpc_value.hasMember("data")) + public: + explicit TargetHpTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "target_hp", graph_queue, character_queue) { - XmlRpc::XmlRpcValue data = rpc_value["data"]; - if (data.hasMember("draw_chassis")) - draw_chassis_ = static_cast(data["draw_chassis"]); - if (data.hasMember("chassis_start") && data.hasMember("chassis_end")) + if (rpc_value.hasMember("enemy_id")) { - chassis_start_[0] = static_cast(data["chassis_start"][0]); - chassis_start_[1] = static_cast(data["chassis_start"][1]); - chassis_end_[0] = static_cast(data["chassis_end"][0]); - chassis_end_[1] = static_cast(data["chassis_end"][1]); + XmlRpc::XmlRpcValue& enemy_id = rpc_value["enemy_id"]; + for (int i = 0; i < enemy_id.size(); i++) + { + int id = static_cast(enemy_id[i]); + enemy_robot_hp_[id] = 0; + } } + } + void setEnemyHp(const rm_msgs::GameRobotHp& data); + void updateTrackID(int id); + void updateTargeHptData(); + + private: + void updateConfig() override; + std::map enemy_robot_hp_; + int target_hp_{}, target_id_{}; + }; + + class LegThetaTimeChangeGroupUi : public TimeChangeGroupUi + { + public: + explicit LegThetaTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "leg_theta", graph_queue, character_queue) + { + XmlRpc::XmlRpcValue line_config; + line_config["type"] = "line"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("color")) + line_config["color"] = rpc_value["config"]["color"]; else - { - ROS_WARN("LegThetaTimeChangeGroupUi: 'data.chassis_start/end' not defined, using default rectangle."); - chassis_start_[0] = 800; - chassis_start_[1] = 600; - chassis_end_[0] = 1120; - chassis_end_[1] = 720; - } + line_config["color"] = "cyan"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("width")) + line_config["width"] = rpc_value["config"]["width"]; + else + line_config["width"] = 2; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("delay")) + line_config["delay"] = rpc_value["config"]["delay"]; + else + line_config["delay"] = 0.2; + + // Virtual rod is rendered as multiple short segments (dash effect). + XmlRpc::XmlRpcValue virtual_rod_config = line_config; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_color")) + virtual_rod_config["color"] = rpc_value["config"]["virtual_rod_color"]; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_width")) + virtual_rod_config["width"] = rpc_value["config"]["virtual_rod_width"]; + else + virtual_rod_config["width"] = 1; + + XmlRpc::XmlRpcValue rect_config; + rect_config["type"] = "rectangle"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("chassis_color")) + rect_config["color"] = rpc_value["config"]["chassis_color"]; + else + rect_config["color"] = line_config["color"]; + rect_config["width"] = line_config["width"]; + rect_config["delay"] = line_config["delay"]; - if (data.hasMember("origin_point")) + if (rpc_value.hasMember("data")) { - origin_point_[0] = static_cast(data["origin_point"][0]); - origin_point_[1] = static_cast(data["origin_point"][1]); + XmlRpc::XmlRpcValue data = rpc_value["data"]; + if (data.hasMember("draw_chassis")) + draw_chassis_ = static_cast(data["draw_chassis"]); + if (data.hasMember("chassis_start") && data.hasMember("chassis_end")) + { + chassis_start_[0] = static_cast(data["chassis_start"][0]); + chassis_start_[1] = static_cast(data["chassis_start"][1]); + chassis_end_[0] = static_cast(data["chassis_end"][0]); + chassis_end_[1] = static_cast(data["chassis_end"][1]); + } + else + { + ROS_WARN("LegThetaTimeChangeGroupUi: 'data.chassis_start/end' not defined, using default rectangle."); + chassis_start_[0] = 800; + chassis_start_[1] = 600; + chassis_end_[0] = 1120; + chassis_end_[1] = 720; + } + + if (data.hasMember("origin_point")) + { + origin_point_[0] = static_cast(data["origin_point"][0]); + origin_point_[1] = static_cast(data["origin_point"][1]); + } + else + { + // Default: rectangle center. + origin_point_[0] = (chassis_start_[0] + chassis_end_[0]) / 2; + origin_point_[1] = (chassis_start_[1] + chassis_end_[1]) / 2; + } + + if (data.hasMember("link1_length")) + link1_length_m_ = static_cast(data["link1_length"]); + if (data.hasMember("link2_length")) + link2_length_m_ = static_cast(data["link2_length"]); + if (data.hasMember("pixels_per_meter")) + pixels_per_meter_ = static_cast(data["pixels_per_meter"]); + if (data.hasMember("leg_side")) + leg_side_ = static_cast(data["leg_side"]); } else { - // Default: rectangle center. - origin_point_[0] = (chassis_start_[0] + chassis_end_[0]) / 2; - origin_point_[1] = (chassis_start_[1] + chassis_end_[1]) / 2; + ROS_WARN("LegThetaTimeChangeGroupUi config 's member 'data' not defined."); } - if (data.hasMember("link1_length")) - link1_length_m_ = static_cast(data["link1_length"]); - if (data.hasMember("link2_length")) - link2_length_m_ = static_cast(data["link2_length"]); - if (data.hasMember("pixels_per_meter")) - pixels_per_meter_ = static_cast(data["pixels_per_meter"]); - if (data.hasMember("leg_side")) - leg_side_ = static_cast(data["leg_side"]); - } - else - { - ROS_WARN("LegThetaTimeChangeGroupUi config 's member 'data' not defined."); - } + // Chassis rectangle (left=rear, right=front). + if (draw_chassis_) + { + rect_config["start_position"][0] = chassis_start_[0]; + rect_config["start_position"][1] = chassis_start_[1]; + rect_config["end_position"][0] = chassis_end_[0]; + rect_config["end_position"][1] = chassis_end_[1]; + graph_vector_.insert(std::make_pair("chassis", new Graph(rect_config, base_, id_++))); + } - // Chassis rectangle (left=rear, right=front). - if (draw_chassis_) - { - rect_config["start_position"][0] = chassis_start_[0]; - rect_config["start_position"][1] = chassis_start_[1]; - rect_config["end_position"][0] = chassis_end_[0]; - rect_config["end_position"][1] = chassis_end_[1]; - graph_vector_.insert(std::make_pair("chassis", new Graph(rect_config, base_, id_++))); + // Link 1 and Link 2. + line_config["start_position"][0] = origin_point_[0]; + line_config["start_position"][1] = origin_point_[1]; + line_config["end_position"][0] = origin_point_[0]; + line_config["end_position"][1] = origin_point_[1]; + graph_vector_.insert(std::make_pair("link1", new Graph(line_config, base_, id_++))); + graph_vector_.insert(std::make_pair("link2", new Graph(line_config, base_, id_++))); } - // Link 1 and Link 2. - line_config["start_position"][0] = origin_point_[0]; - line_config["start_position"][1] = origin_point_[1]; - line_config["end_position"][0] = origin_point_[0]; - line_config["end_position"][1] = origin_point_[1]; - graph_vector_.insert(std::make_pair("link1", new Graph(line_config, base_, id_++))); - graph_vector_.insert(std::make_pair("link2", new Graph(line_config, base_, id_++))); - } + void calculatePointPosition(const rm_msgs::LeggedChassisStatusConstPtr& data, const ros::Time& time); - void calculatePointPosition(const rm_msgs::LeggedChassisStatusConstPtr& data, const ros::Time& time); + private: + void updateConfig() override; -private: - void updateConfig() override; - - int chassis_start_[2]{ 800, 600 }; - int chassis_end_[2]{ 1120, 720 }; - int origin_point_[2]{ 960, 660 }; // hip/pivot point in screen coordinates + int chassis_start_[2]{ 800, 600 }; + int chassis_end_[2]{ 1120, 720 }; + int origin_point_[2]{ 960, 660 }; // hip/pivot point in screen coordinates - bool draw_chassis_{ true }; + bool draw_chassis_{ true }; - // IK config - double link1_length_m_{ 0.21 }; - double link2_length_m_{ 0.248 }; - double pixels_per_meter_{ 600.0 }; - std::string leg_side_{ "left" }; // "left" or "right" + // IK config + double link1_length_m_{ 0.21 }; + double link2_length_m_{ 0.248 }; + double pixels_per_meter_{ 600.0 }; + std::string leg_side_{ "left" }; // "left" or "right" - // Latest input (virtual rod) - double virtual_rod_length_m_{ 0.0 }; - double virtual_rod_theta_rad_{ 0.0 }; + // Latest input (virtual rod) + double virtual_rod_length_m_{ 0.0 }; + double virtual_rod_theta_rad_{ 0.0 }; - // Solved points (screen coordinates) - int knee_point_[2]{ 960, 660 }; - int foot_point_[2]{ 960, 660 }; + // Solved points (screen coordinates) + int knee_point_[2]{ 960, 660 }; + int foot_point_[2]{ 960, 660 }; - // Keep IK solution continuous across updates (avoid knee flipping between the two possible IK branches). - bool knee_initialized_{ false }; -}; + // Keep IK solution continuous across updates (avoid knee flipping between the two possible IK branches). + bool knee_initialized_{ false }; + }; } // namespace rm_referee diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index ba083722..22262eb1 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -607,7 +607,7 @@ void EnemyColorTimeChangeUi::updateEnemyColorData(const std_msgs::Bool::ConstPtr void EnemyColorTimeChangeUi::updateConfig() { char temp_content[32]; - snprintf(temp_content, sizeof(temp_content), "%s", enemy_color_ ? "BLUE" : "RED" ); + snprintf(temp_content, sizeof(temp_content), "%s", enemy_color_ ? "BLUE" : "RED"); std::string enemy_color_content(temp_content); graph_->setContent(enemy_color_content); graph_->setColor(enemy_color_ ? rm_referee::GraphColor::CYAN : rm_referee::GraphColor::PINK); From 9cdc9d64427703935d0f7c736988ec1ef393b931 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 17 Oct 2026 21:48:12 +0800 Subject: [PATCH 7/8] Roll back the code. --- .../include/rm_referee/ui/time_change_ui.h | 322 +++++------------- 1 file changed, 89 insertions(+), 233 deletions(-) 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 acd9873e..f462b143 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -6,10 +6,6 @@ #include #include "rm_referee/ui/ui_base.h" -#include - -#include - namespace rm_referee { class TimeChangeUi : public UiBase @@ -163,6 +159,8 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi robot_height_ = data["height"]; camera_range_ = data["camera_range"]; surface_coefficient_ = data["surface_coefficient"]; + if (data.hasMember("x_offset")) + x_offset_ = data["x_offset"]; } else ROS_WARN("LaneLineTimeChangeGroupUi config 's member 'data' not defined."); @@ -187,6 +185,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi protected: std::string reference_frame_; double robot_radius_, robot_height_, camera_range_, surface_coefficient_ = 0.5; + double x_offset_ = 0.0; double pitch_angle_ = 0., screen_x_ = 1920, screen_y_ = 1080; double end_point_a_angle_, end_point_b_angle_; @@ -355,256 +354,113 @@ class DeployDistanceTimeChangeUi : public TimeChangeUi : TimeChangeUi(rpc_value, base, "deploy_distance", graph_queue, character_queue){}; void updateDeployDistanceData(const geometry_msgs::PointConstPtr& data); - class DeployDistanceTimeChangeUi : public TimeChangeUi +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) { - 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_{}; - }; + } + void updateFeedforwardCountdown(int feedforward_countdown); - class HeroLegTimeChangeUi : public TimeChangeUi +private: + void updateConfig() override; + int feedforward_countdown_{}; +}; +class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi +{ +public: + explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) { - 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) + if (rpc_value.hasMember("data")) { + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + ori_x_ = static_cast(data["ori_x"]); + ori_y_ = static_cast(data["ori_y"]); } - void updateFeedforwardCountdown(int feedforward_countdown); + else + ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined."); - private: - void updateConfig() override; - int feedforward_countdown_{}; - }; - class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi - { - public: - explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) - { - if (rpc_value.hasMember("data")) - { - XmlRpc::XmlRpcValue& data = rpc_value["data"]; - ori_x_ = static_cast(data["ori_x"]); - ori_y_ = static_cast(data["ori_y"]); - } - else - ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined."); - - graph_vector_.insert( - std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert( - std::pair(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert( - std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); - }; - void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data); - - private: - void updateConfig() override; - int ori_x_, ori_y_; - double angle_; - int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, - right_line_y2_; + graph_vector_.insert( + std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); }; + void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data); - class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi - { - public: - explicit FriendBulletsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeGroupUi(rpc_value, base, "friend_bullets", graph_queue, character_queue) - { - graph_vector_.insert(std::pair("hero", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert(std::pair("standard3", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert(std::pair("standard4", new Graph(rpc_value["config"], base_, id_++))); - graph_vector_.insert(std::pair("standard5", new Graph(rpc_value["config"], base_, id_++))); - int ui_start_y = 0; - for (auto it = graph_vector_.begin(); it != graph_vector_.end(); ++it) - { - if (it == graph_vector_.begin()) - ui_start_y = it->second->getConfig().start_y; - else - { - ui_start_y -= 40; - it->second->setStartY(ui_start_y); - } - } - }; - void updateBulletsData(const rm_referee::BulletNumData& data); - - private: - void updateConfig() override; - int hero_bullets_{ 1 }, standard3_bullets_{ 3 }, standard4_bullets_{ 4 }, standard5_bullets_{ 5 }; - }; +private: + void updateConfig() override; + int ori_x_, ori_y_; + double angle_; + int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, + right_line_y2_; +}; - class TargetHpTimeChangeUi : public TimeChangeUi +class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi +{ +public: + explicit FriendBulletsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "friend_bullets", graph_queue, character_queue) { - public: - explicit TargetHpTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeUi(rpc_value, base, "target_hp", graph_queue, character_queue) + graph_vector_.insert(std::pair("hero", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard3", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard4", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard5", new Graph(rpc_value["config"], base_, id_++))); + int ui_start_y = 0; + for (auto it = graph_vector_.begin(); it != graph_vector_.end(); ++it) { - if (rpc_value.hasMember("enemy_id")) + if (it == graph_vector_.begin()) + ui_start_y = it->second->getConfig().start_y; + else { - XmlRpc::XmlRpcValue& enemy_id = rpc_value["enemy_id"]; - for (int i = 0; i < enemy_id.size(); i++) - { - int id = static_cast(enemy_id[i]); - enemy_robot_hp_[id] = 0; - } + ui_start_y -= 40; + it->second->setStartY(ui_start_y); } } - void setEnemyHp(const rm_msgs::GameRobotHp& data); - void updateTrackID(int id); - void updateTargeHptData(); - - private: - void updateConfig() override; - std::map enemy_robot_hp_; - int target_hp_{}, target_id_{}; }; + void updateBulletsData(const rm_referee::BulletNumData& data); + +private: + void updateConfig() override; + int hero_bullets_{ 1 }, standard3_bullets_{ 3 }, standard4_bullets_{ 4 }, standard5_bullets_{ 5 }; +}; - class LegThetaTimeChangeGroupUi : public TimeChangeGroupUi +class TargetHpTimeChangeUi : public TimeChangeUi +{ +public: + explicit TargetHpTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "target_hp", graph_queue, character_queue) { - public: - explicit LegThetaTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeGroupUi(rpc_value, base, "leg_theta", graph_queue, character_queue) + if (rpc_value.hasMember("enemy_id")) { - XmlRpc::XmlRpcValue line_config; - line_config["type"] = "line"; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("color")) - line_config["color"] = rpc_value["config"]["color"]; - else - line_config["color"] = "cyan"; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("width")) - line_config["width"] = rpc_value["config"]["width"]; - else - line_config["width"] = 2; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("delay")) - line_config["delay"] = rpc_value["config"]["delay"]; - else - line_config["delay"] = 0.2; - - // Virtual rod is rendered as multiple short segments (dash effect). - XmlRpc::XmlRpcValue virtual_rod_config = line_config; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_color")) - virtual_rod_config["color"] = rpc_value["config"]["virtual_rod_color"]; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_width")) - virtual_rod_config["width"] = rpc_value["config"]["virtual_rod_width"]; - else - virtual_rod_config["width"] = 1; - - XmlRpc::XmlRpcValue rect_config; - rect_config["type"] = "rectangle"; - if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("chassis_color")) - rect_config["color"] = rpc_value["config"]["chassis_color"]; - else - rect_config["color"] = line_config["color"]; - rect_config["width"] = line_config["width"]; - rect_config["delay"] = line_config["delay"]; - - if (rpc_value.hasMember("data")) + XmlRpc::XmlRpcValue& enemy_id = rpc_value["enemy_id"]; + for (int i = 0; i < enemy_id.size(); i++) { - XmlRpc::XmlRpcValue data = rpc_value["data"]; - if (data.hasMember("draw_chassis")) - draw_chassis_ = static_cast(data["draw_chassis"]); - if (data.hasMember("chassis_start") && data.hasMember("chassis_end")) - { - chassis_start_[0] = static_cast(data["chassis_start"][0]); - chassis_start_[1] = static_cast(data["chassis_start"][1]); - chassis_end_[0] = static_cast(data["chassis_end"][0]); - chassis_end_[1] = static_cast(data["chassis_end"][1]); - } - else - { - ROS_WARN("LegThetaTimeChangeGroupUi: 'data.chassis_start/end' not defined, using default rectangle."); - chassis_start_[0] = 800; - chassis_start_[1] = 600; - chassis_end_[0] = 1120; - chassis_end_[1] = 720; - } - - if (data.hasMember("origin_point")) - { - origin_point_[0] = static_cast(data["origin_point"][0]); - origin_point_[1] = static_cast(data["origin_point"][1]); - } - else - { - // Default: rectangle center. - origin_point_[0] = (chassis_start_[0] + chassis_end_[0]) / 2; - origin_point_[1] = (chassis_start_[1] + chassis_end_[1]) / 2; - } - - if (data.hasMember("link1_length")) - link1_length_m_ = static_cast(data["link1_length"]); - if (data.hasMember("link2_length")) - link2_length_m_ = static_cast(data["link2_length"]); - if (data.hasMember("pixels_per_meter")) - pixels_per_meter_ = static_cast(data["pixels_per_meter"]); - if (data.hasMember("leg_side")) - leg_side_ = static_cast(data["leg_side"]); + int id = static_cast(enemy_id[i]); + enemy_robot_hp_[id] = 0; } - else - { - ROS_WARN("LegThetaTimeChangeGroupUi config 's member 'data' not defined."); - } - - // Chassis rectangle (left=rear, right=front). - if (draw_chassis_) - { - rect_config["start_position"][0] = chassis_start_[0]; - rect_config["start_position"][1] = chassis_start_[1]; - rect_config["end_position"][0] = chassis_end_[0]; - rect_config["end_position"][1] = chassis_end_[1]; - graph_vector_.insert(std::make_pair("chassis", new Graph(rect_config, base_, id_++))); - } - - // Link 1 and Link 2. - line_config["start_position"][0] = origin_point_[0]; - line_config["start_position"][1] = origin_point_[1]; - line_config["end_position"][0] = origin_point_[0]; - line_config["end_position"][1] = origin_point_[1]; - graph_vector_.insert(std::make_pair("link1", new Graph(line_config, base_, id_++))); - graph_vector_.insert(std::make_pair("link2", new Graph(line_config, base_, id_++))); } + } + void setEnemyHp(const rm_msgs::GameRobotHp& data); + void updateTrackID(int id); + void updateTargeHptData(); - void calculatePointPosition(const rm_msgs::LeggedChassisStatusConstPtr& data, const ros::Time& time); - - private: - void updateConfig() override; - - int chassis_start_[2]{ 800, 600 }; - int chassis_end_[2]{ 1120, 720 }; - int origin_point_[2]{ 960, 660 }; // hip/pivot point in screen coordinates - - bool draw_chassis_{ true }; - - // IK config - double link1_length_m_{ 0.21 }; - double link2_length_m_{ 0.248 }; - double pixels_per_meter_{ 600.0 }; - std::string leg_side_{ "left" }; // "left" or "right" - - // Latest input (virtual rod) - double virtual_rod_length_m_{ 0.0 }; - double virtual_rod_theta_rad_{ 0.0 }; - - // Solved points (screen coordinates) - int knee_point_[2]{ 960, 660 }; - int foot_point_[2]{ 960, 660 }; - - // Keep IK solution continuous across updates (avoid knee flipping between the two possible IK branches). - bool knee_initialized_{ false }; - }; +private: + void updateConfig() override; + std::map enemy_robot_hp_; + int target_hp_{}, target_id_{}; +}; } // namespace rm_referee From af8b42762e6f09590685ee688e3a97cdfc8ee262 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 17 Oct 2026 21:56:42 +0800 Subject: [PATCH 8/8] Re-merge time change ui. --- .../include/rm_referee/ui/time_change_ui.h | 142 +++++++++++++++++- 1 file changed, 139 insertions(+), 3 deletions(-) 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 f462b143..d1fdae37 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -6,6 +6,10 @@ #include #include "rm_referee/ui/ui_base.h" +#include + +#include + namespace rm_referee { class TimeChangeUi : public UiBase @@ -159,8 +163,6 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi robot_height_ = data["height"]; camera_range_ = data["camera_range"]; surface_coefficient_ = data["surface_coefficient"]; - if (data.hasMember("x_offset")) - x_offset_ = data["x_offset"]; } else ROS_WARN("LaneLineTimeChangeGroupUi config 's member 'data' not defined."); @@ -185,7 +187,6 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi protected: std::string reference_frame_; double robot_radius_, robot_height_, camera_range_, surface_coefficient_ = 0.5; - double x_offset_ = 0.0; double pitch_angle_ = 0., screen_x_ = 1920, screen_y_ = 1080; double end_point_a_angle_, end_point_b_angle_; @@ -463,4 +464,139 @@ class TargetHpTimeChangeUi : public TimeChangeUi int target_hp_{}, target_id_{}; }; +class LegThetaTimeChangeGroupUi : public TimeChangeGroupUi +{ +public: + explicit LegThetaTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "leg_theta", graph_queue, character_queue) + { + XmlRpc::XmlRpcValue line_config; + line_config["type"] = "line"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("color")) + line_config["color"] = rpc_value["config"]["color"]; + else + line_config["color"] = "cyan"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("width")) + line_config["width"] = rpc_value["config"]["width"]; + else + line_config["width"] = 2; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("delay")) + line_config["delay"] = rpc_value["config"]["delay"]; + else + line_config["delay"] = 0.2; + + // Virtual rod is rendered as multiple short segments (dash effect). + XmlRpc::XmlRpcValue virtual_rod_config = line_config; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_color")) + virtual_rod_config["color"] = rpc_value["config"]["virtual_rod_color"]; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("virtual_rod_width")) + virtual_rod_config["width"] = rpc_value["config"]["virtual_rod_width"]; + else + virtual_rod_config["width"] = 1; + + XmlRpc::XmlRpcValue rect_config; + rect_config["type"] = "rectangle"; + if (rpc_value.hasMember("config") && rpc_value["config"].hasMember("chassis_color")) + rect_config["color"] = rpc_value["config"]["chassis_color"]; + else + rect_config["color"] = line_config["color"]; + rect_config["width"] = line_config["width"]; + rect_config["delay"] = line_config["delay"]; + + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue data = rpc_value["data"]; + if (data.hasMember("draw_chassis")) + draw_chassis_ = static_cast(data["draw_chassis"]); + if (data.hasMember("chassis_start") && data.hasMember("chassis_end")) + { + chassis_start_[0] = static_cast(data["chassis_start"][0]); + chassis_start_[1] = static_cast(data["chassis_start"][1]); + chassis_end_[0] = static_cast(data["chassis_end"][0]); + chassis_end_[1] = static_cast(data["chassis_end"][1]); + } + else + { + ROS_WARN("LegThetaTimeChangeGroupUi: 'data.chassis_start/end' not defined, using default rectangle."); + chassis_start_[0] = 800; + chassis_start_[1] = 600; + chassis_end_[0] = 1120; + chassis_end_[1] = 720; + } + + if (data.hasMember("origin_point")) + { + origin_point_[0] = static_cast(data["origin_point"][0]); + origin_point_[1] = static_cast(data["origin_point"][1]); + } + else + { + // Default: rectangle center. + origin_point_[0] = (chassis_start_[0] + chassis_end_[0]) / 2; + origin_point_[1] = (chassis_start_[1] + chassis_end_[1]) / 2; + } + + if (data.hasMember("link1_length")) + link1_length_m_ = static_cast(data["link1_length"]); + if (data.hasMember("link2_length")) + link2_length_m_ = static_cast(data["link2_length"]); + if (data.hasMember("pixels_per_meter")) + pixels_per_meter_ = static_cast(data["pixels_per_meter"]); + if (data.hasMember("leg_side")) + leg_side_ = static_cast(data["leg_side"]); + } + else + { + ROS_WARN("LegThetaTimeChangeGroupUi config 's member 'data' not defined."); + } + + // Chassis rectangle (left=rear, right=front). + if (draw_chassis_) + { + rect_config["start_position"][0] = chassis_start_[0]; + rect_config["start_position"][1] = chassis_start_[1]; + rect_config["end_position"][0] = chassis_end_[0]; + rect_config["end_position"][1] = chassis_end_[1]; + graph_vector_.insert(std::make_pair("chassis", new Graph(rect_config, base_, id_++))); + } + + // Link 1 and Link 2. + line_config["start_position"][0] = origin_point_[0]; + line_config["start_position"][1] = origin_point_[1]; + line_config["end_position"][0] = origin_point_[0]; + line_config["end_position"][1] = origin_point_[1]; + graph_vector_.insert(std::make_pair("link1", new Graph(line_config, base_, id_++))); + graph_vector_.insert(std::make_pair("link2", new Graph(line_config, base_, id_++))); + } + + void calculatePointPosition(const rm_msgs::LeggedChassisStatusConstPtr& data, const ros::Time& time); + +private: + void updateConfig() override; + + int chassis_start_[2]{ 800, 600 }; + int chassis_end_[2]{ 1120, 720 }; + int origin_point_[2]{ 960, 660 }; // hip/pivot point in screen coordinates + + bool draw_chassis_{ true }; + + // IK config + double link1_length_m_{ 0.21 }; + double link2_length_m_{ 0.248 }; + double pixels_per_meter_{ 600.0 }; + std::string leg_side_{ "left" }; // "left" or "right" + + // Latest input (virtual rod) + double virtual_rod_length_m_{ 0.0 }; + double virtual_rod_theta_rad_{ 0.0 }; + + // Solved points (screen coordinates) + int knee_point_[2]{ 960, 660 }; + int foot_point_[2]{ 960, 660 }; + + // Keep IK solution continuous across updates (avoid knee flipping between the two possible IK branches). + bool knee_initialized_{ false }; +}; + } // namespace rm_referee