From dc8c5fccde586c63cb37ad96e5eaa018b75fd613 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 14 Mar 2026 03:13:19 +0000 Subject: [PATCH 01/32] Add new refree. --- .../include/rm_common/decision/heat_limit.h | 4 - rm_msgs/CMakeLists.txt | 12 ++ rm_msgs/msg/referee/Buff.msg | 2 +- rm_msgs/msg/referee/BulletAllowance.msg | 1 + rm_msgs/msg/referee/ClientMapReceiveData.msg | 4 +- rm_msgs/msg/referee/ClientMapSendData.msg | 2 +- rm_msgs/msg/referee/DartClientCmd.msg | 1 + rm_msgs/msg/referee/DartRemainingTime.msg | 4 + rm_msgs/msg/referee/EnemyHpInfo.msg | 6 + rm_msgs/msg/referee/GameRobotHp.msg | 21 +- rm_msgs/msg/referee/GameRobotStatus.msg | 2 + rm_msgs/msg/referee/PowerHeatData.msg | 1 - rm_msgs/msg/referee/RadarCmd.msg | 8 + rm_msgs/msg/referee/RadarInfo.msg | 4 + rm_msgs/msg/referee/RadarMarkData.msg | 13 ++ .../referee/RadarWirelessEnemyCallSign.msg | 3 + .../RadarWirelessEnemyCoinAndFieldStatus.msg | 5 + .../RadarWirelessEnemyProjectileAllowance.msg | 7 + .../referee/RadarWirelessEnemyRobotBuff.msg | 32 +++ .../msg/referee/RadarWirelessEnemyRobotHp.msg | 8 + .../referee/RadarWirelessEnemyRobotPos.msg | 14 ++ rm_msgs/msg/referee/RfidStatus.msg | 31 ++- rm_msgs/msg/referee/RobotCustomData2.msg | 3 + rm_msgs/msg/referee/SentryCmd.msg | 9 +- rm_msgs/msg/referee/SentryInfo.msg | 9 + rm_referee/include/rm_referee/common/data.h | 1 + .../include/rm_referee/common/protocol.h | 200 +++++++++++------- rm_referee/include/rm_referee/referee_base.h | 2 +- .../include/rm_referee/ui/interactive_data.h | 2 +- rm_referee/src/referee.cpp | 166 +++++++++------ rm_referee/src/referee_base.cpp | 54 ++--- rm_referee/src/ui/flash_ui.cpp | 12 +- rm_referee/src/ui/interactive_data.cpp | 39 +++- rm_referee/src/ui/time_change_ui.cpp | 21 +- 34 files changed, 468 insertions(+), 235 deletions(-) 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/RobotCustomData2.msg diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index d4c3365b..e072d9c2 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -128,10 +128,6 @@ class HeatLimit { shooter_cooling_heat_ = data.shooter_id_1_17_mm_cooling_heat; } - else if (type_ == "ID2_17MM") - { - shooter_cooling_heat_ = data.shooter_id_2_17_mm_cooling_heat; - } else if (type_ == "ID1_42MM") { shooter_cooling_heat_ = data.shooter_id_1_42_mm_cooling_heat; diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index aab8d4f5..a20f9ea9 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -49,6 +49,8 @@ add_service_files( CameraStatus.srv EnableImuTrigger.srv EnableGyro.srv + ExtraFrictionWheelSpeed.srv + ShooterSpeed.srv ) add_message_files( @@ -81,19 +83,29 @@ 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 + RobotCustomData2.msg VisualizeStateData.msg PowerManagementSampleAndStatusData.msg PowerManagementInitializationExceptionData.msg diff --git a/rm_msgs/msg/referee/Buff.msg b/rm_msgs/msg/referee/Buff.msg index eea334e3..60609a50 100644 --- a/rm_msgs/msg/referee/Buff.msg +++ b/rm_msgs/msg/referee/Buff.msg @@ -1,5 +1,5 @@ uint8 recovery_buff -uint8 cooling_buff +uint16 cooling_buff uint8 defence_buff uint8 vulnerability_buff uint16 attack_buff diff --git a/rm_msgs/msg/referee/BulletAllowance.msg b/rm_msgs/msg/referee/BulletAllowance.msg index ef09835a..ef488e51 100644 --- a/rm_msgs/msg/referee/BulletAllowance.msg +++ b/rm_msgs/msg/referee/BulletAllowance.msg @@ -1,5 +1,6 @@ uint16 bullet_allowance_num_17_mm uint16 bullet_allowance_num_42_mm uint16 coin_remaining_num +uint16 projectile_allowance_fortress time stamp diff --git a/rm_msgs/msg/referee/ClientMapReceiveData.msg b/rm_msgs/msg/referee/ClientMapReceiveData.msg index d78a250d..c3407e31 100644 --- a/rm_msgs/msg/referee/ClientMapReceiveData.msg +++ b/rm_msgs/msg/referee/ClientMapReceiveData.msg @@ -6,8 +6,8 @@ uint16 infantry_3_position_x uint16 infantry_3_position_y uint16 infantry_4_position_x uint16 infantry_4_position_y -uint16 infantry_5_position_x -uint16 infantry_5_position_y +uint16 reserved_1 +uint16 reserved_2 uint16 sentry_position_x uint16 sentry_position_y diff --git a/rm_msgs/msg/referee/ClientMapSendData.msg b/rm_msgs/msg/referee/ClientMapSendData.msg index 991c4c9a..7009a559 100644 --- a/rm_msgs/msg/referee/ClientMapSendData.msg +++ b/rm_msgs/msg/referee/ClientMapSendData.msg @@ -27,4 +27,4 @@ float32 target_position_x float32 target_position_y uint8 command_keyboard uint8 target_robot_ID -uint8 cmd_source +uint16 cmd_source diff --git a/rm_msgs/msg/referee/DartClientCmd.msg b/rm_msgs/msg/referee/DartClientCmd.msg index 2aa89c9e..5ae3188a 100644 --- a/rm_msgs/msg/referee/DartClientCmd.msg +++ b/rm_msgs/msg/referee/DartClientCmd.msg @@ -1,4 +1,5 @@ uint8 dart_launch_opening_status +uint8 reserved uint16 target_change_time uint16 latest_launch_cmd_time 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/GameRobotHp.msg b/rm_msgs/msg/referee/GameRobotHp.msg index c93622cd..08a1adc2 100644 --- a/rm_msgs/msg/referee/GameRobotHp.msg +++ b/rm_msgs/msg/referee/GameRobotHp.msg @@ -1,16 +1,9 @@ -uint16 red_1_robot_hp -uint16 red_2_robot_hp -uint16 red_3_robot_hp -uint16 red_4_robot_hp -uint16 red_7_robot_hp -uint16 red_outpost_hp -uint16 red_base_hp -uint16 blue_1_robot_hp -uint16 blue_2_robot_hp -uint16 blue_3_robot_hp -uint16 blue_4_robot_hp -uint16 blue_7_robot_hp -uint16 blue_outpost_hp -uint16 blue_base_hp +uint16 ally_1_robot_hp +uint16 ally_2_robot_hp +uint16 ally_3_robot_hp +uint16 ally_4_robot_hp +uint16 ally_7_robot_hp +uint16 ally_outpost_hp +uint16 ally_base_hp time stamp diff --git a/rm_msgs/msg/referee/GameRobotStatus.msg b/rm_msgs/msg/referee/GameRobotStatus.msg index b09a90f1..91b5444c 100644 --- a/rm_msgs/msg/referee/GameRobotStatus.msg +++ b/rm_msgs/msg/referee/GameRobotStatus.msg @@ -7,6 +7,7 @@ uint8 RED_AERIAL = 6 uint8 RED_SENTRY = 7 uint8 RED_DART = 8 uint8 RED_RADAR = 9 +uint8 RED_OUTPOST = 10 uint8 RED_OUTPUT = 10 uint8 RED_BASE = 11 uint8 BLUE_HERO = 101 @@ -18,6 +19,7 @@ uint8 BLUE_AERIAL = 106 uint8 BLUE_SENTRY = 107 uint8 BLUE_DART = 108 uint8 BLUE_RADAR = 109 +uint8 BLUE_OUTPOST = 110 uint8 BLUE_OUTPUT = 110 uint8 BLUE_BASE = 111 diff --git a/rm_msgs/msg/referee/PowerHeatData.msg b/rm_msgs/msg/referee/PowerHeatData.msg index 7981cd91..1d3ee282 100644 --- a/rm_msgs/msg/referee/PowerHeatData.msg +++ b/rm_msgs/msg/referee/PowerHeatData.msg @@ -1,6 +1,5 @@ uint16 chassis_power_buffer uint16 shooter_id_1_17_mm_cooling_heat -uint16 shooter_id_2_17_mm_cooling_heat uint16 shooter_id_1_42_mm_cooling_heat time stamp 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/RadarInfo.msg b/rm_msgs/msg/referee/RadarInfo.msg index ee153411..3a65f781 100644 --- a/rm_msgs/msg/referee/RadarInfo.msg +++ b/rm_msgs/msg/referee/RadarInfo.msg @@ -1 +1,5 @@ uint8 radar_info +uint8 double_vulnerability_chances +bool enemy_in_double_vulnerability +uint8 own_encryption_level +bool can_modify_key diff --git a/rm_msgs/msg/referee/RadarMarkData.msg b/rm_msgs/msg/referee/RadarMarkData.msg index 237e9393..dd8dcab7 100644 --- a/rm_msgs/msg/referee/RadarMarkData.msg +++ b/rm_msgs/msg/referee/RadarMarkData.msg @@ -1,3 +1,16 @@ +uint16 mark_progress +bool enemy_hero_vulnerable +bool enemy_engineer_vulnerable +bool enemy_standard_3_vulnerable +bool enemy_standard_4_vulnerable +bool enemy_aerial_special_mark +bool enemy_sentry_vulnerable +bool own_hero_special_mark +bool own_engineer_special_mark +bool own_standard_3_special_mark +bool own_standard_4_special_mark +bool own_aerial_special_mark +bool own_sentry_special_mark bool mark_hero_progress bool mark_engineer_progress bool mark_standard_3_progress 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..108f4c78 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg @@ -0,0 +1,32 @@ +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 + +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/RfidStatus.msg b/rm_msgs/msg/referee/RfidStatus.msg index 0a984c0a..ce914640 100644 --- a/rm_msgs/msg/referee/RfidStatus.msg +++ b/rm_msgs/msg/referee/RfidStatus.msg @@ -1,3 +1,6 @@ +uint32 rfid_status +uint8 rfid_status_2 + bool base_buff_point_state bool own_central_elevated_ground_state bool enemy_central_elevated_ground_state @@ -15,12 +18,26 @@ bool below_road_own_terrain_span_buff_point_state bool upper_road_own_terrain_span_buff_point_state bool below_road_enemy_terrain_span_buff_point_state bool upper_road_enemy_terrain_span_buff_point_state -bool own_fort_buff_point -bool own_outpost_buff_point -bool nan_overlapping_supplier_zone -bool overlapping_supplier_zone -bool own_large_resource_island_point -bool enemy_large_resource_island_point -bool central_buff_point +bool own_fort_buff_point_state +bool own_outpost_buff_point_state +bool non_overlapping_supplier_zone_state +bool overlapping_supplier_zone_state +bool own_assembly_buff_point_state +bool enemy_assembly_buff_point_state +bool central_buff_point_state +bool enemy_fort_buff_point_state +bool enemy_outpost_buff_point_state +bool own_tunnel_road_lower_buff_point_state +bool own_tunnel_road_middle_buff_point_state +bool own_tunnel_road_upper_buff_point_state +bool own_tunnel_trapezoid_lower_buff_point_state +bool own_tunnel_trapezoid_middle_buff_point_state +bool own_tunnel_trapezoid_upper_buff_point_state +bool enemy_tunnel_road_lower_buff_point_state +bool enemy_tunnel_road_middle_buff_point_state +bool enemy_tunnel_road_upper_buff_point_state +bool enemy_tunnel_trapezoid_lower_buff_point_state +bool enemy_tunnel_trapezoid_middle_buff_point_state +bool enemy_tunnel_trapezoid_upper_buff_point_state 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 diff --git a/rm_msgs/msg/referee/SentryCmd.msg b/rm_msgs/msg/referee/SentryCmd.msg index 5295edf2..1d90c706 100644 --- a/rm_msgs/msg/referee/SentryCmd.msg +++ b/rm_msgs/msg/referee/SentryCmd.msg @@ -1 +1,8 @@ -uint32 sentry_info +uint32 sentry_cmd +bool confirm_respawn +bool confirm_instant_respawn +uint16 bullet_exchange_target +uint8 remote_bullet_exchange_req_cnt +uint8 remote_hp_exchange_req_cnt +uint8 posture_cmd +bool confirm_rune_activating diff --git a/rm_msgs/msg/referee/SentryInfo.msg b/rm_msgs/msg/referee/SentryInfo.msg index f9319cbe..8ba91b2d 100644 --- a/rm_msgs/msg/referee/SentryInfo.msg +++ b/rm_msgs/msg/referee/SentryInfo.msg @@ -1,3 +1,12 @@ uint32 sentry_info +uint16 sentry_info_2 +uint16 exchanged_bullet_allowance +uint8 remote_bullet_exchange_success_cnt +uint8 remote_hp_exchange_success_cnt +bool can_confirm_free_respawn +bool can_exchange_instant_respawn +uint16 instant_respawn_cost bool is_out_of_war uint16 remaining_bullets_can_supply +uint8 sentry_mode +bool can_activate_energy_mechanism diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index d6ffba28..f2adff30 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -87,6 +87,7 @@ #include "rm_msgs/SentryInfo.h" #include "rm_msgs/SentryCmd.h" #include "rm_msgs/RadarInfo.h" +#include "rm_msgs/RadarCmd.h" #include "rm_msgs/Buff.h" #include "rm_msgs/TrackData.h" #include "rm_msgs/SentryAttackingTarget.h" diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 48a95324..2b524d6f 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -47,8 +47,8 @@ typedef enum GAME_STATUS_CMD = 0x0001, GAME_RESULT_CMD = 0x0002, GAME_ROBOT_HP_CMD = 0x0003, - DART_STATUS_CMD = 0x0004, - ICRA_ZONE_STATUS_CMD = 0x0005, + DART_STATUS_CMD = 0x0004, // legacy, not listed in 2026 V1.2.0 command table + ICRA_ZONE_STATUS_CMD = 0x0005, // legacy, not listed in 2026 V1.2.0 command table FIELD_EVENTS_CMD = 0x0101, SUPPLY_PROJECTILE_ACTION_CMD = 0x0102, REFEREE_WARNING_CMD = 0x0104, @@ -57,7 +57,7 @@ typedef enum POWER_HEAT_DATA_CMD = 0x0202, ROBOT_POS_CMD = 0x0203, BUFF_CMD = 0x0204, - AERIAL_ROBOT_ENERGY_CMD = 0x0205, + AERIAL_ROBOT_ENERGY_CMD = 0x0205, // legacy, not listed in 2026 V1.2.0 command table ROBOT_HURT_CMD = 0x0206, SHOOT_DATA_CMD = 0x0207, BULLET_REMAINING_CMD = 0x0208, @@ -70,12 +70,18 @@ typedef enum INTERACTIVE_DATA_CMD = 0x0301, CUSTOM_CONTROLLER_CMD = 0x0302, // controller TARGET_POS_CMD = 0x0303, - ROBOT_COMMAND_CMD = 0x0304, // controller + ROBOT_COMMAND_CMD = 0x0304, // legacy keyboard/mouse command (deleted in 2026 V1.2.0) CLIENT_MAP_CMD = 0x0305, - CUSTOM_CLIENT_CMD = 0x0306, // controller - MAP_SENTRY_CMD = 0x0307, // send sentry->aerial - CUSTOM_TO_ROBOT_CMD = 0x0308, - ROBOT_TO_CUSTOM_CMD = 0x0309, + KEYBOARD_MOUSE_CMD = 0x0306, // 2026 table 1-42 + CUSTOM_CLIENT_CMD = KEYBOARD_MOUSE_CMD, // legacy alias + MAP_SENTRY_CMD = 0x0307, // send sentry->aerial + CUSTOM_INFO_CMD = 0x0308, // robot -> custom controller text/info + ROBOT_TO_CUSTOM_CONTROLLER_CMD = 0x0309, + ROBOT_TO_CUSTOM_CLIENT_CMD = 0x0310, + CUSTOM_CLIENT_TO_ROBOT_CMD = 0x0311, + CUSTOM_TO_ROBOT_CMD = CUSTOM_INFO_CMD, // legacy misnomer alias kept for compatibility + ROBOT_TO_CUSTOM_CMD = ROBOT_TO_CUSTOM_CONTROLLER_CMD, // legacy alias + ROBOT_TO_CUSTOM_CMD_2 = ROBOT_TO_CUSTOM_CLIENT_CMD, // legacy alias POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD = 0X8301, POWER_MANAGEMENT_INITIALIZATION_EXCEPTION_CMD = 0X8302, POWER_MANAGEMENT_SYSTEM_EXCEPTION_CMD = 0X8303, @@ -239,22 +245,14 @@ typedef struct typedef struct { - uint16_t red_1_robot_hp; - uint16_t red_2_robot_hp; - uint16_t red_3_robot_hp; - uint16_t red_4_robot_hp; - uint16_t reserved_1; - uint16_t red_7_robot_hp; - uint16_t red_outpost_hp; - uint16_t red_base_hp; - uint16_t blue_1_robot_hp; - uint16_t blue_2_robot_hp; - uint16_t blue_3_robot_hp; - uint16_t blue_4_robot_hp; - uint16_t reserved_2; - uint16_t blue_7_robot_hp; - uint16_t blue_outpost_hp; - uint16_t blue_base_hp; + uint16_t ally_1_robot_hp; + uint16_t ally_2_robot_hp; + uint16_t ally_3_robot_hp; + uint16_t ally_4_robot_hp; + uint16_t reserved; + uint16_t ally_7_robot_hp; + uint16_t ally_outpost_hp; + uint16_t ally_base_hp; } __packed GameRobotHp; typedef struct @@ -343,7 +341,6 @@ typedef struct float reserved_3; uint16_t chassis_power_buffer; uint16_t shooter_id_1_17_mm_cooling_heat; - uint16_t shooter_id_2_17_mm_cooling_heat; uint16_t shooter_id_1_42_mm_cooling_heat; } __packed PowerHeatData; @@ -357,7 +354,7 @@ typedef struct typedef struct { uint8_t recovery_buff; - uint8_t cooling_buff; + uint16_t cooling_buff; uint8_t defence_buff; uint8_t vulnerability_buff; uint16_t attack_buff; @@ -388,41 +385,19 @@ typedef struct uint16_t bullet_allowance_num_17_mm; uint16_t bullet_allowance_num_42_mm; uint16_t coin_remaining_num; + uint16_t projectile_allowance_fortress; } __packed BulletAllowance; typedef struct { - uint8_t base_buff_point_state : 1; - uint8_t own_central_elevated_ground_state : 1; - uint8_t enemy_central_elevated_ground_state : 1; - uint8_t own_trapezoidal_elevated_ground_state : 1; - uint8_t enemy_trapezoidal_elevated_ground_state : 1; - uint8_t forward_own_terrain_span_buff_point_state : 1; - uint8_t behind_own_terrain_span_buff_point_state : 1; - uint8_t forward_enemy_terrain_span_buff_point_state : 1; - uint8_t behind_enemy_terrain_span_buff_point_state : 1; - uint8_t below_central_own_terrain_span_buff_point_state : 1; - uint8_t upper_central_own_terrain_span_buff_point_state : 1; - uint8_t below_central_enemy_terrain_span_buff_point_state : 1; - uint8_t upper_central_enemy_terrain_span_buff_point_state : 1; - uint8_t below_road_own_terrain_span_buff_point_state : 1; - uint8_t upper_road_own_terrain_span_buff_point_state : 1; - uint8_t below_road_enemy_terrain_span_buff_point_state : 1; - uint8_t upper_road_enemy_terrain_span_buff_point_state : 1; - uint8_t own_fort_buff_point : 1; - uint8_t own_outpost_buff_point : 1; - uint8_t nan_overlapping_supplier_zone : 1; - uint8_t overlapping_supplier_zone : 1; - uint8_t own_large_resource_island_point : 1; - uint8_t enemy_large_resource_island_point : 1; - uint8_t central_buff_point : 1; - uint32_t reversed : 8; + uint32_t rfid_status; + uint8_t rfid_status_2; } __packed RfidStatus; typedef struct { uint8_t dart_launch_opening_status; - uint8_t reversed; + uint8_t reserved; uint16_t target_change_time; uint16_t latest_launch_cmd_time; } __packed DartClientCmd; @@ -495,13 +470,25 @@ typedef struct float reserved_2; } __packed RobotsPositionData; -typedef struct +typedef union { - uint8_t mark_hero_progress : 1; - uint8_t mark_engineer_progress : 1; - uint8_t mark_standard_3_progress : 1; - uint8_t mark_standard_4_progress : 1; - uint8_t mark_sentry_progress : 1; + uint16_t mark_progress; + struct + { + uint16_t enemy_hero_vulnerable : 1; + uint16_t enemy_engineer_vulnerable : 1; + uint16_t enemy_standard_3_vulnerable : 1; + uint16_t enemy_standard_4_vulnerable : 1; + uint16_t enemy_aerial_special_mark : 1; + uint16_t enemy_sentry_vulnerable : 1; + uint16_t own_hero_special_mark : 1; + uint16_t own_engineer_special_mark : 1; + uint16_t own_standard_3_special_mark : 1; + uint16_t own_standard_4_special_mark : 1; + uint16_t own_aerial_special_mark : 1; + uint16_t own_sentry_special_mark : 1; + uint16_t reserved : 4; + }; } __packed RadarMarkData; typedef struct @@ -541,18 +528,65 @@ typedef struct uint8_t data; } __packed InteractiveData; -typedef struct +typedef union { - InteractiveDataHeader header; - uint32_t sentry_info; + uint32_t sentry_cmd; + struct + { + uint32_t confirm_respawn : 1; + uint32_t confirm_instant_respawn : 1; + uint32_t bullet_exchange_target : 11; + uint32_t remote_bullet_exchange_req_cnt : 4; + uint32_t remote_hp_exchange_req_cnt : 4; + uint32_t posture_cmd : 2; + uint32_t confirm_rune_activating : 1; + uint32_t reserved : 8; + }; } __packed SentryCmd; typedef struct { InteractiveDataHeader header; + SentryCmd sentry_cmd; +} __packed SentryCmdInteractiveData; + +typedef union +{ uint8_t radar_info; + struct + { + uint8_t double_vulnerability_chances : 2; + uint8_t enemy_in_double_vulnerability : 1; + uint8_t own_encryption_level : 2; + uint8_t can_modify_key : 1; + uint8_t reserved : 2; + }; } __packed RadarInfo; +typedef struct +{ + InteractiveDataHeader header; + RadarInfo radar_info; +} __packed RadarInfoInteractiveData; + +typedef struct +{ + uint8_t radar_cmd; + uint8_t password_cmd; + uint8_t password_1; + uint8_t password_2; + uint8_t password_3; + uint8_t password_4; + uint8_t password_5; + uint8_t password_6; +} __packed RadarCmd; + +typedef struct +{ + InteractiveDataHeader header; + RadarCmd radar_cmd; +} __packed RadarCmdInteractiveData; + typedef struct { uint8_t data[30]; @@ -560,10 +594,32 @@ typedef struct typedef struct { - uint32_t sentry_info; - uint16_t is_out_of_war : 1; - uint16_t remaining_bullets_can_supply : 11; - uint16_t reverse : 4; + union + { + uint32_t sentry_info; + struct + { + uint32_t exchanged_bullet_allowance : 11; + uint32_t remote_bullet_exchange_success_cnt : 4; + uint32_t remote_hp_exchange_success_cnt : 4; + uint32_t can_confirm_free_respawn : 1; + uint32_t can_exchange_instant_respawn : 1; + uint32_t instant_respawn_cost : 10; + uint32_t reserved : 1; + }; + }; + union + { + uint16_t sentry_info_2; + struct + { + uint16_t is_out_of_war : 1; + uint16_t remaining_bullets_can_supply : 11; + uint16_t sentry_mode : 2; + uint16_t can_activate_energy_mechanism : 1; + uint16_t reserved_1 : 1; + }; + }; } __packed SentryInfo; typedef struct @@ -572,7 +628,7 @@ typedef struct float target_position_y; uint8_t command_keyboard; uint8_t target_robot_ID; - uint8_t cmd_source; + uint16_t cmd_source; } __packed ClientMapSendData; typedef struct @@ -585,8 +641,8 @@ typedef struct uint16_t infantry_3_position_y; uint16_t infantry_4_position_x; uint16_t infantry_4_position_y; - uint16_t infantry_5_position_x; - uint16_t infantry_5_position_y; + uint16_t reserved_1; + uint16_t reserved_2; uint16_t sentry_position_x; uint16_t sentry_position_y; } __packed ClientMapReceiveData; @@ -647,10 +703,10 @@ typedef struct { uint8_t chassis_power_high_8_bit; uint8_t chassis_power_low_8_bit; - uint8_t chassis_expect_power_high_8_bit; - uint8_t chassis_expect_power_low_8_bit; - uint8_t capacity_recent_charge_power_high_8_bit; - uint8_t capacity_recent_charge_power_low_8_bit; + uint8_t cap_error_flag_high_8_bit; + uint8_t cap_error_flag_low_8_bit; + uint8_t cap_received_msg_high_8_bit; + uint8_t cap_received_msg_low_8_bit; uint8_t capacity_remain_charge_high_8_bit; uint8_t capacity_remain_charge_low_8_bit; uint8_t capacity_expect_charge_power; diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index dc750cac..6217da4c 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -59,7 +59,7 @@ class RefereeBase virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data); virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data); virtual void sendSentryCmdCallback(const rm_msgs::SentryCmdConstPtr& data); - virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data); + virtual void sendRadarCmdCallback(const rm_msgs::RadarCmdConstPtr& data); virtual void sendCustomInfoCallback(const std_msgs::StringConstPtr& data); virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data); diff --git a/rm_referee/include/rm_referee/ui/interactive_data.h b/rm_referee/include/rm_referee/ui/interactive_data.h index 6c3d6729..17c07e2a 100644 --- a/rm_referee/include/rm_referee/ui/interactive_data.h +++ b/rm_referee/include/rm_referee/ui/interactive_data.h @@ -19,7 +19,7 @@ class InteractiveSender : public UiBase void sendRadarInteractiveData(const rm_msgs::ClientMapReceiveData::ConstPtr& data); void sendMapSentryData(const rm_referee::MapSentryData& data); void sendSentryCmdData(const rm_msgs::SentryCmdConstPtr& data); - void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); + void sendRadarCmdData(const rm_msgs::RadarCmdConstPtr& data); virtual bool needSendInteractiveData(); ros::Time last_send_time_; }; diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index d0349ad6..817976e2 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -123,20 +123,13 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::GameRobotHp game_robot_hp_data; memcpy(&game_robot_hp_ref, rx_data + 7, sizeof(rm_referee::GameRobotHp)); - game_robot_hp_data.blue_1_robot_hp = game_robot_hp_ref.blue_1_robot_hp; - game_robot_hp_data.blue_2_robot_hp = game_robot_hp_ref.blue_2_robot_hp; - game_robot_hp_data.blue_3_robot_hp = game_robot_hp_ref.blue_3_robot_hp; - game_robot_hp_data.blue_4_robot_hp = game_robot_hp_ref.blue_4_robot_hp; - game_robot_hp_data.blue_7_robot_hp = game_robot_hp_ref.blue_7_robot_hp; - game_robot_hp_data.blue_outpost_hp = game_robot_hp_ref.blue_outpost_hp; - game_robot_hp_data.blue_base_hp = game_robot_hp_ref.blue_base_hp; - game_robot_hp_data.red_1_robot_hp = game_robot_hp_ref.red_1_robot_hp; - game_robot_hp_data.red_2_robot_hp = game_robot_hp_ref.red_2_robot_hp; - game_robot_hp_data.red_3_robot_hp = game_robot_hp_ref.red_3_robot_hp; - game_robot_hp_data.red_4_robot_hp = game_robot_hp_ref.red_4_robot_hp; - game_robot_hp_data.red_7_robot_hp = game_robot_hp_ref.red_7_robot_hp; - game_robot_hp_data.red_outpost_hp = game_robot_hp_ref.red_outpost_hp; - game_robot_hp_data.red_base_hp = game_robot_hp_ref.red_base_hp; + game_robot_hp_data.ally_1_robot_hp = game_robot_hp_ref.ally_1_robot_hp; + game_robot_hp_data.ally_2_robot_hp = game_robot_hp_ref.ally_2_robot_hp; + game_robot_hp_data.ally_3_robot_hp = game_robot_hp_ref.ally_3_robot_hp; + game_robot_hp_data.ally_4_robot_hp = game_robot_hp_ref.ally_4_robot_hp; + game_robot_hp_data.ally_7_robot_hp = game_robot_hp_ref.ally_7_robot_hp; + game_robot_hp_data.ally_outpost_hp = game_robot_hp_ref.ally_outpost_hp; + game_robot_hp_data.ally_base_hp = game_robot_hp_ref.ally_base_hp; game_robot_hp_data.stamp = last_get_data_time_; referee_ui_.updateGameRobotHpDataCallBack(game_robot_hp_data); @@ -278,7 +271,6 @@ int Referee::unpack(uint8_t* rx_data) power_heat_data.chassis_power_buffer = power_heat_ref.chassis_power_buffer; power_heat_data.shooter_id_1_17_mm_cooling_heat = power_heat_ref.shooter_id_1_17_mm_cooling_heat; - power_heat_data.shooter_id_2_17_mm_cooling_heat = power_heat_ref.shooter_id_2_17_mm_cooling_heat; power_heat_data.shooter_id_1_42_mm_cooling_heat = power_heat_ref.shooter_id_1_42_mm_cooling_heat; power_heat_data.stamp = last_get_data_time_; @@ -309,16 +301,24 @@ int Referee::unpack(uint8_t* rx_data) robot_buff.cooling_buff = referee_buff.cooling_buff; robot_buff.recovery_buff = referee_buff.recovery_buff; - if (referee_buff.remaining_energy & 0x1E) + if (referee_buff.remaining_energy == 0x80) + robot_buff.remaining_energy = 100; + else if (referee_buff.remaining_energy & 0x01) + robot_buff.remaining_energy = 125; + else if (referee_buff.remaining_energy & 0x02) + robot_buff.remaining_energy = 100; + else if (referee_buff.remaining_energy & 0x04) robot_buff.remaining_energy = 50; - else if (referee_buff.remaining_energy & 0x1C) + else if (referee_buff.remaining_energy & 0x08) robot_buff.remaining_energy = 30; - else if (referee_buff.remaining_energy & 0x18) - robot_buff.remaining_energy = 15; else if (referee_buff.remaining_energy & 0x10) + robot_buff.remaining_energy = 15; + else if (referee_buff.remaining_energy & 0x20) robot_buff.remaining_energy = 5; - else if (referee_buff.remaining_energy & 0x00) + else if (referee_buff.remaining_energy & 0x40) robot_buff.remaining_energy = 1; + else + robot_buff.remaining_energy = 0; buff_pub_.publish(robot_buff); break; @@ -370,6 +370,7 @@ int Referee::unpack(uint8_t* rx_data) bullet_allowance_data.bullet_allowance_num_17_mm = bullet_allowance_ref.bullet_allowance_num_17_mm; bullet_allowance_data.bullet_allowance_num_42_mm = bullet_allowance_ref.bullet_allowance_num_42_mm; bullet_allowance_data.coin_remaining_num = bullet_allowance_ref.coin_remaining_num; + bullet_allowance_data.projectile_allowance_fortress = bullet_allowance_ref.projectile_allowance_fortress; bullet_allowance_data.stamp = last_get_data_time_; referee_ui_.bulletRemainDataCallBack(bullet_allowance_data, last_get_data_time_); @@ -382,44 +383,51 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::RfidStatus rfid_status_data; memcpy(&rfid_status_ref, rx_data + 7, sizeof(rm_referee::RfidStatus)); - rfid_status_data.base_buff_point_state = rfid_status_ref.base_buff_point_state; - rfid_status_data.own_central_elevated_ground_state = rfid_status_ref.own_central_elevated_ground_state; - rfid_status_data.enemy_central_elevated_ground_state = rfid_status_ref.enemy_central_elevated_ground_state; - rfid_status_data.own_trapezoidal_elevated_ground_state = - rfid_status_ref.own_trapezoidal_elevated_ground_state; - rfid_status_data.enemy_trapezoidal_elevated_ground_state = - rfid_status_ref.enemy_trapezoidal_elevated_ground_state; - rfid_status_data.forward_own_terrain_span_buff_point_state = - rfid_status_ref.forward_own_terrain_span_buff_point_state; - rfid_status_data.behind_own_terrain_span_buff_point_state = - rfid_status_ref.behind_own_terrain_span_buff_point_state; - rfid_status_data.forward_enemy_terrain_span_buff_point_state = - rfid_status_ref.forward_enemy_terrain_span_buff_point_state; - rfid_status_data.behind_enemy_terrain_span_buff_point_state = - rfid_status_ref.behind_enemy_terrain_span_buff_point_state; - rfid_status_data.below_central_own_terrain_span_buff_point_state = - rfid_status_ref.below_central_own_terrain_span_buff_point_state; - rfid_status_data.upper_central_own_terrain_span_buff_point_state = - rfid_status_ref.upper_central_own_terrain_span_buff_point_state; - rfid_status_data.below_central_enemy_terrain_span_buff_point_state = - rfid_status_ref.below_central_enemy_terrain_span_buff_point_state; - rfid_status_data.upper_central_enemy_terrain_span_buff_point_state = - rfid_status_ref.upper_central_enemy_terrain_span_buff_point_state; - rfid_status_data.below_road_own_terrain_span_buff_point_state = - rfid_status_ref.below_road_own_terrain_span_buff_point_state; - rfid_status_data.upper_road_own_terrain_span_buff_point_state = - rfid_status_ref.upper_road_own_terrain_span_buff_point_state; - rfid_status_data.below_road_enemy_terrain_span_buff_point_state = - rfid_status_ref.below_road_enemy_terrain_span_buff_point_state; - rfid_status_data.upper_road_enemy_terrain_span_buff_point_state = - rfid_status_ref.upper_road_enemy_terrain_span_buff_point_state; - rfid_status_data.own_fort_buff_point = rfid_status_ref.own_fort_buff_point; - rfid_status_data.own_outpost_buff_point = rfid_status_ref.own_outpost_buff_point; - rfid_status_data.nan_overlapping_supplier_zone = rfid_status_ref.nan_overlapping_supplier_zone; - rfid_status_data.overlapping_supplier_zone = rfid_status_ref.overlapping_supplier_zone; - rfid_status_data.own_large_resource_island_point = rfid_status_ref.own_large_resource_island_point; - rfid_status_data.enemy_large_resource_island_point = rfid_status_ref.enemy_large_resource_island_point; - rfid_status_data.central_buff_point = rfid_status_ref.central_buff_point; + const uint32_t rfid_status = rfid_status_ref.rfid_status; + const uint8_t rfid_status_2 = rfid_status_ref.rfid_status_2; + auto bit32 = [&](int bit) { return static_cast((rfid_status >> bit) & 0x1u); }; + auto bit8 = [&](int bit) { return static_cast((rfid_status_2 >> bit) & 0x1u); }; + + rfid_status_data.rfid_status = rfid_status_ref.rfid_status; + rfid_status_data.rfid_status_2 = rfid_status_ref.rfid_status_2; + rfid_status_data.base_buff_point_state = bit32(0); + rfid_status_data.own_central_elevated_ground_state = bit32(1); + rfid_status_data.enemy_central_elevated_ground_state = bit32(2); + rfid_status_data.own_trapezoidal_elevated_ground_state = bit32(3); + rfid_status_data.enemy_trapezoidal_elevated_ground_state = bit32(4); + rfid_status_data.forward_own_terrain_span_buff_point_state = bit32(5); + rfid_status_data.behind_own_terrain_span_buff_point_state = bit32(6); + rfid_status_data.forward_enemy_terrain_span_buff_point_state = bit32(7); + rfid_status_data.behind_enemy_terrain_span_buff_point_state = bit32(8); + rfid_status_data.below_central_own_terrain_span_buff_point_state = bit32(9); + rfid_status_data.upper_central_own_terrain_span_buff_point_state = bit32(10); + rfid_status_data.below_central_enemy_terrain_span_buff_point_state = bit32(11); + rfid_status_data.upper_central_enemy_terrain_span_buff_point_state = bit32(12); + rfid_status_data.below_road_own_terrain_span_buff_point_state = bit32(13); + rfid_status_data.upper_road_own_terrain_span_buff_point_state = bit32(14); + rfid_status_data.below_road_enemy_terrain_span_buff_point_state = bit32(15); + rfid_status_data.upper_road_enemy_terrain_span_buff_point_state = bit32(16); + rfid_status_data.own_fort_buff_point_state = bit32(17); + rfid_status_data.own_outpost_buff_point_state = bit32(18); + rfid_status_data.non_overlapping_supplier_zone_state = bit32(19); + rfid_status_data.overlapping_supplier_zone_state = bit32(20); + rfid_status_data.own_assembly_buff_point_state = bit32(21); + rfid_status_data.enemy_assembly_buff_point_state = bit32(22); + rfid_status_data.central_buff_point_state = bit32(23); + rfid_status_data.enemy_fort_buff_point_state = bit32(24); + rfid_status_data.enemy_outpost_buff_point_state = bit32(25); + rfid_status_data.own_tunnel_road_lower_buff_point_state = bit32(26); + rfid_status_data.own_tunnel_road_middle_buff_point_state = bit32(27); + rfid_status_data.own_tunnel_road_upper_buff_point_state = bit32(28); + rfid_status_data.own_tunnel_trapezoid_lower_buff_point_state = bit32(29); + rfid_status_data.own_tunnel_trapezoid_middle_buff_point_state = bit32(30); + rfid_status_data.own_tunnel_trapezoid_upper_buff_point_state = bit32(31); + rfid_status_data.enemy_tunnel_road_lower_buff_point_state = bit8(0); + rfid_status_data.enemy_tunnel_road_middle_buff_point_state = bit8(1); + rfid_status_data.enemy_tunnel_road_upper_buff_point_state = bit8(2); + rfid_status_data.enemy_tunnel_trapezoid_lower_buff_point_state = bit8(3); + rfid_status_data.enemy_tunnel_trapezoid_middle_buff_point_state = bit8(4); + rfid_status_data.enemy_tunnel_trapezoid_upper_buff_point_state = bit8(5); rfid_status_data.stamp = last_get_data_time_; rfid_status_pub_.publish(rfid_status_data); @@ -431,6 +439,7 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::DartClientCmd dart_client_cmd_data; memcpy(&dart_client_cmd_ref, rx_data + 7, sizeof(rm_referee::DartClientCmd)); dart_client_cmd_data.dart_launch_opening_status = dart_client_cmd_ref.dart_launch_opening_status; + dart_client_cmd_data.reserved = dart_client_cmd_ref.reserved; dart_client_cmd_data.target_change_time = dart_client_cmd_ref.target_change_time; dart_client_cmd_data.latest_launch_cmd_time = dart_client_cmd_ref.latest_launch_cmd_time; dart_client_cmd_data.stamp = last_get_data_time_; @@ -463,14 +472,28 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::RadarMarkData radar_mark_data; memcpy(&radar_mark_ref, rx_data + 7, sizeof(rm_referee::RadarMarkData)); - radar_mark_data.mark_engineer_progress = radar_mark_ref.mark_engineer_progress; - radar_mark_data.mark_hero_progress = radar_mark_ref.mark_hero_progress; - radar_mark_data.mark_sentry_progress = radar_mark_ref.mark_sentry_progress; - radar_mark_data.mark_standard_3_progress = radar_mark_ref.mark_standard_3_progress; - radar_mark_data.mark_standard_4_progress = radar_mark_ref.mark_standard_4_progress; + radar_mark_data.mark_progress = radar_mark_ref.mark_progress; + radar_mark_data.enemy_hero_vulnerable = radar_mark_ref.enemy_hero_vulnerable; + radar_mark_data.enemy_engineer_vulnerable = radar_mark_ref.enemy_engineer_vulnerable; + radar_mark_data.enemy_standard_3_vulnerable = radar_mark_ref.enemy_standard_3_vulnerable; + radar_mark_data.enemy_standard_4_vulnerable = radar_mark_ref.enemy_standard_4_vulnerable; + radar_mark_data.enemy_aerial_special_mark = radar_mark_ref.enemy_aerial_special_mark; + radar_mark_data.enemy_sentry_vulnerable = radar_mark_ref.enemy_sentry_vulnerable; + radar_mark_data.own_hero_special_mark = radar_mark_ref.own_hero_special_mark; + radar_mark_data.own_engineer_special_mark = radar_mark_ref.own_engineer_special_mark; + radar_mark_data.own_standard_3_special_mark = radar_mark_ref.own_standard_3_special_mark; + radar_mark_data.own_standard_4_special_mark = radar_mark_ref.own_standard_4_special_mark; + radar_mark_data.own_aerial_special_mark = radar_mark_ref.own_aerial_special_mark; + radar_mark_data.own_sentry_special_mark = radar_mark_ref.own_sentry_special_mark; + radar_mark_data.mark_hero_progress = radar_mark_ref.enemy_hero_vulnerable; + radar_mark_data.mark_engineer_progress = radar_mark_ref.enemy_engineer_vulnerable; + radar_mark_data.mark_standard_3_progress = radar_mark_ref.enemy_standard_3_vulnerable; + radar_mark_data.mark_standard_4_progress = radar_mark_ref.enemy_standard_4_vulnerable; + radar_mark_data.mark_sentry_progress = radar_mark_ref.enemy_sentry_vulnerable; radar_mark_data.stamp = last_get_data_time_; radar_mark_pub_.publish(radar_mark_data); + break; } case rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD: { @@ -538,15 +561,30 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::SentryInfo sentry_info; memcpy(&sentry_info_ref, rx_data + 7, sizeof(rm_referee::SentryInfo)); sentry_info.sentry_info = sentry_info_ref.sentry_info; + sentry_info.sentry_info_2 = sentry_info_ref.sentry_info_2; + sentry_info.exchanged_bullet_allowance = sentry_info_ref.exchanged_bullet_allowance; + sentry_info.remote_bullet_exchange_success_cnt = sentry_info_ref.remote_bullet_exchange_success_cnt; + sentry_info.remote_hp_exchange_success_cnt = sentry_info_ref.remote_hp_exchange_success_cnt; + sentry_info.can_confirm_free_respawn = sentry_info_ref.can_confirm_free_respawn; + sentry_info.can_exchange_instant_respawn = sentry_info_ref.can_exchange_instant_respawn; + sentry_info.instant_respawn_cost = sentry_info_ref.instant_respawn_cost; sentry_info.is_out_of_war = sentry_info_ref.is_out_of_war; sentry_info.remaining_bullets_can_supply = sentry_info_ref.remaining_bullets_can_supply; + sentry_info.sentry_mode = sentry_info_ref.sentry_mode; + sentry_info.can_activate_energy_mechanism = sentry_info_ref.can_activate_energy_mechanism; sentry_info_pub_.publish(sentry_info); break; } case rm_referee::RADAR_INFO_CMD: { + rm_referee::RadarInfo radar_info_ref; rm_msgs::RadarInfo radar_info; - memcpy(&radar_info, rx_data + 7, sizeof(rm_msgs::RadarInfo)); + memcpy(&radar_info_ref, rx_data + 7, sizeof(rm_referee::RadarInfo)); + radar_info.radar_info = radar_info_ref.radar_info; + radar_info.double_vulnerability_chances = radar_info_ref.double_vulnerability_chances; + radar_info.enemy_in_double_vulnerability = radar_info_ref.enemy_in_double_vulnerability; + radar_info.own_encryption_level = radar_info_ref.own_encryption_level; + radar_info.can_modify_key = radar_info_ref.can_modify_key; radar_info_pub_.publish(radar_info); break; } diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 872241cd..ccbc3a64 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -39,7 +39,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::sentry_cmd_sub_ = nh.subscribe("/sentry_cmd", 1, &RefereeBase::sendSentryCmdCallback, this); RefereeBase::radar_cmd_sub_ = - nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); + nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); RefereeBase::sentry_state_sub_ = nh.subscribe("/custom_info", 1, &RefereeBase::sendCustomInfoCallback, this); RefereeBase::drone_pose_sub_ = @@ -115,9 +115,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "pitch") pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "image_transmission") - image_transmission_angle_time_change_ui_ = - new ImageTransmissionAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + // if (rpc_value[i]["name"] == "image_transmission") + // image_transmission_angle_time_change_ui_ = + // new ImageTransmissionAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "balance_pitch") balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); @@ -141,8 +141,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "friend_bullets") friend_bullets_time_change_group_ui_ = new FriendBulletsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "target_hp") - target_hp_time_change_ui_ = new TargetHpTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + // if (rpc_value[i]["name"] == "target_hp") + // target_hp_time_change_ui_ = new TargetHpTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -155,8 +155,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") @@ -165,8 +165,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "customize_display") customize_display_flash_ui_ = new CustomizeDisplayFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "burst") - burst_flash_ui_ = new BurstFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + // if (rpc_value[i]["name"] == "burst") + // burst_flash_ui_ = new BurstFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -237,8 +237,8 @@ void RefereeBase::addUi() balance_pitch_time_change_group_ui_->addForQueue(); if (pitch_angle_time_change_ui_) pitch_angle_time_change_ui_->addForQueue(); - if (image_transmission_angle_time_change_ui_) - image_transmission_angle_time_change_ui_->addForQueue(); + // if (image_transmission_angle_time_change_ui_) + // image_transmission_angle_time_change_ui_->addForQueue(); if (engineer_joint1_time_change_ui) engineer_joint1_time_change_ui->addForQueue(); if (engineer_joint2_time_change_ui) @@ -264,8 +264,8 @@ void RefereeBase::addUi() target_distance_time_change_ui_->addForQueue(); if (friend_bullets_time_change_group_ui_) friend_bullets_time_change_group_ui_->addForQueue(); - if (target_hp_time_change_ui_) - target_hp_time_change_ui_->addForQueue(); + // if (target_hp_time_change_ui_) + // target_hp_time_change_ui_->addForQueue(); if (visualize_state_trigger_change_ui_) visualize_state_trigger_change_ui_->addForQueue(); add_ui_times_++; @@ -367,8 +367,8 @@ void RefereeBase::updateGameRobotHpDataCallBack(const rm_msgs::GameRobotHp& game { if (hero_hit_flash_ui_) hero_hit_flash_ui_->updateHittingConfig(game_robot_hp_data); - if (target_hp_time_change_ui_) - target_hp_time_change_ui_->setEnemyHp(game_robot_hp_data); + // if (target_hp_time_change_ui_) + // target_hp_time_change_ui_->setEnemyHp(game_robot_hp_data); } void RefereeBase::gameStatusDataCallBack(const rm_msgs::GameStatus& data, const ros::Time& last_get_data_time) { @@ -411,8 +411,8 @@ void RefereeBase::jointStateCallback(const sensor_msgs::JointState::ConstPtr& da lane_line_time_change_ui_->updateJointStateData(data, ros::Time::now()); if (pitch_angle_time_change_ui_ && !is_adding_) pitch_angle_time_change_ui_->updateJointStateData(data, ros::Time::now()); - if (image_transmission_angle_time_change_ui_ && !is_adding_) - image_transmission_angle_time_change_ui_->updateJointStateData(data, ros::Time::now()); + // if (image_transmission_angle_time_change_ui_ && !is_adding_) + // image_transmission_angle_time_change_ui_->updateJointStateData(data, ros::Time::now()); if (engineer_joint1_time_change_ui && !is_adding_) engineer_joint1_time_change_ui->updateJointStateData(data, ros::Time::now()); if (engineer_joint2_time_change_ui && !is_adding_) @@ -449,15 +449,15 @@ void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& da 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, ros::Time::now()); 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) { @@ -497,8 +497,8 @@ void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& d target_trigger_change_ui_->updateManualCmdData(data); if (cover_flash_ui_ && !is_adding_) cover_flash_ui_->updateManualCmdData(data, ros::Time::now()); - if (burst_flash_ui_ && !is_adding_) - burst_flash_ui_->updateBurstTimeData(data); + // if (burst_flash_ui_ && !is_adding_) + // burst_flash_ui_->updateBurstTimeData(data); } void RefereeBase::radarDataCallBack(const std_msgs::Int8MultiArrayConstPtr& data) { @@ -514,8 +514,8 @@ void RefereeBase::trackCallBack(const rm_msgs::TrackDataConstPtr& data) target_view_angle_trigger_change_ui_->updateTrackID(data->id); if (target_distance_time_change_ui_ && !is_adding_) target_distance_time_change_ui_->updateTargetDistanceData(data); - if (target_hp_time_change_ui_ && !is_adding_) - target_hp_time_change_ui_->updateTrackID(data->id); + // if (target_hp_time_change_ui_ && !is_adding_) + // target_hp_time_change_ui_->updateTrackID(data->id); } void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data) { @@ -566,7 +566,7 @@ void RefereeBase::sendSentryCmdCallback(const rm_msgs::SentryCmdConstPtr& data) sentry_cmd_data_last_send_ = ros::Time::now(); } } -void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data) +void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarCmdConstPtr& data) { if (ros::Time::now() - radar_cmd_data_last_send_ <= ros::Duration(0.15)) return; diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index c443e0c6..ca77b696 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -186,16 +186,8 @@ void DeployFlashUi::updateChassisVelData(const geometry_msgs::Twist::ConstPtr& d void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg) { - if (base_.robot_id_ > 100) - { - hitted_ = - (last_hp_msg_.red_outpost_hp - msg.red_outpost_hp > 190 || last_hp_msg_.red_base_hp - msg.red_base_hp > 190); - } - else - { - hitted_ = (last_hp_msg_.blue_outpost_hp - msg.blue_outpost_hp > 190 || - last_hp_msg_.blue_base_hp - msg.blue_base_hp > 190); - } + // Temporarily disable hit flash trigger after GameRobotHp switched to ally semantics. + hitted_ = false; last_hp_msg_ = msg; display(ros::Time::now()); } diff --git a/rm_referee/src/ui/interactive_data.cpp b/rm_referee/src/ui/interactive_data.cpp index 81f399ac..ac1f89a6 100644 --- a/rm_referee/src/ui/interactive_data.cpp +++ b/rm_referee/src/ui/interactive_data.cpp @@ -105,8 +105,8 @@ void InteractiveSender::sendRadarInteractiveData(const rm_msgs::ClientMapReceive radar_interactive_data->infantry_3_position_y = data->infantry_3_position_y; radar_interactive_data->infantry_4_position_x = data->infantry_4_position_x; radar_interactive_data->infantry_4_position_y = data->infantry_4_position_y; - radar_interactive_data->infantry_5_position_x = data->infantry_5_position_x; - radar_interactive_data->infantry_5_position_y = data->infantry_5_position_y; + radar_interactive_data->reserved_1 = data->reserved_1; + radar_interactive_data->reserved_2 = data->reserved_2; radar_interactive_data->sentry_position_x = data->sentry_position_x; radar_interactive_data->sentry_position_y = data->sentry_position_y; pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::CLIENT_MAP_CMD, sizeof(rm_referee::ClientMapReceiveData)); @@ -126,27 +126,46 @@ void InteractiveSender::sendRadarInteractiveData(const rm_msgs::ClientMapReceive void InteractiveSender::sendSentryCmdData(const rm_msgs::SentryCmdConstPtr& data) { int data_len; - rm_referee::SentryCmd tx_data; - data_len = static_cast(sizeof(rm_referee::SentryCmd)); + rm_referee::SentryCmdInteractiveData tx_data; + rm_referee::SentryCmd sentry_cmd{ 0 }; + data_len = static_cast(sizeof(rm_referee::SentryCmdInteractiveData)); tx_data.header.sender_id = base_.robot_id_; tx_data.header.receiver_id = REFEREE_SERVER; - tx_data.sentry_info = data->sentry_info; + sentry_cmd.confirm_respawn = data->confirm_respawn; + sentry_cmd.confirm_instant_respawn = data->confirm_instant_respawn; + sentry_cmd.bullet_exchange_target = data->bullet_exchange_target; + sentry_cmd.remote_bullet_exchange_req_cnt = data->remote_bullet_exchange_req_cnt; + sentry_cmd.remote_hp_exchange_req_cnt = data->remote_hp_exchange_req_cnt; + sentry_cmd.posture_cmd = data->posture_cmd; + sentry_cmd.confirm_rune_activating = data->confirm_rune_activating; + if (data->confirm_respawn || data->confirm_instant_respawn || data->bullet_exchange_target || + data->remote_bullet_exchange_req_cnt || data->remote_hp_exchange_req_cnt || data->posture_cmd || + data->confirm_rune_activating) + tx_data.sentry_cmd = sentry_cmd; + else + tx_data.sentry_cmd.sentry_cmd = data->sentry_cmd; tx_data.header.data_cmd_id = rm_referee::DataCmdId::SENTRY_CMD; pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); sendSerial(ros::Time::now(), data_len); } -void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data) +void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarCmdConstPtr& data) { int data_len; - rm_referee::RadarInfo tx_data; - data_len = static_cast(sizeof(rm_referee::RadarInfo)); + rm_referee::RadarCmdInteractiveData tx_data; + data_len = static_cast(sizeof(rm_referee::RadarCmdInteractiveData)); tx_data.header.sender_id = base_.robot_id_; tx_data.header.receiver_id = REFEREE_SERVER; - tx_data.radar_info = data->radar_info; - + tx_data.radar_cmd.radar_cmd = data->radar_cmd; + tx_data.radar_cmd.password_cmd = data->password_cmd; + tx_data.radar_cmd.password_1 = data->password_1; + tx_data.radar_cmd.password_2 = data->password_2; + tx_data.radar_cmd.password_3 = data->password_3; + tx_data.radar_cmd.password_4 = data->password_4; + tx_data.radar_cmd.password_5 = data->password_5; + tx_data.radar_cmd.password_6 = data->password_6; tx_data.header.data_cmd_id = rm_referee::DataCmdId::RADAR_CMD; pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); sendSerial(ros::Time::now(), data_len); diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 48ee5647..6256855b 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -503,23 +503,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 +514,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 ed40b5537d87aee94061ec49f5d19c897e27f97c Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 14 Mar 2026 12:13:51 +0000 Subject: [PATCH 02/32] Fix heatlimit. --- rm_common/include/rm_common/decision/heat_limit.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index e072d9c2..bcfb1555 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -142,6 +142,8 @@ class HeatLimit double getShootFrequency() const { std::lock_guard lock(heat_mutex_); + if (!referee_is_online_) + return 1.0; if (state_ == BURST) return shoot_frequency_; double shooter_cooling_heat = From fad7c21d3c9474673d0322e16e68d969c075a472 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 18 Mar 2026 04:25:10 +0000 Subject: [PATCH 03/32] Backup. --- .../rm_common/decision/command_sender.h | 10 ++-- .../include/rm_common/decision/heat_limit.h | 7 ++- .../rm_common/decision/invincible_detect.h | 54 +++++++++++++++++++ .../include/rm_common/decision/power_limit.h | 23 +++++++- rm_msgs/CMakeLists.txt | 8 +++ rm_msgs/action/QueryGlobalLocalization.action | 9 ++++ rm_msgs/msg/BulletSolverData.msg | 14 +++++ rm_msgs/msg/ChassisActiveSusCmd.msg | 6 +++ rm_msgs/msg/LQRkMatrix.msg | 1 + rm_msgs/msg/LeggedChassisMode.msg | 8 +++ rm_msgs/msg/LeggedChassisStatus.msg | 14 +++++ rm_msgs/msg/LeggedLQRStatus.msg | 8 +++ rm_msgs/msg/LeggedUpstairStatus.msg | 1 + rm_msgs/msg/detection/TrackData.msg | 1 + .../PowerManagementSampleAndStatusData.msg | 4 +- rm_referee/include/rm_referee/common/data.h | 2 + .../include/rm_referee/common/protocol.h | 8 +-- rm_referee/include/rm_referee/referee.h | 2 + rm_referee/src/referee.cpp | 5 +- 19 files changed, 167 insertions(+), 18 deletions(-) create mode 100644 rm_common/include/rm_common/decision/invincible_detect.h create mode 100644 rm_msgs/action/QueryGlobalLocalization.action create mode 100644 rm_msgs/msg/BulletSolverData.msg create mode 100644 rm_msgs/msg/ChassisActiveSusCmd.msg create mode 100644 rm_msgs/msg/LQRkMatrix.msg create mode 100644 rm_msgs/msg/LeggedChassisMode.msg create mode 100644 rm_msgs/msg/LeggedChassisStatus.msg create mode 100644 rm_msgs/msg/LeggedLQRStatus.msg create mode 100644 rm_msgs/msg/LeggedUpstairStatus.msg diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 0b2e7130..10775bcb 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -900,16 +900,12 @@ class CameraSwitchCommandSender : public CommandSenderBase public: explicit CameraSwitchCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) { - ROS_ASSERT(nh.getParam("camera_left_name", camera1_name_) && nh.getParam("camera_right_name", camera2_name_)); - msg_.data = camera2_name_; - } - void switchCameraLeft() - { + ROS_ASSERT(nh.getParam("camera1_name", camera1_name_) && nh.getParam("camera2_name", camera2_name_)); msg_.data = camera1_name_; } - void switchCameraRight() + void switchCamera() { - msg_.data = camera2_name_; + msg_.data = msg_.data == camera1_name_ ? camera2_name_ : camera1_name_; } void sendCommand(const ros::Time& time) override { diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index bcfb1555..f42b55fe 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -128,6 +128,11 @@ class HeatLimit { shooter_cooling_heat_ = data.shooter_id_1_17_mm_cooling_heat; } + else if (type_ == "ID2_17MM") + { + // 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; @@ -143,7 +148,7 @@ class HeatLimit { std::lock_guard lock(heat_mutex_); if (!referee_is_online_) - return 1.0; + return 5.0; if (state_ == BURST) return shoot_frequency_; double shooter_cooling_heat = diff --git a/rm_common/include/rm_common/decision/invincible_detect.h b/rm_common/include/rm_common/decision/invincible_detect.h new file mode 100644 index 00000000..3eaa9fbe --- /dev/null +++ b/rm_common/include/rm_common/decision/invincible_detect.h @@ -0,0 +1,54 @@ +// +// Created by ray on 25-1-11. +// + +#pragma once + +#include +#include +#include + +namespace rm_common +{ +class InvincibleDetect +{ +public: + InvincibleDetect(ros::NodeHandle& nh) + { + if (!nh.getParam("invincible_check", invincible_check_)) + ROS_ERROR("Invincible_check no defined (namespace: %s)", nh.getNamespace().c_str()); + } + typedef enum + { + INJURABLE = 0, + IN_SUPPLY_BASE = 1, + NORMAL_FRESHLY_RESURRECTED = 2, + MONEY_FRESHLY_RESURRECTED = 3, + } InvincibleState; + + // void updateEnemyStatus(int robot_id) + // 调用上面的update函数，需要输入敌方id，然后函数中直接更新对应id机器人的无敌状态 + // InvincibleState getInvincibleState(int robot_id) + // 用这个函数来get对应机器人的无敌状态，然后用这个状态做判断，如果不是INJURABLE，就把shooter设为READY + +private: + // updateState() + void updateRobotInfo(int track_id, rm_msgs::GameRobotStatus& data) + { + robot_id = data.robot_id; + if (robot_id < 100) + track_id = track_id + 100; + } + + void updateInvincibleState(int track_id, bool is_red) + { + } + + int last_hp; + float last_revive_time; + float last_heal_time; + int robot_id; + InvincibleState invincible_state = INJURABLE; + XmlRpc::XmlRpcValue invincible_check_; +}; +} // namespace rm_common diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index c07b54ab..b09f7256 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -37,6 +37,9 @@ #pragma once +#include +#include + #include #include #include @@ -121,7 +124,7 @@ class PowerLimit } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(6); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } @@ -207,6 +210,7 @@ class PowerLimit else chassis_cmd.power_limit = safety_power_; } + applyPosturePowerScale(chassis_cmd); } private: @@ -217,6 +221,11 @@ class PowerLimit } void normal(rm_msgs::ChassisCmd& chassis_cmd) { + if (is_new_capacitor_) + { + 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_; @@ -233,7 +242,9 @@ class PowerLimit { if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) { - if (is_gyro) + if (is_new_capacitor_) + chassis_cmd.power_limit = burst_power_; + else if (is_gyro) setGyroPower(chassis_cmd); else setBurstPower(chassis_cmd); @@ -243,6 +254,13 @@ class PowerLimit // expect_state_ = NORMAL; } + void applyPosturePowerScale(rm_msgs::ChassisCmd& chassis_cmd) const + { + if (std::abs(posture_power_scale_ - 1.0) < 1e-6) + return; + 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 }; @@ -259,6 +277,7 @@ class PowerLimit 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 }; ros::Time start_burst_time_{}; int total_burst_time_{}; diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index a20f9ea9..3fb5b660 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -13,10 +13,12 @@ add_message_files( FILES ActuatorState.msg BalanceState.msg + BulletSolverData.msg BusState.msg DbusData.msg Dart.msg ExchangerMsg.msg + ChassisActiveSusCmd.msg ChassisCmd.msg ShootCmd.msg ShootState.msg @@ -25,6 +27,11 @@ add_message_files( GimbalDesError.msg GimbalPosState.msg LegCmd.msg + LeggedChassisMode.msg + LeggedChassisStatus.msg + LeggedUpstairStatus.msg + LeggedLQRStatus.msg + LQRkMatrix.msg LpData.msg LocalHeatData.msg LocalHeatState.msg @@ -117,6 +124,7 @@ add_message_files( add_action_files( FILES Engineer.action + QueryGlobalLocalization.action ) # Generate added messages and services with any dependencies listed here generate_messages( diff --git a/rm_msgs/action/QueryGlobalLocalization.action b/rm_msgs/action/QueryGlobalLocalization.action new file mode 100644 index 00000000..952fe423 --- /dev/null +++ b/rm_msgs/action/QueryGlobalLocalization.action @@ -0,0 +1,9 @@ +# Goal (Empty) +--- +# Result +bool success +float64 coarse_align_inlier_fraction +float64 fine_align_inlier_fraction +--- +# Feedback +float64 progress diff --git a/rm_msgs/msg/BulletSolverData.msg b/rm_msgs/msg/BulletSolverData.msg new file mode 100644 index 00000000..a4defc57 --- /dev/null +++ b/rm_msgs/msg/BulletSolverData.msg @@ -0,0 +1,14 @@ + +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_ +float64 traject_acc_ +int16 selected_armor_ diff --git a/rm_msgs/msg/ChassisActiveSusCmd.msg b/rm_msgs/msg/ChassisActiveSusCmd.msg new file mode 100644 index 00000000..95fd2aa0 --- /dev/null +++ b/rm_msgs/msg/ChassisActiveSusCmd.msg @@ -0,0 +1,6 @@ +uint8 DOWN = 0 +uint8 MID = 1 +uint8 UP = 2 + +time stamp +uint8 mode diff --git a/rm_msgs/msg/LQRkMatrix.msg b/rm_msgs/msg/LQRkMatrix.msg new file mode 100644 index 00000000..bc745d98 --- /dev/null +++ b/rm_msgs/msg/LQRkMatrix.msg @@ -0,0 +1 @@ +float64[] k diff --git a/rm_msgs/msg/LeggedChassisMode.msg b/rm_msgs/msg/LeggedChassisMode.msg new file mode 100644 index 00000000..e5dd8044 --- /dev/null +++ b/rm_msgs/msg/LeggedChassisMode.msg @@ -0,0 +1,8 @@ +uint8 NORMAL = 0 +uint8 STAND_UP = 1 +uint8 SIT_DOWN = 2 +uint8 RECOVERY = 3 +uint8 UPSTAIRS = 4 + +uint8 mode +string mode_name diff --git a/rm_msgs/msg/LeggedChassisStatus.msg b/rm_msgs/msg/LeggedChassisStatus.msg new file mode 100644 index 00000000..3e7e19a0 --- /dev/null +++ b/rm_msgs/msg/LeggedChassisStatus.msg @@ -0,0 +1,14 @@ +float64 roll +float64 pitch +float64 d_pitch +float64 yaw +float64 d_yaw +float64 left_leg_length +float64 right_leg_length +float64 x +float64 x_dot +float64 left_leg_theta +float64 left_leg_theta_dot +float64 right_leg_theta +float64 right_leg_theta_dot +float64[] linear_acc_base diff --git a/rm_msgs/msg/LeggedLQRStatus.msg b/rm_msgs/msg/LeggedLQRStatus.msg new file mode 100644 index 00000000..abd78b57 --- /dev/null +++ b/rm_msgs/msg/LeggedLQRStatus.msg @@ -0,0 +1,8 @@ +float64[] left_leg_error +float64[] right_leg_error +float64[] left_leg_u +float64[] right_leg_u +float64[] left_leg_ref +float64[] right_leg_ref +float64[] F_leg +bool[] unstick diff --git a/rm_msgs/msg/LeggedUpstairStatus.msg b/rm_msgs/msg/LeggedUpstairStatus.msg new file mode 100644 index 00000000..ffcd1756 --- /dev/null +++ b/rm_msgs/msg/LeggedUpstairStatus.msg @@ -0,0 +1 @@ +bool upstair_flag diff --git a/rm_msgs/msg/detection/TrackData.msg b/rm_msgs/msg/detection/TrackData.msg index 276929d6..4a749245 100644 --- a/rm_msgs/msg/detection/TrackData.msg +++ b/rm_msgs/msg/detection/TrackData.msg @@ -10,4 +10,5 @@ float64 v_yaw float64 radius_1 float64 radius_2 float64 dz +float64 za float64 accel diff --git a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg index 11d38c39..9946e9aa 100644 --- a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg +++ b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg @@ -1,6 +1,6 @@ float32 chassis_power -float32 cap_error_flag -float32 cap_received_msg +float32 chassis_expect_power +float32 capacity_recent_charge_power float32 capacity_remain_charge uint8 capacity_discharge_power uint8 state_machine_running_state diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index f2adff30..d53a1636 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -37,6 +37,8 @@ #pragma once +#include +#include #include #include #include diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 2b524d6f..9aac3761 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -703,10 +703,10 @@ typedef struct { uint8_t chassis_power_high_8_bit; uint8_t chassis_power_low_8_bit; - uint8_t cap_error_flag_high_8_bit; - uint8_t cap_error_flag_low_8_bit; - uint8_t cap_received_msg_high_8_bit; - uint8_t cap_received_msg_low_8_bit; + uint8_t chassis_expect_power_high_8_bit; + uint8_t chassis_expect_power_low_8_bit; + uint8_t capacity_recent_charge_power_high_8_bit; + uint8_t capacity_recent_charge_power_low_8_bit; uint8_t capacity_remain_charge_high_8_bit; uint8_t capacity_remain_charge_low_8_bit; uint8_t capacity_expect_charge_power; diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index a75045d9..c123a980 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -36,7 +36,9 @@ // #pragma once +#include #include +#include #include #include "rm_referee/common/data.h" diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 817976e2..f0e579c7 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -594,8 +594,9 @@ int Referee::unpack(uint8_t* rx_data) uint8_t data[sizeof(rm_referee::PowerManagementSampleAndStatusData)]; memcpy(&data, rx_data + 7, sizeof(rm_referee::PowerManagementSampleAndStatusData)); sample_and_status_pub_data.chassis_power = (static_cast((data[0] << 8) | data[1]) / 100.); - sample_and_status_pub_data.cap_error_flag = (static_cast((data[2] << 8) | data[3]) / 100.); - sample_and_status_pub_data.cap_received_msg = (static_cast((data[4] << 8) | data[5]) / 100.); + sample_and_status_pub_data.chassis_expect_power = (static_cast((data[2] << 8) | data[3]) / 100.); + sample_and_status_pub_data.capacity_recent_charge_power = + (static_cast((data[4] << 8) | data[5]) / 100.); sample_and_status_pub_data.capacity_remain_charge = (static_cast((data[6] << 8) | data[7]) / 10000.); sample_and_status_pub_data.capacity_discharge_power = static_cast(data[8]); From 962322e213f9febe406bccf9b283c51301a00a38 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 22 Mar 2026 16:19:18 +0000 Subject: [PATCH 04/32] Add ui. --- .../include/rm_common/decision/power_limit.h | 9 +++---- rm_referee/include/rm_referee/referee_base.h | 1 + rm_referee/include/rm_referee/ui/graph.h | 5 ++++ .../include/rm_referee/ui/trigger_change_ui.h | 25 +++++++++++++++++++ rm_referee/src/referee_base.cpp | 6 +++++ rm_referee/src/ui/trigger_change_ui.cpp | 16 ++++++++++++ 6 files changed, 56 insertions(+), 6 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index b09f7256..2f504fbe 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -153,8 +153,7 @@ class PowerLimit if (allow_gyro_cap_ && chassis_power_limit_ < 80) chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } void setBurstPower(rm_msgs::ChassisCmd& chassis_cmd) { @@ -170,8 +169,7 @@ class PowerLimit chassis_cmd.power_limit = standard_power_; } else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } void setLimitPower(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { @@ -250,8 +248,7 @@ class PowerLimit setBurstPower(chassis_cmd); } else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } void applyPosturePowerScale(rm_msgs::ChassisCmd& chassis_cmd) const diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 6217da4c..365fbef8 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -106,6 +106,7 @@ class RefereeBase TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{}; + GyroTriggerChangeUi* gyro_trigger_change_ui_{}; BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index 23a9979e..27485ad8 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -53,6 +53,11 @@ class Graph { config_.radius = radius; } + void setWidth(int width) + { + if (width > 0) + config_.width = width; + } void setIntNum(int num) { int a = num & 1023; 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..226e28d3 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -3,6 +3,9 @@ // #pragma once +#include +#include + #include "rm_referee/ui/ui_base.h" #include #include "std_msgs/String.h" @@ -78,6 +81,28 @@ class ChassisTriggerChangeUi : public TriggerChangeUi double mode_change_threshold_; }; +class GyroTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit GyroTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "gyro", graph_queue, character_queue) + { + outline_width_ = graph_->getConfig().width; + const auto config = graph_->getConfig(); + const int rect_w = std::abs(config.end_x - config.start_x); + const int rect_h = std::abs(config.end_y - config.start_y); + fill_width_ = std::max(outline_width_ + 1, std::min(rect_w, rect_h) / 1); + } + void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data); + +private: + void update() override; + uint8_t chassis_mode_{}; + int outline_width_{}; + int fill_width_{}; +}; + class ShooterTriggerChangeUi : public TriggerChangeUi { public: diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index ccbc3a64..a3e3ecd4 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -83,6 +83,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "friction_speed") friction_speed_trigger_change_ui_ = new FrictionSpeedTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "gyro") + gyro_trigger_change_ui_ = new GyroTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gripper") gripper_state_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "gripper", &graph_queue_, &character_queue_); @@ -255,6 +257,8 @@ void RefereeBase::addUi() servo_mode_trigger_change_ui_->addForQueue(); if (friction_speed_trigger_change_ui_) friction_speed_trigger_change_ui_->addForQueue(); + if (gyro_trigger_change_ui_) + gyro_trigger_change_ui_->addForQueue(); if (bullet_time_change_ui_) { bullet_time_change_ui_->reset(); @@ -447,6 +451,8 @@ void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& da { if (chassis_trigger_change_ui_) chassis_trigger_change_ui_->updateChassisCmdData(data); + if (gyro_trigger_change_ui_ && !is_adding_) + gyro_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_) diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index bea8e352..381e81b5 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -227,6 +227,22 @@ void ChassisTriggerChangeUi::updateCapacityResetStatus() displayInCapacity(); } +void GyroTriggerChangeUi::update() +{ + if (chassis_mode_ == rm_msgs::ChassisCmd::RAW) + graph_->setWidth(fill_width_); + else + graph_->setWidth(outline_width_); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(true); +} + +void GyroTriggerChangeUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data) +{ + chassis_mode_ = data->mode; + update(); +} + void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); From 3b2e059c6054920b8023448a8304d501aa83947e Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 25 Mar 2026 16:44:23 +0000 Subject: [PATCH 05/32] Add controller find. --- rm_common/include/rm_common/decision/controller_manager.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rm_common/include/rm_common/decision/controller_manager.h b/rm_common/include/rm_common/decision/controller_manager.h index 15904a3c..ee63b003 100644 --- a/rm_common/include/rm_common/decision/controller_manager.h +++ b/rm_common/include/rm_common/decision/controller_manager.h @@ -153,6 +153,13 @@ class ControllerManager { return switch_caller_.isCalling(); } + bool hasController(const std::string& controller) const + { + return std::find(state_controllers_.begin(), state_controllers_.end(), controller) != state_controllers_.end() || + std::find(main_controllers_.begin(), main_controllers_.end(), controller) != main_controllers_.end() || + std::find(calibration_controllers_.begin(), calibration_controllers_.end(), controller) != + calibration_controllers_.end(); + } private: ros::ServiceClient load_client_; From ddf5b0db631eb9f928e1164c5dd795af9be5e45a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 25 Mar 2026 16:49:15 +0000 Subject: [PATCH 06/32] Merge branch 'master' into dev/standard6 --- .../rm_common/decision/command_sender.h | 62 +++++- .../include/rm_common/decision/power_limit.h | 7 + rm_gazebo/launch/rmul2026.launch | 33 ++++ rm_gazebo/launch/vmc_test.launch | 29 +++ .../worlds/legged_balance/vmc_test.world | 180 ++++++++++++++++++ rm_gazebo/worlds/place/RMUL2026.stl | Bin 0 -> 161684 bytes rm_gazebo/worlds/rmul2026.world | 75 ++++++++ rm_msgs/CMakeLists.txt | 0 8 files changed, 377 insertions(+), 9 deletions(-) create mode 100644 rm_gazebo/launch/rmul2026.launch create mode 100644 rm_gazebo/launch/vmc_test.launch create mode 100644 rm_gazebo/worlds/legged_balance/vmc_test.world create mode 100644 rm_gazebo/worlds/place/RMUL2026.stl create mode 100644 rm_gazebo/worlds/rmul2026.world mode change 100644 => 100755 rm_msgs/CMakeLists.txt diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 10775bcb..f357371b 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -61,6 +62,7 @@ #include #include #include +#include #include "rm_common/ros_utilities.h" #include "rm_common/decision/heat_limit.h" @@ -70,6 +72,21 @@ namespace rm_common { +namespace detail +{ +template +auto setTrajFrameIdIfSupported(TMsg& msg, const std::string& traj_frame_id, int) + -> decltype((void)(msg.traj_frame_id = traj_frame_id), void()) +{ + msg.traj_frame_id = traj_frame_id; +} + +template +void setTrajFrameIdIfSupported(TMsg&, const std::string&, long) +{ +} +} // namespace detail + template class CommandSenderBase { @@ -231,6 +248,10 @@ class ChassisCommandSender : public TimeStampCommandSenderBase +{ +public: + explicit ChassisActiveSuspensionCommandSender(ros::NodeHandle& nh) + : TimeStampCommandSenderBase(nh) + { + } + void setZero() override + { + msg_.mode = 0; + } +}; class GimbalCommandSender : public TimeStampCommandSenderBase { public: @@ -332,9 +371,9 @@ class GimbalCommandSender : public TimeStampCommandSenderBase +class BallisticSolverRequestCommandSender : public CommandSenderBase { public: - explicit UseLioCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) + explicit BallisticSolverRequestCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) { } - void setUseLio(bool flag) + void setBallisticSolverRequest(bool flag) { msg_.data = flag; } - bool getUseLio() const + bool getBallisticSolverRequest() const { return msg_.data; } diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 2f504fbe..01fce382 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -258,6 +258,13 @@ class PowerLimit chassis_cmd.power_limit = std::max(0.0, std::floor(chassis_cmd.power_limit * posture_power_scale_)); } + void applyPosturePowerScale(rm_msgs::ChassisCmd& chassis_cmd) const + { + if (std::abs(posture_power_scale_ - 1.0) < 1e-6) + return; + 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 }; diff --git a/rm_gazebo/launch/rmul2026.launch b/rm_gazebo/launch/rmul2026.launch new file mode 100644 index 00000000..9c468a38 --- /dev/null +++ b/rm_gazebo/launch/rmul2026.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + r + + + + + + + diff --git a/rm_gazebo/launch/vmc_test.launch b/rm_gazebo/launch/vmc_test.launch new file mode 100644 index 00000000..695bf93b --- /dev/null +++ b/rm_gazebo/launch/vmc_test.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rm_gazebo/worlds/legged_balance/vmc_test.world b/rm_gazebo/worlds/legged_balance/vmc_test.world new file mode 100644 index 00000000..acd2cec5 --- /dev/null +++ b/rm_gazebo/worlds/legged_balance/vmc_test.world @@ -0,0 +1,180 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 0.00066 0.007637 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + 36 582000000 + 36 753468831 + 1753689095 941445034 + 36582 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.000665 0.00764 0.499996 -1e-06 9e-06 -2e-06 + 1 0.23663 1 + + 0.000665 0.00764 0.499996 -1e-06 9e-06 -2e-06 + 0 0 0 0 -0 0 + 0.984348 -0.232776 -8.16025 -2.67587 1.17539 3.14113 + 0.984348 -0.232776 -8.16025 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5.29511 0.080612 1.43762 0 0.267643 3.07219 + orbit + perspective + + + + diff --git a/rm_gazebo/worlds/place/RMUL2026.stl b/rm_gazebo/worlds/place/RMUL2026.stl new file mode 100644 index 0000000000000000000000000000000000000000..1c501d7b7498e1e406047855343846e2029fbe06 GIT binary patch literal 161684 zcmb5X3EXd0S@-={loYd6td6M+2hR~KCb1llbKf|GCYhY%DUA$=4_aCWCsHBY4j`t2 z1HKAQXBIVaswW&c_aE>*n&Ffs`uLdOkOK}`ie{qk^;>Iy*Z!@w_eS6U$N%ASUF*B9 zVO?wO@xJdrc;tx>IOzCC{?0*1-|L8b-s^~?jyUR|2mbzvzrXpoty{PL^q~L$pRK>R z(5H+4ZHVv6e%Bnhde!&OTgG?eDHq(Y$ZEf4N4Jdc#-~5LCD1q_nf62oTG2n=Ys%g! z7hJiiA{G$RIDrh2#}69|G>Stqp3pu|V8aQ?5E{K(!v?)PTI|ge(l~((k;j(}G-0Qj z585lDa{?RfY(l87KHZweQZH#dKdlW_i9VSiCr*qa@1l3AtA||ofKp309C~Vz)lLrU zx^j&Zk}E*4$sG)jm(4aRLvE zjV{Jd`_(zuUr>C%^n^DQS?#RgdcSgw6Ot=JF{HZkUQ_mpR}c#bX`DcY80rd5M88oo zp3pu|V8aQ?5aCqJh5q$%JzCo532B@_h8XG!O?a?TvSw2;(OwaqymC!TPz&{bMK6uz z`SFpMb6M!tT;s$j@{YRNdHS~E`ML(KSv;t_XRQ>dJdf*&|me1`!Jg zX`DcY80rd*=0Gx@&^}LK!wJa{$*WXXKCVYg`#d3y6UY!lU7-mNHcF*Kb$(&+khYp!tudw4Y*2hX{FSQ8(;dTWV8?bT7OqY_=?gyf2lN2#v7*Oa~D z6~qEU8Yhq;hPpzdx{{11w9gaRa6&RfI92wmVO)KVOUQhHG{#i>&s|m;GcYOL-Qd046WZqqX`DcY80H*}aw6ID=3XVD zlW!2%a6&RRQXDHEdGaN@d|a~Nb2Cp!;{@_R-1T91+NCOSjbwbs_Ly)@OF)mSo`SM>66C99n{>bgSH60!l2<|7@Wyq9}I`#d3y z6UY#CT@j&r5z{2m$v4-u1o5TpM&Fz-??ry1XPFzk(&#x~5Yjk-T!?PpFTW(!)s$Dz zb#@^`HvK2NyZKT8$lUQ+#)p z$f?AlaYC{a!+dBgZ1}jeA&nEr5JR=2QBEWi6YUj| zX2dluAsQmjUK|ItFO_l0g3rx7A&nErh3M}3u!}ejCL0iB6x)SZK)A*;s&?pcmBc=~ z65mu;$f9FUT#4_lF@Z+oLP(}vL|s?(@^K}rojB^cLemnm0YP0Iy4!oXC$!HK(l~(( zQP&j_suwX$5~Z&2wHDcD|MPPSh0|CnVE8uB)kWWkc;mUe^^G_XHUtWw-2CSa(l; zHleYcK*q*US7?+I$;3qaJb?`-h;OJX9nXDS@)JFaJ#6Hke$es`-?qj<#0@R z7F%KzN2;sX#rDM2sIF&s$9@~v3RzZr+!fJ%pxq_%LZER%GVO&(YtVa5+4DH^E(alv z6UY$5o`^voH6OIk6Vj*` zGl&eKIP~ds?Vox{<5_5JI#kON;00%UJ;!#=9-oe4WYhy-o2McOZ$jOHWh=x!c`gFR+Px*nsL$y~#mw=u?v7;4P?Qw5T_ZOSG za*Y#`M?|VC?=@ww@~R@FaRM1)s4FxP{cg#4LVHD|e7UA2L_;XIdEUL3M@##NSh&Us zWQd`z&?xVcHJgfw_IW~@mY^0`=P4Fl9#SA)h0$rYg(()q6U znzC2Cf{1rNvM16wfebO!6`F`X_N&1|+UE&u;5#%#IMr7P8s)^JrG1`|#tCGIp{~$i zL$j%vXs?J)zPU!S6Otj+SD!APt>pZ;SCZ8}PoSByS8R0DRa`-a<@@QoG44zhS?%#Q zpuSCAxyA{}P7HJIHk=;X^ZTNSNU^)7C1j(-k@6(Od$}jH&lA!(febOsIU40evgggc zN<=5GAh6*?mYX+aJ0Ew*g3nDuBpZz7#2`a(m(w+p@qMt}60T_p=(M*-)a8ErGT!>r zH{}Xhbi6;P??hdpkv#~>w97_aSM>66C9A#6pmw+PaZO9e28252dG}uK3GMR)bB^!O z5OrO7FJhV`I{D@r$xe9W>YMZBy~slJEOUcbg-A9S%L(K{#Ph-1>{7m_yi%`mt{_~4 zhY;8v*Hygrw~Vju$?3mI)=2UGpuQ7zg~kcVw2$j*YFyb+JCWCwh{ioZhDg~>SCDAr zuX{rKJRyw}$Phzap}__*(Owaqd~=OtCx|c2NA%6ndoTI^Gl^t_v7A80D_yG%&j&SA zMD8^0coj@kqGu5gMUQN#ce)#;T`Z2AtMT2f-4t2vaW=Rb-`&_42sBPero9kp&b`-^ zy%KqfL6Ja6;{-CqFz09#hh#jVy&^ireC$v{Yr;NF#B}7B0Z=QGW<(An?#Ll4h_=Ku<81dxAZ;!R~Wx@m>2NacDn` zYm(LYHg)A1CnS%ER9D_hG&X3jys8LkoIr*c>IzMa9sAYbJM9&b^5vSAP#h4-E&J6l zu158BvlxYEc& zdbKuke+Y8ogvVD`NxTtXe2=%_i>!9$W=x=QLNe`zNLAv!rtEp-xl$mcaRM1)s4Fy@ z1Ic(odqs50m}^=>G=%!*dG}r(E$t&B*Omv zmh8D=BxQF@Xsni~szh$ZC;b#enkfjz^^Bw$>=MuujD?5Lw3j(gRT58lR`5NZ8x>jY ztobp4#tF%^7a~=O_nNXtc5~GF7eX2*kRgV;LKCs*J%Hi~?G@3Z#;*LZZyn8Q?mi7^mY%rD+$Phzap$SUwk(K$NeV&k}C8q1j{q=FxUJ;#QS6#Wr z3GCrj>fP^G=assOCqG4wXGJU8AGIHsDk%gSCnVEeh_qjMFO5s>(q4I05z;t;3^CLd z8r79#JfXcJI>qmrmJkh*a+QvQ-piw=MuujOCtS53kbrsY>FG>FUnYAH0mOo)r{X?YcV=-;Je83W3H6 z$+U|Y=G<-gxU}O{B2p}_X$jeYNO7b&_g?M^?IU908Yhq;hB-&0oJjV(xmSruQv#u} zT0%57QXFYTc`teBHg1~GSWX}p8}V}!<>v>}xDfb`?Lx%QO=Q9~Q+XSRnDmmP52GQJaag+}%uB-1{wD|-33lGTn^bzPxp3E6k8}c3G|3ab;VdtAVbu3 zOsl66Uf*Y>I#i=BAJ+IuZUDvu4xJ4EAx@+%6pNY=vnMxW2h?#X`DbFh`W5~=-v5j zDbJKNEv+1m3D06njN(Xj6?Y(3Ip5iQ+4dra8AU0q*&JICBn1N+E8zOx&pH0an)WConlvAxyA|X;Z^G0?>@ttIOdo)l{n74<91yO zPFxSV#tF$IBGr}m5-r~~|Flu?1}b_q{utYeIJN6vUZLf_pwBSThuoRR6<>{qUFqCcN2#4zV>!|9P1YGgj`E?WW_EC;oq# zn|VSSCy)n%w?Exq;(*56TqO{m#g-VwQR*t*d{~Wd+^4=$|4p(+iuWe;ov14`PDrMG zTvt=$%7)sByxyfsBozuFxnal8K4-c|w|&AilEB z(>nBC6t4F4jk?tNu>>GLFg{o)Xs?KvlYzhnGblnbg!<}}evZCqJU^|CeC&WACr*qa z@2IQz+38YC{WHi^S86BIbzQl}iT)YnP>G5m)s^>}vgeWKx`L3#31o<&uF$BiB;z~n z^8_}WActjrq`kz)^=N6wMxH(W>l!DJA%?m_6CUX2Ypc0&uZF!HLSwbWbX`q(MHc1> zG*kA7BfUSL_N({_>Ee6*RCJNm9@ihs_%{2MYn+f=5v8uY*Oa~D6~qEU8Yhq;hPpzd zx{{11w9gaRa6&SK>Lbs)kL%IWK2J#F1Tw@>S7^e6jgmE+ii!4$=;WJgT7p_&eW-u> zsrZy1_eye`EA`eV6O1)w53f@1a()%>N9f&&^gF$fbwrKIOuruzuZAZ-X!050K+`T_ z$d$&L8kcsw8rGm|+!JJo6i3<<(S!#n4m_cKL@ZpRI3SQAhT27g4Pv6bBIH^+Z`Zie zsFxFxAyOP^PxM~$@aHEqmJ`UuMqCdLzgG?GZ5C+Ha}I%5*zlN;$5j$X$vyGa?_ym= zmc4j8VlBS;_drJILtUY9LNe{+x|$kSHq=h!bzPxxPmm$fe8lg+%D-0)>+Z?VCN!24 z$k?ds%54zSBq7&QU5UVk6U0~ML%+{5tqFROpZNb}ZtyBaUJ=qbfjkgg-MPP#@noDU z2-ma(bcv(vSMj#PT7308EaMs{%Ni-(o6yf(Pt+9}CnVE8uB)kWWkc;mUe^^G_XHUt zW!Ltr?5}$=OZ5BIdC9sH$k-U_3XO6inV4vwC!}c!;w$qJRdPu+F8PU`r5zj8)j&w& z1hNyEa)0p(S~EB%Jc}(+#nIXBvyAsI^xlN-krX*bT5=yHMP3nToRCa=s;e~LibP{! z!^fqaIMNIXC`2D^85bHtee>}aO?Z&vz!Tc%32EGSWQaVzETO>$G0|QTofFt_LNbK< z=K9rG^ddj;|I6Hrh~yPxIe|P7+~stQWaY{|920mYO-n$Jt3-E9`+LYsWYO`?gx-9~ z_pc&PNU=l9S76gdK?hg z9@ka8>7aK$bnk76EF1C8#AI#h$l4&3BS5xE4hT4g|t}8U|2{J@oS6FvXem0@8 zoIu9LP*-S_6UoFxdqt$Wa!pGRUtL$;i~K~-VhL+|1}nKV<< zw6t;R#G4LFu4Usri6X0A_ePv^w9gaLIDrf?)GnIvAnxxD^Fez> z#4PG(0dn54(at7>`s$P3dP(yvjpxTlV)q#ca^l1&@-D_t=k4(ZM5(2C>!HYMC)3y% z2sBPeroCQ+-fPO9N1pdA2x*)^h8XG!jp|A=p3pu|V8aQ?5IM8@*T?l}X`d&gaRM1) zs4Fz#LHwPcVLoWDh?vFnPEm>$8`Kpvg!<}}-g>DBqHS}P`$K44Y(R``bktS60a0ox z-g+pq+PPC$*OhCWkX#XpA)PaLuPJ*Td9Eu6X`DcY80rd*>Pj-6&^}LK!wJa{$}aoW zFs?_7y?H_!Cy*hAxwsMOM42QtPI!E7v$7xgr!psw?j`Wv_Szv4D`q31o<&uF$Bi zB;yI~^8_}WkPM;h=6UyVJzCo532B@_h8XG!O?VK0?{1h6+AE@yZ?0(xYJua2`s=5c z^SBzz^W!5i=L11boESykQCD$YP--d8?2D{+GL4N~iLP-%az!YHR9D_>${x8&F{E=* z2x*)^h8XG!jp|A=p3pu|V8aQ?5IM8@*T?l}X`d&gaRM1)s4Fz#f&NZ)nGf14qLWvy zX^H8&a(|s}bEVPs>DFB11orT1cs>~C|Hbz>voEsRIWp9BSA;xDb>+RL>=myd z77)@nfebO!6`H(X4ZhPpPhbPzMQ~(bowHvJ<9f6|n~)7Bkc(Gqnm>Kega;cXt1=W5 z?G@2Efej}lL#VGl-J1LBiV{UI332|O$|y6DaQ_GLU1T8Xdz21}83Wf@O?(%;Rg z>&i7wNOoeFa}n5Zns&TOL<+_=Eg>5a>YV4?d$}jH&lA!(febP1C1}EfDp#~uL}WPM zzrtVjA`8e6>f5E~QD`(DWWncVo{+`~O;#+41V6GvTFXxtNIh%_JR`3D;L>z>d)Pe|hg zGDKZhM5tcGG)Y9ha$SkQh7-h><|F#5uJYftq?de0#vWd2^qP>y3FJale}Ay`3Yzxp zmHqr+np^d9PsTR9O5zF6N_=I#i=BAJ+IuZT{*xuzwE&+F>YbpMla$xr{7eXGzH~;-X+dT=$4ochM^je2mA<4k`w&K>p5 z_uJ)EA<#G>nRdL&bK<=;ZfmbZE*t4g140@nkRgUSN253-;|c8*5!qcVT+a9<@f=n50iOI-Q zmBg8T@g&afi>!8KMDdj>DFhlPB-37qR3+YP${xAOG3Z|iX`DcY80rd*>Pj-6&|VRp zY`CT+L_?@=o_Fu%(b7I5k`2ak0vTecD>Omr_wSbZpnaZ@rX^Hk5Q;;ebaj{V1A&L0 zku-x{0(ye6+!O4%jXWoDMqhl7>wzNEK2J#F1ad|6JKT6&&A*7a?$NiYUDx1=2+2+i z^WmB)0%8MeBW4?JW4`)7G+a<{Vkbq~6KibEdtpG-(I zCYFUr$MT-a#7D0_anoXP>ZBYjV`*!Clk_SIgcy(;)_1Jrdp`H8WHpo z5%YcAG=av)9f+$38=ZV|O-p2ZMr0txG58+2S@v~2j-v}CzIi_A6-{Y37;HC@C&J@! zBB-^J-IxPDd8K9>N$$PM?J+{y8B_^;22H>FII#?3nDZVpLtN4G4 zTAxhxbKVd`9O_MNWulK`srEi^m5o9)Hqx5ti7dwm8_EvXd*)sZz3`Ux5kog#6+I$a8>N@efAXpe zzW2&&X&4t9F|)&-I7_Hk?1{*Y6B? z)wKk@)E@JFc|}BTeFlx>G12dE*f1j1QblCCA-e2U&Ixi~m4%+l?s#pxw_}0`dmCxT z$?|Z1rCH*0>Qb|oJxTJa$!Ru|jS-ROc|@$1IWNRFPx_&LCQ8>G@p~YDSbo1K?K-op zj?N3$v;zgl2zc*^Bys5p4JkphOS44^Yoa>)gG?$)`e)rB&bD;Jr@`^y?gk;+5 zT*YsHF8PVy7hPnvPe<+=CnQ%y@k)0Gcbxbm5o$M=x24Kz+jroGP9=F67L_*p^yb=?j7U2{c|)gC_` zxE9|WN11ciI3anO2!B^2Pb=}&^N%8{JtB|ihF9*Yy>g8clIMxYPu$!6o}LxOm2j3d zQbc}Se0Pj(pm9Pn?R9n|@>Ts@Wqfx_`1$kmimdi(_bHLP#tF$4QR0Yo5aXY%@5ZR`BC9=)Z2IQo zM@67Uc%|sjC~9Q2W3P_fHBLybh!TgQV-+c9$ZD@v zR7Ie1B4>PR)y5e>u^vbLBCDO4>Rh?T3CR^va&^x2&nucZ>K9qXV5iHNUn%7A7|ciPto*e_xk_w`x^JiZn9m^hijaWToEN#@z(V! zwG{7Z7g_C9l~e>8CnVEed!-qS-CAG$JYSL39`mp7?lFPJiGKcPh^RPKU9y2J8?lS) zJ7EKj#)Xhfdp(1ylGv5?jc1To6j|-Dzw4WAlw7&S3CR^v<~+_UwF4~UyRqL5lC1Wa z`(=D{?N$UDC;EM5Ac|Ky(}w7GwbV=P*sddYjT8O;m^`d!P-ogvSIhXO-zS5t_NbC& ze4992qceU8$9eaJAwJfVWJWJPbwO6ijLh?KjnM~J~>V>S4bk-T)=331) zPDrkZG9RiEokhl1*MmjQ)sugZLOma@aU$2%6cJu&U)P?l^-*NCQ~9+G*Ek`0p0HKB zMAk^LQ^!7hR@vRFRpH2L*G?MWopl_paYFJup`Aaz{kzUt)<`-Zjc;?m#5Ee%>53@1 z(puA5WPEi`rpRj7S!aBkdrPiyLUKivTt$_v#!TrOzxO=dp-r;dqZagS_7c}PA-N(- z9NJ;fXy-vzJGSd}?iwc~S44?JJ51D)zUh4rWVJ_?=-b5M8trrtlBWrtVCZgt$Sd(* zef7@7YW!E0iYO`NLUiLN8BiJ@0+Uo01k#l|TjYx<{Ty;pilHcbqgnn?D-MmMrC zB2q7&w@nf0S!#HYastMP5>$~BSQ=tUfQlf#JkKlw>RWU**!BGIeSZ&X(!BK686b%l`S{Lytm ztejrQpGUc-C1M4~xk-M4rSY?!esvZb*rR=(kOq5rC>lcblHVNYcY|ek zP(AaaeV&j8Ph!Y`E^HV)p_g?NTPZTrZ8YjMa(z9l4IN`k{6H{%&BNxp)drURq8Ykv$xW)i{n}I)$v7(rmtLZ#=L2e!Qi1b^M*cAtQU&J?-_MIBc;v=q*Ek_rUS=EX-m`hbnYaE6 zw~?%SFUj&a6UYD8M|VNE#tF`rGI6UMbvB4gln9L*<6bmOdi%YobX;T=SFzaMX* zq{vG~_O83jhmMUo-@38Ql@pTXWzKFh=VdJKC0QP4;*;(| z8+^_S;k_g)`fNj!tr=Gw54mnX?R}{dIm=WpwO4DfOF&PcapIHmhXXbA5W)l;%w z&!B6ZhmHk|NY`(5*b8NxMANG2n-S1my=wIh!SG(Ilp@MS(qu6!)-rF*YB>mP{t zi)?B4ag7skZnqjUJP`4_sm6r&(%ss@#z`-K)Xw++;vG`8yT*x2Pk8T4{`(1O$*hY%O$MRk}!^C#Z6>+%63CU#S zrU`m!B;+xH#>eH|pW%q(UO{-T(G14#Hq1vH*Er$TQ+wsJ*3N%bCE2N%a}j8qxMs(H zl@p0q2{U&TM zmiOZAlyMxcapJNA&Y7{{g!hWM7!#$gm?_^meQxT=eK*h!N3MJY*Mw_Yf@9a1K;wk( zsD(&z$c7WdiN@8=-A7w^N3XV5iHNG9vGS1m!Wx=OD9 z%J1Fn&!2UtbUo-AAJ^-x*eEkdc73PzEEFQ;%DqzUQi-0!ns7}^5Yw1I9mBujV z5Uz0|?i$Syu4#$EtFP?%gN@j5LNdMPXV5iHNRBsTcHZ74AlA8S zd|bs;5g7==H7$X?H0Q;J6O!qr*`%iHx^hiRP;X-bjT4f|bWKP@gsr{B;=GCMia_Ip z?-C{Qh(6~E!h88V6(Ta+t@w3@pY_K*(YU8Lb)UgCPDp0W*EU*$UTUA755LyeH7CU6 zy^IuxYn<>a)-oR{S8XMdw^6QQe0|2x4Iq4;_O5%yJ$GKh?!y~R+5vo*P`j?(^UiSV zPu;jH?%t)n#5GRHhVC2;d&#%|?1dx3d+E8sU_%i=xW)-(WU%pxi*Ipv;&8%ysaFL{WZ>uQSdUY>P51<*CEKH7cq8)mZZ zgk;^F%3j_5id*l#&8^NY*(E~nrIA!ynK<}XuiX96S3aQ-Xq=F&ds~@^X9jO}ubiNl z2-S5a6aa)Y99bkYYhE#X*L~&2lQ%LSo_C**y4oXGyRqRKCnRG#uP7q-2qlwI5yVsz zu4xJAz3a|>?7Cgpa6&S@+*{50yGnLld19sIN`!EYyn>KDWDy@Ob>$k#RJhmM-gUow_9G_u5@i}s>Q#I4$+wKBmN{3h zaY8b+uy@^2Cw#luXbF1x%;w0kaoTBnbyk$Jc+4?Z=?po^YDZ2)$C;&ToKO~YB{s~* z6yd!hpOuXd#~HXjdJ|&MI3ZcrBiY8MUiaH?zWnQl_vh_3L9e<>zVnWKcJsW}H9oFn zT_tB5_dWl0<@nJO^b(;vZYAE&}%i0~|DmtLM} zw^xX8Oy@pV6zdPUE?t?XXw_a<3Fi#1aiXu#Ay>NVoMzAo?^VZf#%#(H=W2@ZUU>~-qj@i**l3C2sJ8As|Gbl(!N-zJFQ3i5>+XC0ec!^7 z(=|>==4iKf-TQXjY~z1E@Wp99+BwjjJmp6jVeiV3zvgCdN`z~aD+tAeJWqHpc{SMh z#AW|l-gI(JOR(GRU3aTbpH$8hy_aNaOd}0MRBgBEWk>auIB1+ud=;^PaE%kn0`~T< zJM-4xDqcC^z0^MJ)a%YY>Fy7G<(Er#(X_-+?OJ_PgvOaCE7v$7d06NEZj|O3jbv;n4%TDJZpU@4Yn)I_6;ZB)UE_p%m}fBWaylC1pT|+; z9#gyx6MH+q|EhTxr)Zpr_gU&AXPFNl%X_Ik?u7JvJM*V~*ELQ^=2((*#hkYUy%gWH z4cGX%@n%hJqr~Ba_mW(nGvHNA&`a$@c0F>}__*<2)=k)8EbkTfN@i@h#)-HJu5Faa zo$y|9on8}q$19!t_>tac&?_caBDwO6`h2uI1KBF~uTuRuF*+~lu;I~4rb;}f+AG(z z1oW6dMJE`HznFqZdpPb4$XZm-UjYgz(&OrUXM-YeOF@Lr0X_S%Mz>s8`(P555o`-j@6cWT!-;WeFX zloizy`0f>&iK*kDdpO>&AY9{w*L042itt`uZ*{KX+Icm6Pv<3Z-L@L_nq;*jXTt7G z6pa&cX0RIPL5ZFxyjPrkG=!FuR#cpw6|Z8RS9w3py)AJh!Zl9FL*grh%)~pXb>#H& zm=Y1=>r;Cr56O?)+q=&1u(-wvMIPsmeXjOC@ZP(*>+FR0k{oCHJ#pk$AH4y>HBQ9Y zS>)~bp78TTC%l*BxHc#=nC>lgirmL-f8T;qgdqACj!=^E#X zUY>t))yznH*EM%Pil!yv3Nk$f+Pki~ zQ&b3zORsu9&VT&J;!aWd>9cYKa6)q2De7&U|E2ru`A2-xd3#0BOGG>iNk92i>Z%jr z8Xs3#9}&rhYgz((y2CO&wRA!+UzM@Em(Ty`&fRKcIqk^ukmHi<4adPqYgM~woRAIZaqU8QFL_9No^!kV zQI4r1IQGe_;aDrLAY9{wJcLer$(u!~uAJ~*YL7c!{W0p-?tDslUhA5c7sfvglk#?dhfa~{NS%QOcA2#Rj=AO(%)Tn6uE1h$hO(9;<zA?OoUWoN&>!gs#`JSDW`e zV52>oD1^qPS6^2>5oa;wUbt(VkgR(t*@nM6*b?*-5pTCyuDFhJjgPC0OcSnY3GD4% z=kHlMA(>uYF=a)?Jn9-Jt(@eq6Ozf>-gT$k`z^b!+wb<=*#DU>i9O92 zG(N6ovm&Zv6dEU#1?=rzxA}JOjrXtKmF^DuSl-JkCP&UwGWCl0R?-_AvVkmOShcEl z2-i5F$OqyFcX&YD)6YLs?HVT}4`&(ec_L$ZFO9?=Gd|+zZi@1*UhE&Db@wID$NL`m ztNt9PXj)>}8B`uTWAU*h(@QxVY$#U{u4xJAz3aaA>^;RRCnVF$SDKzEzV6MkV@I4+ zaZX4c_7ZjFc-}@!-ZZo1EmsMIYZM2BY#@uM-g+q-^&&rY94UnGOmV34;wmM@Avvyb zldL+Z?q5Z3eRQlHG%YcG&r$>$Csc*lOObcUL~jGad#MU(&$)tdjT1WWt!!iR;+1SbcrV34d!1d^IH9YqiYU2q!h7lLrmB)c zxW);|)7fnadZ~SS&Ryf<>UQkybS$tpAyXlT3bmZnBL$h3Tg=%S^Qd^q4^7gs-mJD_Mu|UaAt>Yr-{7=$fP=N+mks zy>v}GU01GgLNZlZd({&3Qv39Za*dCx>nZ554aV|b@pDBpHeBOG{ES&`qvXm7?-iMy zA$*>Fhx55faTFU~;eM1+`}BOc#tGk#>s+-2m7%pV*!ZKJk9Z4buR1T`d{;8pPUBS@ zfppqSbY>AhD^t#WV(cXA9EJ8ea@UA<0=W>$h7*$M6+dY+Q|+#4iRro$fyN2R!&Q<0 z^jS;LYcf|#SIQN>d*0@GPxj-P3Izd#RnBZ|}P1XGV+0iE$-zto5(diSdr#sY}|Q{_qoD@$nrepL|YF3>qgS^Y;#){_rnc^TKU$ZaYcPtGBT# zdryzQQ@!XOzU4ViB#n<7^C52sVlfbdrX{eKY%HcC*SJLPF=Zkw#vL^sp_jU|#^0t6 zPl}w{^*1N4-6wj+ClT@AOh^;56G^U!T|uQ*_4L1;*~?+0OSX#*BeEvj=#0)D{w$)7 zL-rEUg+;UBUb&p@6&n$QJ}GiM3E%Typv@j;dPFGtSz^f5J3h|u_{}z?x%&;>rq7-D zgFTk%^cO4l-}1u6e{a41e$VUcD$POCIPsI?x9xfJXEMG2mPhRT*7~2`kJ=pw?{(a( zZ@G5Y=P!C2_uBHQgCbYhNHZl*KKb}BY`W8dZ^EmRh5fhu`e#3}eV>0SHj2iH+uZ#d zo4)dTY~1-b-?jbopJIIsk$W$XY5y%BzV~@sYp?QO?tJw7Key@j|L_YdeXfeP`)~Q~ z_5ZN#{F6(ripGicpZ%4U^Dp_}CeizE`HN>g`hYJUKaSjc{pZEs-E_kTzjRzC{_e#0 z?0D?;`Db`tgEv$oAgG8?JuM`g`5w`6;_jc(1KDfA6O2AMog&IQ*;seaH=G z{7E8Qv8`re|F$Uc0B0~{hS-&y^j6K zxAyFM)IEC}cU`;l`sdstXV*1O9DLmAdoFm>nV08kzs2i5v3}i+dCr~iUYCCQ|J`$! z7k#(4@xr&<=WhRc)vMBcxWFeseXMFzPZ#*l{xoe#Gwc{6i zwqBR%{kQyRzhk!_`N$l(6W;56pMChAGmd^`Z{u(O^J52p>@KG#uYBC!ea$U4edd6? zQk~v^%eHH;I`FDHo{ zb(d}W#0lT-<9OxYy>|V6mz|lsa*Y$u{`Y5X`mOcsyPrSc6^H!lU7k83yw_U}yK2)T z_hDx^^R>4-`03aBF50D?(|2t@!tKB1%+JC~ii z{l8AjYtRYr^~$dux#@S_cKHYL`nbo|?c1MpWUdm|II-o-otu9B{x|eCZt=p8Z~xQZ z&$)6NPS;iP`gc5Y{l~wWYu7auQPu9}{^+yY?(@?eN43M*PISd<_FsF}YuTy4d(5qO z-0d}a4Z6nn!B0HyGkae93xC|t`A^>ZiXESPS^hl}PI#|Ze)P6$FSs4Y!M}XyZ?7D& z=a@Ae4}I64IvRZM#z)3^;uIl`6UdqP`Yn%KS^MYv#o5K8^COPiqkTjyT;l|CCT@Po z5o<5K$FZ`p@U;f5%AkFokj4q*OdNXVkt;i%kmK-m&bYEc`-oV$#tGz1eD>W(to+46 z$ENx4x`LJs+UE&roIuXRSFSu_<*Bz%x$3;y8CN!F9}x@JIDwprjn6)N<;nl+3&&~B z{fre_HfSFa3)eV-oQePE@ypAP{!Nc9)G`N!;j94 zD;u=W6Vf<=oQWqMe$>i6KXBhP?_`WC8??_8(l~*f ziQE3t`jtOD{CCoP_%6!0vO)ViA&nErnRv+wN31>Wg?T>wXu!C#LHj%*jT6Y3IN<(A zt=<39`=9e?$ZlkSnv;hgYZ^$cFR_U%J1`a%Bv z46bpao{uNJ^o;HA{!l*aa~n?AN1WF^Y18(vJ^n>0a@ROf*Y3N1_3A^e`C>i;aKd}l zJHy{xa>yY+yz8GO8?JGpUW2du(N*i;xZ~v`!h6*_!}~vZv-OYpll(K)?v>lCj~|b} z;_lmj{G5EYuf6?* zd_4E-bH9$JeV&lU3FJ)dALk{%b^m-E^s8;gRjz2CC!}!#ITPu=0EoZo3?T-l&~L@Zq61ac<+;~Ph;EWVk~Mg2}Qv~18mPe|hgawcBYi&KKQiaa z?;$a+Y|uVWNaF-@CeFCe(JN28JRdpzeiGx#2JQ2NG)^FA;yJ5Ft*v`&zUE{{_HiZC zK2J!4Cr)SL#se-m_$gn0MmkUQvD}k-r+&(Y(+~O6pFZGiJ8pi9eEs2s_wtyo-RC9Wx^e3te=q;`R@XRDufhNM*k=zq^ZI=K;e_|9 zck0J{{@yzt`VW7VX3#ZG)ce(mkN@o*+xE%lyH0qodKZ1l%U`zR@YC{~`?$VGIeqOu zTVH(qA@6x&{%vnQmLCD!_O<(b{L$y{xV*0`uLeJcd%fkS;r8Z0>|1)bD7`Zg|GheX zZ)tjGVwLu%Km7Bj?zi*H_c=f1YOy%AXyW~XLP(Cc6XM+iy#eu$PyGD);3o-s^&`bw zHvKyj_69_OGtyvhOu9#9>}V#9Ma_8qG7~`uxum?upZx=wf`m zTe4WR(Ry}?PuWd`03&i7P7q%r`pk;aU!Crj@s0P&vK%}0u~}Y3d;VM4%tieF)GKSI z2+`&5J1ds>ELZhU9yiPJ|9gA6y(RMB!k!~Ui*T~oAz#7Z#uPvd*wv_spg+a z#G6NHHf3Ai-BU!?;E8-kR($a@)mPkWYq0$~ALiiZ|MP~`+a3G+eN00}F5G+k!pe5c6A@?M^|3paPq z+i%-2AMz?^_lWb~z53GgPwc%KyuEPq>;Gf1{wHt6t3l($xBuuJtGE5{&AnG&|G~T7 zbn6d~Bllh&(}kNKyyLI8-u0*1hHISIaP{uhb3gfz-o~GN>Mh&XKRchvxTYmu`}zm> z#5><|;2~3lXnK7kX7HEqeL_z}Zn_^`KpaC|dBu3OU%2_S@BH4j%l_1Jm9y)F>|ML> z;k}L9-Qz{uu6n_k@LvCX?{}@9^m2%ofBMDSesJeZxW+~I9jyI=pe&uoAAlk>Aw z*Eq53v`bc>{15l;y}Is6pW1%LXU2s0`p_5NwtDh2kL`(5&%f)AQ~o8N!@0%@ub2xr z|IK0VI_T~Hl<(p=;k|Bq%XhDS_#UkDGhTey`g>fL3D-DL&&S&y_}y)<`*6;c6W+^n zc;V*1f7*8sx~AWKd^hk_bo{H|wEBQ=-mi~*)6U0k{pZuO4c9pFv?uIbJ^1Di>WS`E z7i>TIPjYsh@Lrq0wR82WZ`srnQHhIl|0+MDca0OO-NDAbUw`C|!#CvlaKd|O{;B*6 zH>=ve@ZNle?BiZ~^sds&=?gc%@8IWd|HNV0D_?hRueR~IW1hKo{XwtVBU@g5$Xo8P z&#_)HKYgwfu9+f4|Ma-K9DL{*It%E0-65+NbUvOt;hHHT+c@j9_g#73FCC>b zfR0^X8YgJaHjG%f#tGz1{MRjhYbCylW8p^`WZCnPe)?P|Tr)-FI5d85qw{Jd=R9$txqL9@KI(y}WYo>^7Ae{Gm*^ z#tFA~;NlK5m_-al$iw;NrEHe`MQVd@~M5u5se#Z~xoXM?LzXJ@K3yKd|j@9+3&xIKez+8+U#3cej7=yLlIN!h1dWgiBWs zyWqs$#$z9MuR~6|bKa?4*EqqS|yT=-t%SHj35{3^Vxs1>ab*Gv(j3z2Hqud0#N3wm7bu9+f=jk0R}UIwyy zK^Gf^aLp7^*-+#ikxSzQ?UjvDuAD&5#M$4u_4Y+y?Pz3!aXk*n2QHi-j+P*YnF#lj zM~Wlvcr4>TtFn&V-`|b)MQTs?cN-#W#>DWuUW$zvMm8!Up2Vbk$kE>EyRq2lqGx=P zS4(N2qj5K~ydB*`R`29*RQNLPA@?!8JleDE(LLm%aU#z8mT?a`(W86Fh45Z+C9{ls z$UV{ALoOO8;w*C+_mF#{xrbZ`?-l34%eaT!6A@8o_mIP@9C_?r%eaS}dc}R;(muL} zTr^I^4!4YZ$i0o`9&#bPm*;JC4>^dO_c+sE#y#ZTt2DE{jpiP5vEf8qi!9?Fa!)k( zkPG3xJf_h-+(Ry!mWZ>Ao@nkN7eeFGD|YH-9ItvJaBq4io54pF|+(Rye_lg~38TXKTqPd4$G)}~>WVPGeLoOO8;_7Z0_mF!V z%{}Boc(2&)mT?ccCz^Z6MdO55%;+9+A-q@YiOaZ$+}mjGAs3Ak^?Wqg+c6 zkjpOPt0=BgmvIld_o}&vTr^I^v3(h506o#%LoS5(iYw%0+(Yh(s64-iTr^Iob_W~H zJ>)`oFU>!dpY9bPst`+(S;sLSL!K>II#T=W!3Y5U!abvW>VSycYM6*Axf4zBEqIo^4DK(l~*f z2|w%eqYSd_)i#`P%@mR22twyqDOY~%Lsl>7;#DDBGeu+@aYuM9?jh^Bf$j*WYL~_d z+Ov%*LK-KKGZFWY_ryKqG#^x-Y%s25+UE&roIvi0xbLdFsfvT6jE^gs_IW}YJaIY` z{d^=FzN;aRZ8+i9rwGyM9`Z`uLr!zp`ko z8mdw)ZEw<-wmi zE!%Lyd&NDcW!ytv&0f9vH{Y=SLl4cr1Hv^<#C@%0+(Yh(S3Y3(_QSqe?syfA6U-p_AC%l(uWN}t=54mK~3D5fCtb0FY=l08Al_G zMu~&-TG5h;gR|F?UESH%J>=rm6j5ww)g~K$j*Cnj*e>_TMqW)3#YS0Ceol?7UeHyp zMqW)3*+$$EUWG^K=R_Ohy3@A+kW)^az{AbHE0QJWa90gyTkVX`cOjQQ4>>m&OUtNZA=GLe`x?&R(sq{K@v^Px84B8rfi6k3;g}tgV0iqxDZX zDgR6ZakKKpAc*u1{R9nh{+II4u}l-5 zg()Iu*KK5(_WC{j_)8c1jjic@bIJN0ta@%h`zaS(xv75l+%+vB8bUeB^&5YSqq2cL z+6$q?D!U8UU~kf^)Effdp+!h`ugZ!_Hqf+$JS<*y;gxEu^Jp0tuNXZSIpzJK1}RYjn2!fhw7x{PESPT>2cC){kthHHG>`8b^L zUXrWqR&fxyXk?G6j>9!hjBJ$1UE_p%TUR@Ik9_TBZo;!9lfyzJI}ps8;(#Eg6h}m! zs|4%V(72Lm_bRCgY(Ti}k>m$VhAsa0rdzDwoj^_t@HuAJ` zA+RA&@X);)_LBJfO=T~MzfV(t%ukyl_+dMgz_C4*}cl+J0a^W;WpCn@lUsQ!=Y!DmAdov z#}=6^lsLM~X!pr8D2)@6+qx1nCg?>Lij8cdC-B$DmABBW!NCTE_aX~p8?I>y&u;Hj zcqIaj6O!9%PZd8R=;b*qmDtG=8PzHx@`yOe(9;C-q5MF!xyri({$fMpN~Ya&l~8W_ z8(C{G{$59ErxMBTpqU~>rx~Y z^Z2_ml@09Cj*XPvijW3-lU}9X;xrb%Gp-2Qv5`G;LYkIfS1NHRU!6zGxOi1XuFfJB z5ZG`JTSEQ2$kUZ;tWF{qt@d_B4Fnn|+%|Dc6L_-o^c!bvxW>mN3uIU0>(i|{;l0T8 znDEF&Pi3Kw!=rU#WTQ(?g;%a|!o8J^q1wrNWOz3oYTu=uI1-WUxTYnDDaDcUB+syp z4b7%x+OeU&*)!K*&uNHZFA*W@@*Nr*?$wykSS=wBv5~yeI_x}Jw@3SkSh&UsuQ>y7g;Dak_Bwwua7Hlq3bw&Ebm1Y#x`8j5}w`OsfgUO>x5+Tmb~hMiSL-8m*-H# zuxiO@^xRG45pj~CrwK*a5^b)s9sI?H#+6LF=PIu%*R&p1UX|JvA?xxT8XNA_uzXM-beh4E zD<_b#!8zlYKr=;PW5|^}aRQn4d7|}1_C!MrwM(=*-(}qBndIs7T?mcU5^f{Kk>(@* ze$h5E`o`WT=}3mVzI8jj}>vfdILAF1~2*?!mTvt+l7 zyOBjE3nh+-JQJ>QLbC7Fxxez&67(Vq#YT^00)Ks6c?(^~;bVC(vM{#cnwIeF4sm#P zosdiplUH3Z@$J@Hf?l4Hv@>Y05*2q+^BhD^-D6MD>KH{kaSW^0H7(KRDpUB24S6M* zc5KL>ysBJ-J*OdtN)#dM@*Nr*?$vNShtOCpArG;Uyh>}tqlK0|+UE&uxQ8RJWM~n8 z-?!{hl3C}pV`GTiH7!9+QzbdO@z(?^8|;s?VXvU@cq zG*(Mg>!VYphmFpoWn8>+8}cXT3j!PNVM}Bx?m(8c=FzI%XHdQM(OxoWoN(LB$25T_ zn=dJ(E0rz6+tTTEcA%=ex0Y*xr6l zk!c^#pljl9?-l|XBE_MEMeUv&bDQtuL(4pa_N33{nL zzE!RU0*#L=Z=qSWW5Rop1)q;hxTYnl$kjbQ-I{0D3CZkA$*WG?rwDp^M$*oZcAlW( zy^K5u(KFu5$Qg}@lMFpgD8iO#bCuUR{$fMpN~Ya&l~jB4fkp| z4noL!OSlbXDObEl3oU!JS8*iIT;l{X#IT~`@1HMK4O(}SRh7tIC1XNkwFEUymE`Qk z-&L<{V2^gZN|CEl;@Ng4umOR$5W}7*0^gw(2krBOG%ewg=StV4c(ja5uByn@In4(I zHr&IOP~6&m-dWa~N2_+PEA`eV6KI@p+swx_fhUKZvNmJGH9js`AiK$>x*H&6Cb6?IpX(E7wdBqU$+#0vQ{emyQWEQv^0ru2OBu6DN>qpC?*R zWKT52FoQ&^^IgV`o=KiQ--XavE#WqX^WDo1_{s{q^MU7FUu4?HGw2#8kRgV>T?9KP zv^=DJo{**`ruTMPcN@sGV`G>>*Wk&dS7k3DLiK`1cCUtGErhJM1iLoXo;~~KNk3d+ zJ;&4cB9ny@N2mVUPh8`K{}Nzq2cNrs*#6k$uWxyoxDf3cx)CDZP? z%B#vXt%vTFBFNR~o`{ww#4++JRXl{OJAsT>L$#lI$0H)*d@d@PHAwqBAx%qAZ_Hqh z{DLdLf1KMurX3q8a?N7ARh$WIK#aUfy&>=&TI*c0do}EEu4xI`D_(W|d~_Zyv~18` zh;S%p*fmZdJE17LwRjpHZ_edA@gA+(+c_TyG)}l}=3|;5Kl@#?N@RU?tB*E!c;QQDc0Ot{7gydB$c!h7LiAv#%*S3Yj*VHHQ0fSy3(gk-9{j)OUn zb?>G2(@x8GAjbrnmRKzEiW(DWoKQyQy@K#w$`$Rk4cD{;-i`@0PDo}ixoLu4lQS4| zlHS=aS@d2%Uv^u9nZ>KT%i(X{-EtjEroGfv6iMD4T%-9=98M3!H$Rwv0}TY@YSx^P zoV|kZaYZ|UEF#CC`gMXTaT|CwCcJhb+PccSgO5Aq6*lBgO-SPeGQ_a^pphq%$u8{` z(aDl)T7nu&*^S?lXbU&Ogmm_^qgVWv>sMoRmTsH)@`^~!wh;(u%|eX@oFG8U$%^lZd#3R z+~3P`j(oY)Pw~XJ`MDzXLNi5(uFtPpLbQ7Acg+=Lq?&LIv=i7ER+I?haGLgt(4@o} zStfj3MGFm~zS$1ZDp$~L-bx&)8X>UZ9!kc>Fdv#J{p6oClH)DhBCB09C>vu!nwAg^ zkxn?k} zD%ar2*sG&A?c8wJhuvwHsB3np+RsW^lpOCdB$+G>HZEG-k_gv0p&cALRY^BRcrUV$ zZG>05@X9q#NXA<#G0kE+BO{C6i!2OrsJ|k2jT4^3+JUT7-ijXyfk_%BHca3Tc0vTeM4-xvU z4(y2#w9gaLV9#lYdQWs4$nr{ZArwd1MbY31HfS&N5mY`e!FR^>ykTRQ56?+UxQ#{k zdj~(F>{sz_X!4iHHLhlp_PnAXT;qh>-n;JHQ{S*_-!H!;*>J*psa?MlBFFL2S3V(r ztB^kVDiw_r#6;xkeNa$if@sNfCc>)?c%^kfgp#r6G3{L!UTu^W*>FuuV2|A=d*#-h zs3T{5{ddUK0ckJE9v+a^Bi>?T*cs3aF*)6z0jLT)Y$)3J+PK*US*R<~{dErqIor_a zc|RdqY+#RepTS(euECzu5W_leqjei(B*md)c_-fnBJaCSNXABe&Y=62tUF}QtmHz- zL^)?bqk4rvh8W_|Sh~~6ZU901JRuF9I1N$nS8fAYUP&&5;wbwS8a%-U?RAw9A-QrJ zWMrCc~vRAHY37^5_ zS0}IBx)bg#+3nUo^5jD|koWLxCmu?s-6O9FX`DcY$oq(V!+J!#opG^27V5f!&{!=& z4oe&>@7Q+ME~3ST#+6JvUdf-XBCrABwv$(BR}e>ppxGJRtGrh^A%9z< z@~Ts%2a(Q2-3BsQD3Pmknnl;h1_W{@j^1>)jdy+6MdkcTPjjWw48{maCJTd&i&npr z2-i5Fx`IyGjqAZN;l0R0@oG0-xyA{}c*_hbxB2RhEPAgxa`nxhOfXYUC_dV28?JFe zS(x_93GbzLyv0V|PsnKW*d-hB-*_MA%C!C^5AzXGr)q?d#tCGIVGZI*_*}-thR2bs zQ2Bz;SlELYd6g=W{9r@lN~RsJGTHP~|+qTY99U7kQ=!@U~Tpn9Qc33*s-q!{Fz z?s(}fs`&0g&g)!reqO|M-Y(7HkX`jcGew9la~^gM+Sn4J)l2u}7^x;)1MLJhhFpmt z4yS3KCwyE*3k{KGFe>q&bidVYAbS={92J2Lh>=%iK2AGr?=s#mSc;Gw&;E<7cFkaV znlU1zX$jE~!|`LW_;6(dd$boKLZ`Mefei?}g`l@`rb^7dGp=HyeV&k}B|P%s+Q6e_ zT+bUeX;Tk8jOF;AFCqWhAz1lgKN11Sq6U=7GRUusCgk&q zCFsTc7aPeZMeZ6O*K47+;Tk9A=iCYJHJZWVl@s1e?V}kCs#|l76F#%`&VVO6&t=Y$ z$y*^hxduUnt1=*}Dp~A!N&Z_Y5b8y)oRFNe>jdLEA(?jOBk%9-yZ55q=REH~u5khx zB9E`g(X?^UK2J!aUdpJ`5E^|r1EAMb-sTBuoIr*cj?U~1+6UQjkiD+PgvM$KcA_*N zUBnSbHT`Dpyvz8wl4-{)^{okP$P=d_a&K(Nx{r(OUX2Njg(nam`EdO3XhmR;cDzb) zq~on?oIr*cvYXEhj*oMbEg-gaDA@dlOc_EwYYDb zg3)o9uMen@JIpMv=+3hQ_8!~#p#v$?pHV*C0lZJH8K;E4gmNeWZLtbt0%SV8tgd@G3<#$ggIAkgN&rTq^#P3$ma}B zNXEu+jM7Z$cV@7*k!dePiNiIjF$iRcdWX|**%59|i zD0?CrJi!L-b#{r6T)7R!gje;Rhz3ubhNz#{#rDJj?eJosg_vI%lN4CR~F(C$KThxd`HLn)Z3Z#|>{b zKtm|zY=>x-E5>c}R^sUQ#GGAhxQCLlG0eGElzvmIG?MlEUy;>5oYp@%Iv$%l1kW)k-KsC$PzU?m;4nn7@~C(U^I#3Y9yA#@Y!*i$>MyM}GHb^K$?i z3k__qTj%k@2{a&FTf{sQ$&gw>&`wly54wj)6d^Xm`gm90sn0IL)(pySB2u4w5RcXv z1UAH+4~?a7;pfDGp#7eZhe(`;SkFY)z*bbUn@}Fd**-l)f`${bx@1VLT!V`A3~H(5 znTUr-obIUN^NIi5=T$_L@shcEX$EPpbp_!bCy2H*zUkk;BR*L(KcC20-b*9tTlhPw z%t7<+9w*4@-3cnH2(jP(U_=$4LaL~=4~R-Ok$6t*CMbN$wLrU)12}KBzQN7TAKlGvBe~#}d(cdfB+EJhWz?Zvyok90p5u%ss z#&z^_ENnF58&BeK(Hr#|az9#9FFaQS8kvWvp-7y-ru{@@=IXUaf;PmQ53;3JuAv$( z#87My@(@W-S!uMhy3spz#-IN9U&gMj_YHpH-|~CIa$MTaGw2>Cupwq1B6tS_S`pHI zPspo{LWKI1*UNhxir;CU3`9w%h;9^|UA6ZE1Ai^kAOagUFyXrY;p85J3NFRIWQF@6#5 z*$J<%9y3e=j}x+~;hMS6hxhV|h?r}aipF^HepFR7GEUnyYBwn^2xYhvORb`B=@1DT z8do;$U8@s;283&89(h(NDvc#|#m=~BcvSOV4MG|_A-yH4)bVVgrAPaT$XcQ91UAIn zQE&c2ew%8ZQL@?RwBHl*>;x;6wOh0J{;z$e&qT)6KBpZG)wO2OJx*XlRBv%2h#eXY zkLpZltes#NbRKzTdA6=cJ5gmG3xS45=uwH#O(#Qs_h+sK&-VD%%4L;If;o5MDynOO zNVEzWDKl61xam_!)8pe(1<(9UcrPk_COmWLu_rvIH7fTwaj)SXCuDo(bJtS$@V-KX z(L3#TCY0e$NXIiT9n{g#xUy+K5fAt55iU`^$O(-9h&cXwTr|{RK1*b?qiDY;thiF*zA zIN{MsW1d&kN_(5RiOACS`8X5In(}}kC-Nu?bu=`tY}$R5)XsI!9^n#IpI0J?1R4#G zYMxgRQr`*Js8y){dVQd8GtbB%(C`R7s`+|iTtnYyNxbsdHtkET4viw@aRM7+o(vj` zvxISVme77r$g>m6Y9GhidPvcif?i(3Jg;)4i=m>?^A$4JKBt{L=011NPEcv0 zsy#xyXecV#w4+fu=)dkk&uNHxii(iBVuwb0vlqUyJV}^6QT9$Angk=6q|c?!e39ECtQ5${wYqfCygaL_G8;>*9_)o z=&5dK6d}(}h=!Q=QE`RBqJbXmO+*G{Uj!NuuANckno{N(i`W@gInj;=<4;1Ko$$;v zkIde)Wn8Zt8XCJQ41tD6xYSBW-12&yr`S=?Ix6=#5$|j?%~cSUxf9-NpYuZ^GIx&?%%;yrX}HG;*;HhW zYA5K${I|vkh4|C@s+A~@kLzopHIi_T6Q?YALX<4TmaSz~HH6O2oBp=nPdE352%ED`jg zz32QyFy|1^5cBlGLnNvXw&;ZziV6Y^C%n4qTXQ0UUOqRpqtQfaFqRY85cB?nlR@Vo zCl0os3}-@P?F4JOX0UdX-n*+)#>bUSJ5gmGd5_?pJ;Eib>@7}&)O}oRk7~{!gw)Z4 zaE*EY;n_lqpnb{XRYm0RLI0 z4H1trVjaJ+eMh5u;khEvP|j6hMdAcD?f1kU3EB|zydvAESN#6<)T8~LkY^_pA;g^X zc&Dn@RlJ+kHtm|hWj-G6aRM7+?kEvjaZ-oIW+kqVcOf*^PO#cDs=*$Vx@%z5PE>OS z-Gkm`RK2dqP`#kBJ*v3}A*8+&p1GpZZRq{lr+D_0=WD#r)izb=JVpXRxW@_EoYc%< zY3u~Os6uO$#GygFKCYsLK5IaDFRF0XaL-P7b&sGTDzB~+vZ*1fq#pb11iic>BIX{X zqS5n@$viSn8~U11hC8v>s&t4K4UH?CcCS_KD);OWE>ZQ`6(M!S4vmIKm1}pX#t>59 z3D;2HTJfGOwDf3S^2o^C;{-Ou+)?qKSD#U`+2^$16Y}f?E0k4I)s1(_77eXF?Q`1E z$jo2Wf89eQBCsK9db>UK zs@>O>dh3?&@V3VZ*JeJh3F@O&c&*_cANM{FC%l*JrMj5~d9V-ke{9cb&BHxToHRy> z=yyz}x)ZWJ^SM8$yY?M(6TQAx!nor_?(vx>S$ zL+xnf{>VHavl~LyGJOyqw4C$XHlQxTIBS|j_at~R{LoN9}x2FglGs}U3tWuzrARn zM|%^Rk0bLU(1395+#lIn)zw(U&bZ2!b~NS;x@RXm^ZDGsvxSxh?Pw^TTJi320=tOq z`tw+c@5?IcS$w?1nD(Cea{YN$S7SM$R}G+RmDm;QO?WS=&{6sI=a+!)*PlyD*K;=Ez0TETCC-ZG_u(ED&2Q7)s(U6X zkD3FgkL%C12P6A>{Yvr1U!3jCd1=f%qI2(Bk9{ZBIX|dDMbSWy_L_4=gN}R9a~fjq zC=s5mYj{*i3@J@w&bc&PJLf!l&m9$4Naq=)wS=u1luc9`eI?{^0vlphS7XKXz>225GNd3*jCoh_LbrioHQ~S zJ(`RwpCo=sAJL zJQ+lghtsq##8B$)*$E=-sN#gn>*yB^^t=k!1hGT7wum`{+JkW|a(;ii?6^9&ZMC20 z973L*5Dl?D-gOOZoj$IS3#89>Jm@JWY&7P4Xe?r9ToJV26Y}f?+F9M0y{eaI%eX|f zWFCpE1R5UUB)03%pW;(O`F-=+QE@GAR&>u?wbHE~!x66&+Vjw?k~87Gs6y3hyZ*dq z$_d#-d%FI7A?QWjn2&tKHVpP)wBHkG zKzQBqEXf@!B{Ebbj4M6b(OB;XiFYTY2a#t<-XmzH;u_vO&$VK(X>TGkS5)qC0vlpI z6XUAi)PQE6tNoskXD1XP#Cj&W2DYMd4P{WLoO`4JfsMv`CQ=`0&P0#uOlYi~Sfk=< za;=7lATuO$(V9)#>-2$ej}y|HQEk_s_gLObBgIv}v$`r)-0JZz9w*3&T2=omf@~Eb z_UZcbj!IQP&vQDTyJWZ%Q-hckm8&~(m3f^fR5wU{cSc4in|9BmDbF zwluci$LX=Wmqv=uNPVSDy>~2@toGjaPHX&pTVlF zs_W`bc(hcvzO9GqM(y>TJ{h5G+CB4?kjDvZh`CBokBq;EanYa(=kp5BPVB2>y!f*} z_+@=(MOG9I(XweLs#=9k$g@X?_W8V$x?*QsG(4(#GI&jP;-rz&s2Pl32I0GRp5m8C z+EzPxObrNmWIG`nVm(V-16%9ZHL6znuY1trymDgBxdnq?e}rw6}tb;DryE}?6j%E)UaQ>=_)#*H3nUK@QUzWRG~B? zD*t_))^I{L(X!8L7JV%5MHM=apcF9L!;qQ<^C8>D36_R4fU;E<=MI(?LOx#frcU>LXT>mC3k0RXTgzSA?T@&=83X8_jYIKi}t7xG!kLa!2 zN_a1-&>ACwAl$PPUfnuJq#**26SAq>THT$Xm)B6lT)R{>dcH#Dk#XA4O=K<*%%JK6 zvD7N-r0j_o4UH?C_O8{5Km)?HGpas!sgELITr@nYdG10;V<)7yM3p*TAL!f6b5}v2 zp-70(qf(V>PV}gY>(8;Z2la@LcILbY_goR8yH+XW1U4FRmC$qiZQp09UU;quG;;W? zl_GHhoAwj&aL=wM8e*dD0v$cmR;?CdC^iUrc4E=U*%P7H^R)*>$mZOoomDc= zMEC5(awd*r&WGL`a1CtQ(NH{tss61OI?ls)wgh!j}D)+eJqV7=_-DhM}I(KPj2Isl!o}C~k z@+b;*G&BdYX-6aLlIOXb*1y%4oig1q;viEg$ zP0))fEE+>+iFdwczUR@_-Q@6FcJ3%k6p@_M5 zsc7`nbMmZcWSlm16PXKyGTe!!R#_)yPrPVoT-mgDtxg0Q5U!n3Y3+{qOAVIC*eyigwc1S5}Yv9$;FiAma-^O{BXToIzX zR+1P_V57m~(uH`a7oIBujT}B}rAVB>ru{@@=IXUaf;PllyJV}!yNnwnq<#H(7eZt0 z1ln2M5oJ;yx{r$YJnA<&^$ra-?aK@S^x+26na(PMcpY3O?)t5!exJ3ph}wTxSR{tAy1S3AY84^>p|*@?BfT!ZE6PI$Cc-D7`@S@-<)%Ok@4{mG7Y zAs&;c&ok8AIKBTqC+d;0e(!G4pbFB+bv2Yb#JVzWqRRD4wrFTv*|hr%*7|i1dQL;k zdmIs-t!uDCGmkt+l*c5hR!+!XGz8>%rEgc{{D^y+w$*-5$fNmpLN>&DmbeDC*0F0S zo_Utwkp=`d8tchGB#g^h;!&LmjkObK=Sm#M+Ra+I1~ye7s?4L$cGRT-fnCH8edNu! za2=Z{6_Jzn=xo0Pcu%pU3AzRf7dav6xLF^u(Y@%Av_D_DYKEt40 zgspupTQj?U`<{E8z=oK$67hI^?wIEqSCPjSLkD<6?VMz1s602hUDOucI2JP>(EmHmXRz@cB2g*Jr=})hF7S4>99~dz_FS z^gZFdWXH3l8C3;!+#4{KP6Hy5-aUcG3E4agU!(G|yqDUk;hDg*6H9foP^y*2!sCQ$ zxI{H7^_B2m%;tFp-Ln%!n|ZW`2s}>6rqXNXJ3%kCQ}uhod%d^n?!>N9d!2aWvkX6~ z>MAPPw0EtdxGN!#6WC3R|MnJDk3M)aDp|K)g4E@;{LqOD9%Bh z5}eeW7_7vqFoZO;t{|{Gj~9waHxXKKJ}$Pe#5scy8Vfy$C8}}EN1oKKf$b~5^H7DG zKm)?Hb6sU`2+tPcl=+FER!+$7S~c=rO~*k8;mWEU)RSRxcut z@ju`Hl_%a;fmU^CM?*Q+oVdpcY=}97B8VLt4Ug(fXsn&sUvIDMJzLkKov1R8tgw5W zz;>dtKfawXXUelZzWFcPc?>rRcBd1r{mumS`GJqW$aCc!m3w^L`#hZRUb2_!Dv7x1 zo-@C_Gcy#;`QbUOdAP@kdkyzE;nDVLABu~*M}_10M@1NsrA^hZ2~}|?$ca4aBt;z! zjVqgWpO0FJ?%5+;qSF4$n+T~Zc4#y_s@kzaAf&z%uA#oQt2|q1>CwLAk&(H_3G61u zp)q2u_F3{5-<98Od7Sgr-h@RpJx*Xl%#%SH+7opTF!gA^C*<)6u_5w#RGth{cMWXX zn@}DLfd+&$WJ6T%z#e3%JQx?-_u!e(SUcew8eKO%n|Pyl>hW!V8Hu8Djp{GLJx<8p z*Hvlk1ijQAf1P=13^efgxQZ4!_h9tat)hbPUQ~hVY6f-FCmL%8<=KfP^O3+o;Bi8> z@4@Pih7xM$ZB z4KdGMveo;w(9tvP>-TFRG}cbIM&^<8p>MflKCngW8#ZZQ=fgc$MCYM8rH~WYXi&Fx zKGX}(6@f+$pYx$eoWQ32M8sz~i`XMU8)D7}*=jzZLnrO)`GC+^JD~{C$ooOf$Iv%v z>Lim#-_MCn`*}yX#|dnRnTH6S?b3k8=FHFe$Q7a*Lujm>@N;+GpG(~}uxTf%p27H} zY!PTcxc0hA$dFn=(2mBuKX(s$PD3av-SjD>_w{{Iyyw_9RX7vwaY8m{B|EA#c7k4e z<`t5na*vOzXrWVMB(o9Tiz;*;O}J+#_Bofzc;VG`LbmRGTBCxxAn4^4Syv(z)qNkC zN5*MGH<7tONPQ=kT4fFjjd;<}xUy;YJZe|D2R)}D=Do8BsVjD9G(4&^p|N&C5u%|C zit=n-kMlt-S65Yli0o2cfQ_=V5^{qq+-|Laf6BkXwPu5H@i;$)~Z%ZRr&0y>FV ztAo(lE<)qxIqxI&{)k+3O+!a@N7Va+ebqfASx$hXM4TM`LU0EEHy$$ktp8y z6~5=E_;=g1zy6xCo`kMaKNE@;`kwG!mfuH`7_PxsYlP!gV|rHN(b4W2PRw4B>aE7~ zs25fE`EUEoOW3={B)r#0{`LpoB)_#*?r|dYp072W@Lsa7HQcij<3?j8W-pC@r!hV1 zLF^G!+k35?n7u^YX-tog8|&&;V-ns=_M$Q3U&sDf33^pjkK8Mj$4ZpP$1Qs$K4BV8 zlr4?l`>*eNS$p&K*M95e7vKDNOahOOEBh;-`u3NK_?A!oOAs>;C+KzLp_Ad*gHAkR zmCNJf9(wPAfG&-3OO(gQE!zn@88lNJ6}B{DjhTiMczj&hw6A;6$BOa0*C`J_6FaJv z7~Q^WOwUR@TJN2R*(<7it1&(5MYeISyotSQOu~EVB%;zQ;T|VK@9A2@3GXHQTEjgn zF>W+gV)oMbcN){99>gAHYkSSy$MRmX?=+^ziCBrZ8k6u|vR94x7iUR(yjMl_nET_g z66JBC?Ds}gFKKYDy#CsD`n1Q#mCdPiI^hZrnu&^78Y zdk8%sG?r{9WM6Bny&j#@iI7BU;3*9!9=95^7jq81L}eN^mU=nypFjMaFE7LhWbD17 z?TNabh}OV!MAC%llg5$x95-a|iPm7O(r{wjYRq1oSI|pTrcq<5mlLuf)VbD{_d3>V z=W$77=6IaIK5O979H>r2Mf-_}=yHE#yem=ki5P3IhiTjpszCPASn&@d%D&Q=y{_&D zPecqm$8tiC=c!@c<3u!i-Z{Ki{QH?&-4YM?IDvhNYT!BMqcvzh5#gvvpHAM%a0E+&@)R-i!Ni=p`!CsIk<`i4XtOue`JnF?4BouV{Kc&fVt| zF(B|9ku)LtTEoYUD%=yT!C0l?gxwErwVJ(R51PhCm_~V|;e>4Rh&YZ%0N(3Zm96p4 zgvN3L`>cV-_IYa1eovIf=sZ;A>!|RQ#^W0xO2dh=ucDg0^vr_lYF6)}njR z;k{zqCp7K}_c(!l*1&VjM{Cf2PskIMHlpk+!h1b`14L;!QTEh`XWK7F6mdT_J&HSfyOu|Ld|Zuxt>I&NFWGk*)3XxKw;E2&UXjMF#`LHcv2zArt&&N2FV4y)^cr%n z62=XUC$g1I`#OW}aUwLDPQo;L^mJlpO(oS@f{hjjYR8j^pwZPJ@gjBYc&bz z(ipcyd3@ZmotUd6XRxD+ili-#STUyI1Rft(Htpwig-7F0dupGF8Hp3)iT0hCo>k+~ zdhbNcUaIn)#`LHcdBi!kJ!6@*nuPZXI&L-}dL@-}w>=y2jNYEd94oBuCHq#RJu5Mq zK4};+d&T(Oz7x}?PtZkLmGoVT5Qgj)!D9eKz%Ux%~I``qnu554z5L{8bO zG{!Aa9v`>tjmXykG)wV!JXX)$b=cLu!TFr68cTcniW0i&zqczl-*M4$7j&aBHs_{;Q;*H)$luaJ4 z;k_Q;08tuFlzpW!d+9mu%;Vh%jkOUG`c>vzuP4*EA)>iGVLTa;_GKQ{D^I&)Zi>Dq zpKdyYt&kaNjA`=5J_dtwxldE{6{J26^AeK}8Z XK8|t6Q?#4L1rcNsH6KotU3vUJi3 + + + + + + + + 0.001 + 1000 + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 1 + + 0 0 0.1 0 0 0 + + + + model://rm_gazebo/worlds/place/RMUL2026.stl + 1 1 1 + + + true + + 0.25 0.25 0.25 1 + 0.75 0.75 0.75 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + + model://rm_gazebo/worlds/place/RMUL2026.stl + 1 1 1 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 1 + + + diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt old mode 100644 new mode 100755 From 39fc86fa261577ea3bf69473866ca87e61e617ab Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 25 Mar 2026 16:51:53 +0000 Subject: [PATCH 07/32] Fix bugs. --- rm_common/include/rm_common/decision/power_limit.h | 7 ------- 1 file changed, 7 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 01fce382..2f504fbe 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -258,13 +258,6 @@ class PowerLimit chassis_cmd.power_limit = std::max(0.0, std::floor(chassis_cmd.power_limit * posture_power_scale_)); } - void applyPosturePowerScale(rm_msgs::ChassisCmd& chassis_cmd) const - { - if (std::abs(posture_power_scale_ - 1.0) < 1e-6) - return; - 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 }; From ada64d85ca8b64317ef557702a08a5c951f0ef4b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Thu, 26 Mar 2026 03:17:26 +0000 Subject: [PATCH 08/32] Fix. --- rm_common/include/rm_common/decision/command_sender.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index f357371b..12ffc346 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -944,12 +944,16 @@ class CameraSwitchCommandSender : public CommandSenderBase public: explicit CameraSwitchCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) { - ROS_ASSERT(nh.getParam("camera1_name", camera1_name_) && nh.getParam("camera2_name", camera2_name_)); + ROS_ASSERT(nh.getParam("camera_left_name", camera1_name_) && nh.getParam("camera_right_name", camera2_name_)); + msg_.data = camera2_name_; + } + void switchCameraLeft() + { msg_.data = camera1_name_; } - void switchCamera() + void switchCameraRight() { - msg_.data = msg_.data == camera1_name_ ? camera2_name_ : camera1_name_; + msg_.data = camera2_name_; } void sendCommand(const ros::Time& time) override { From 737ea2d37f40803f0a6b4cd9947e8eb5a030edf4 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 29 Mar 2026 03:19:38 +0000 Subject: [PATCH 09/32] Add rls. --- rm_common/include/rm_common/rls.h | 138 ++++++++++++++++++++++++++++++ 1 file changed, 138 insertions(+) create mode 100644 rm_common/include/rm_common/rls.h diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h new file mode 100644 index 00000000..a127d2c8 --- /dev/null +++ b/rm_common/include/rm_common/rls.h @@ -0,0 +1,138 @@ +// +// Created by xiezd on 3/25/26. +// + +#pragma once + +#include +#include +#include + +#include +#include "eigen_types.h" + +template +class Rls +{ +public: + Rls(int xSize, int ySize, T lambda, T pInit) : xSize_(xSize), ySize_(ySize), lambda_(lambda) + { + if (xSize_ <= 0) + throw std::invalid_argument("rls: xSize should be positive"); + if (ySize_ <= 0) + throw std::invalid_argument("rls: ySize should be positive"); + if (std::abs(lambda_) <= std::numeric_limits::epsilon()) + throw std::invalid_argument("rls: lambda should be non-zero"); + + x_.resize(xSize_, 1); + x_.setZero(); + xt_.resize(1, xSize_); + xt_.setZero(); + + w_.resize(xSize_, ySize_); + w_.setZero(); + output_.resize(xSize_, ySize_); + output_.setZero(); + k_.resize(xSize_, 1); + k_.setZero(); + kNumerator_.resize(xSize_, 1); + kNumerator_.setZero(); + + p_.resize(xSize_, xSize_); + p_.setIdentity(); + p_ *= pInit; + + y_.resize(ySize_, 1); + y_.setZero(); + u_.resize(ySize_, 1); + u_.setZero(); + e_.resize(ySize_, 1); + e_.setZero(); + } + + template + bool setX(const Eigen::MatrixBase& x) + { + static_assert(TX::ColsAtCompileTime == 1 || TX::ColsAtCompileTime == Eigen::Dynamic, + "rls: X should be a column vector"); + if (x.rows() != xSize_ || x.cols() != 1) + return false; + x_ = x; + return true; + } + + template + bool setY(const Eigen::MatrixBase& y) + { + static_assert(TY::ColsAtCompileTime == 1 || TY::ColsAtCompileTime == Eigen::Dynamic, + "rls: Y should be a column vector"); + if (y.rows() != ySize_ || y.cols() != 1) + return false; + y_ = y; + return true; + } + + bool setY(T y) + { + if (ySize_ != 1) + return false; + y_(0, 0) = y; + return true; + } + + bool update() + { + xt_ = x_.transpose(); + u_ = w_.transpose() * x_; + e_ = y_ - u_; + + kNumerator_ = p_ * x_; + T denominator = lambda_ + (xt_ * p_ * x_)(0, 0); + if (std::abs(denominator) <= std::numeric_limits::epsilon()) + return false; + + k_ = kNumerator_ / denominator; + output_ = w_ + k_ * e_.transpose(); + w_ = output_; + + p_ = (p_ - (p_ * x_ * xt_ * p_) / denominator) / lambda_; + return true; + } + + DMat getW() const + { + return w_; + } + + DMat getP() const + { + return p_; + } + + DVec getError() const + { + return e_; + } + + DVec getEstimate() const + { + return u_; + } + +private: + int xSize_; + int ySize_; + T lambda_; + + DVec x_; + DMat xt_; + DMat w_; + DMat output_; + DVec k_; + DVec kNumerator_; + DMat p_; + + DVec y_; + DVec u_; + DVec e_; +}; From 324bb42d343c7370fe8c8639328d0084d9c8348d Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 31 Mar 2026 03:49:12 +0000 Subject: [PATCH 10/32] Add temp. --- .../include/rm_common/decision/power_limit.h | 95 +++++-------------- 1 file changed, 22 insertions(+), 73 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 01fce382..630d3d5d 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -59,30 +59,18 @@ 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("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("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()); } @@ -124,7 +112,7 @@ class PowerLimit } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(6); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } @@ -144,33 +132,7 @@ class PowerLimit { return expect_state_; } - void setGyroPower(rm_msgs::ChassisCmd& chassis_cmd) - { - if (!allow_gyro_cap_ && cap_energy_ >= enable_cap_gyro_threshold_) - allow_gyro_cap_ = true; - if (allow_gyro_cap_ && cap_energy_ <= disable_cap_gyro_threshold_) - allow_gyro_cap_ = false; - if (allow_gyro_cap_ && chassis_power_limit_ < 80) - chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; - else - expect_state_ = NORMAL; - } - void setBurstPower(rm_msgs::ChassisCmd& chassis_cmd) - { - if (!allow_use_cap_ && cap_energy_ >= enable_use_cap_threshold_) - allow_use_cap_ = true; - if (allow_use_cap_ && cap_energy_ <= disable_use_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_; - } - else - 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) @@ -185,7 +147,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); @@ -214,38 +176,36 @@ class PowerLimit private: void charge(rm_msgs::ChassisCmd& chassis_cmd) { - allow_use_cap_ = false; chassis_cmd.power_limit = chassis_power_limit_ * 0.70; } + void normal(rm_msgs::ChassisCmd& chassis_cmd) { - if (is_new_capacitor_) + if (chassis_cmd.power_limit > max_power_limit_) { - chassis_cmd.power_limit = chassis_power_limit_; + chassis_cmd.power_limit = max_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_; + if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) + { + chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; + } + else + { + chassis_cmd.power_limit = chassis_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) - setGyroPower(chassis_cmd); - else - setBurstPower(chassis_cmd); + chassis_cmd.power_limit = burst_power_; } else expect_state_ = NORMAL; @@ -258,29 +218,18 @@ class PowerLimit chassis_cmd.power_limit = std::max(0.0, std::floor(chassis_cmd.power_limit * posture_power_scale_)); } - void applyPosturePowerScale(rm_msgs::ChassisCmd& chassis_cmd) const - { - if (std::abs(posture_power_scale_ - 1.0) < 1e-6) - return; - 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 chassis_power_buffer_{}; + int robot_id_{}, chassis_power_limit_{}; int max_power_limit_{ 70 }; - float cap_energy_; + 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_use_cap_threshold_{}, disable_use_cap_threshold_{}; + double extra_power_{}, 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 }; ros::Time start_burst_time_{}; From c063a9a8a1cbf7dd19248af62c96d033a733d922 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 31 Mar 2026 15:55:55 +0000 Subject: [PATCH 11/32] Add limit. --- rm_common/include/rm_common/math_utilities.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rm_common/include/rm_common/math_utilities.h b/rm_common/include/rm_common/math_utilities.h index 841a5674..08aac317 100644 --- a/rm_common/include/rm_common/math_utilities.h +++ b/rm_common/include/rm_common/math_utilities.h @@ -76,3 +76,13 @@ T alpha(T cutoff, double freq) T te = 1.0 / freq; return 1.0 / (1.0 + tau / te); } + +template +T limit(T val, T min, T max) +{ + if (val > max) + return max; + if (val < min) + return min; + return val; +} From 094324c5c2ac9cb22e61ed1b26dd84a66e02139b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Thu, 2 Apr 2026 09:51:45 +0000 Subject: [PATCH 12/32] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E5=8A=9F=E7=8E=87?= =?UTF-8?q?=E9=99=90=E5=88=B6=E6=9D=A1=E4=BB=B6=E4=BB=A5=E4=BD=BF=E7=94=A8?= =?UTF-8?q?=E6=96=B0=E7=9A=84=E9=98=88=E5=80=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rm_common/include/rm_common/decision/power_limit.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 630d3d5d..575fe385 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -186,7 +186,8 @@ class PowerLimit chassis_cmd.power_limit = max_power_limit_; return; } - if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) + if (cap_state_ != ALLOFF && cap_energy_ > enable_use_cap_threshold_ && + chassis_power_buffer_ > power_buffer_threshold_) { chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; } From bdbbaa1fb7a1691f1f2cbe77de13864ec6d1dc37 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Fri, 3 Apr 2026 08:06:50 +0000 Subject: [PATCH 13/32] Fix time. --- rm_common/include/rm_common/decision/power_limit.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 781a831a..575fe385 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -112,7 +112,7 @@ class PowerLimit } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(10); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } From 4b4b0c1cc28786bb6f2b966347fc169c4a020067 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 4 Apr 2026 02:53:27 +0000 Subject: [PATCH 14/32] =?UTF-8?q?=E7=A7=BB=E9=99=A4=E4=B8=B2=E5=8F=A3?= =?UTF-8?q?=E5=BC=82=E5=B8=B8=E5=A4=84=E7=90=86=E4=B8=AD=E7=9A=84=E9=87=8D?= =?UTF-8?q?=E8=BF=9E=E8=B0=83=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rm_referee/src/main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_referee/src/main.cpp b/rm_referee/src/main.cpp index 19933b29..beaf3768 100644 --- a/rm_referee/src/main.cpp +++ b/rm_referee/src/main.cpp @@ -23,7 +23,6 @@ int main(int argc, char** argv) catch (const serial::SerialException& e) { ROS_ERROR_STREAM("Serial lost: " << e.what()); - referee.reconnect(); } return 0; } From 466c3b1ca5d5a55715e1970c38312d2000e2c10a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Fri, 10 Apr 2026 16:04:53 +0000 Subject: [PATCH 15/32] Add gyro and time window. --- .../include/rm_common/decision/power_limit.h | 91 ++++++++++++++----- 1 file changed, 67 insertions(+), 24 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 575fe385..2288d9cf 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -59,14 +59,22 @@ 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_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("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("enable_normal_cap_threshold", enable_normal_cap_threshold_)) + ROS_ERROR("Enable 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("gyro_power", gyro_power_)) + ROS_ERROR("Gyro 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_)) @@ -91,14 +99,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) { @@ -108,7 +109,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) { @@ -133,6 +133,37 @@ class PowerLimit return expect_state_; } + void setGyroPower(rm_msgs::ChassisCmd& chassis_cmd) + { + if (!allow_gyro_cap_ && cap_energy_ >= enable_gyro_cap_threshold_) + allow_gyro_cap_ = true; + if (allow_gyro_cap_ && cap_energy_ <= disable_gyro_cap_threshold_) + allow_gyro_cap_ = false; + if (allow_gyro_cap_) + { + chassis_cmd.power_limit = gyro_power_; + } + else + expect_state_ = NORMAL; + } + + void setBurstPower(rm_msgs::ChassisCmd& chassis_cmd) + { + if (!allow_use_cap_ && cap_energy_ >= enable_burst_cap_threshold_) + allow_use_cap_ = true; + 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(0.5)) + chassis_cmd.power_limit = burst_power_; + else + expect_state_ = NORMAL; + } + else + 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) @@ -176,17 +207,14 @@ class PowerLimit private: void charge(rm_msgs::ChassisCmd& chassis_cmd) { + allow_use_cap_ = false; chassis_cmd.power_limit = chassis_power_limit_ * 0.70; } void normal(rm_msgs::ChassisCmd& chassis_cmd) { - if (chassis_cmd.power_limit > max_power_limit_) - { - chassis_cmd.power_limit = max_power_limit_; - return; - } - if (cap_state_ != ALLOFF && cap_energy_ > enable_use_cap_threshold_ && + allow_use_cap_ = false; + if (cap_state_ != ALLOFF && cap_energy_ > enable_normal_cap_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) { chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; @@ -195,6 +223,10 @@ class PowerLimit { chassis_cmd.power_limit = chassis_power_limit_; } + if (chassis_cmd.power_limit > max_power_limit_) + { + chassis_cmd.power_limit = max_power_limit_; + } } void zero(rm_msgs::ChassisCmd& chassis_cmd) @@ -206,7 +238,14 @@ class PowerLimit { if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) { - chassis_cmd.power_limit = burst_power_; + if (is_gyro) + { + setGyroPower(chassis_cmd); + } + else + { + setBurstPower(chassis_cmd); + } } else expect_state_ = NORMAL; @@ -219,6 +258,8 @@ class PowerLimit chassis_cmd.power_limit = std::max(0.0, std::floor(chassis_cmd.power_limit * posture_power_scale_)); } + uint8_t expect_state_{}, cap_state_{}; + int chassis_power_buffer_{}; int robot_id_{}, chassis_power_limit_{}; int max_power_limit_{ 70 }; @@ -226,11 +267,13 @@ class PowerLimit double safety_power_{}; double capacitor_threshold_{}; double power_buffer_threshold_{ 50.0 }; - double enable_use_cap_threshold_{}, disable_use_cap_threshold_{}; - double extra_power_{}, burst_power_{}; + double enable_burst_cap_threshold_{}, disable_burst_cap_threshold_{}; + double enable_gyro_cap_threshold_{}, disable_gyro_cap_threshold_{}; + double enable_normal_cap_threshold_{}; + double extra_power_{}, burst_power_{}, gyro_power_{}; double power_gain_{}; - 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 }; ros::Time start_burst_time_{}; From 7c9797e9f0a07c04cae643e8e0496fbc935e6680 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Fri, 10 Apr 2026 16:23:22 +0000 Subject: [PATCH 16/32] Fix name. --- rm_common/include/rm_common/decision/power_limit.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 2288d9cf..53a4b9c6 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -63,7 +63,7 @@ class PowerLimit 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("enable_normal_cap_threshold", enable_normal_cap_threshold_)) + if (!nh.getParam("disable_normal_cap_threshold", disable_normal_cap_threshold_)) ROS_ERROR("Enable 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()); @@ -214,7 +214,7 @@ class PowerLimit void normal(rm_msgs::ChassisCmd& chassis_cmd) { allow_use_cap_ = false; - if (cap_state_ != ALLOFF && cap_energy_ > enable_normal_cap_threshold_ && + 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_; @@ -269,7 +269,7 @@ class PowerLimit double power_buffer_threshold_{ 50.0 }; double enable_burst_cap_threshold_{}, disable_burst_cap_threshold_{}; double enable_gyro_cap_threshold_{}, disable_gyro_cap_threshold_{}; - double enable_normal_cap_threshold_{}; + double disable_normal_cap_threshold_{}; double extra_power_{}, burst_power_{}, gyro_power_{}; double power_gain_{}; From 76339905db41920ef28d0e0e4a804d723bc88045 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 11 Apr 2026 08:07:57 +0000 Subject: [PATCH 17/32] Fix name. --- rm_common/include/rm_common/decision/power_limit.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 53a4b9c6..83731d13 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -64,7 +64,7 @@ class PowerLimit 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("Enable normal cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); + 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_)) From 5877a0dbf8f23fe4fbaa5658143291f2cc74b5e2 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 12 Apr 2026 14:46:52 +0000 Subject: [PATCH 18/32] Add w and u set. --- rm_common/include/rm_common/rls.h | 51 ++++++++++++++++++++++--------- 1 file changed, 37 insertions(+), 14 deletions(-) diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h index a127d2c8..79f7f2c6 100644 --- a/rm_common/include/rm_common/rls.h +++ b/rm_common/include/rm_common/rls.h @@ -11,18 +11,19 @@ #include #include "eigen_types.h" +#define RLS_ASSERT(condition, message) \ + if (!(condition)) \ + throw std::invalid_argument(message) + template class Rls { public: Rls(int xSize, int ySize, T lambda, T pInit) : xSize_(xSize), ySize_(ySize), lambda_(lambda) { - if (xSize_ <= 0) - throw std::invalid_argument("rls: xSize should be positive"); - if (ySize_ <= 0) - throw std::invalid_argument("rls: ySize should be positive"); - if (std::abs(lambda_) <= std::numeric_limits::epsilon()) - throw std::invalid_argument("rls: lambda should be non-zero"); + RLS_ASSERT(xSize_ > 0, "rls: xSize should be positive"); + RLS_ASSERT(ySize_ > 0, "rls: ySize should be positive"); + RLS_ASSERT(std::abs(lambda_) > std::numeric_limits::epsilon(), "rls: lambda should be non-zero"); x_.resize(xSize_, 1); x_.setZero(); @@ -55,8 +56,7 @@ class Rls { static_assert(TX::ColsAtCompileTime == 1 || TX::ColsAtCompileTime == Eigen::Dynamic, "rls: X should be a column vector"); - if (x.rows() != xSize_ || x.cols() != 1) - return false; + RLS_ASSERT(x.rows() == xSize_ && x.cols() == 1, "rls: X size mismatch"); x_ = x; return true; } @@ -66,20 +66,43 @@ class Rls { static_assert(TY::ColsAtCompileTime == 1 || TY::ColsAtCompileTime == Eigen::Dynamic, "rls: Y should be a column vector"); - if (y.rows() != ySize_ || y.cols() != 1) - return false; + RLS_ASSERT(y.rows() == ySize_ && y.cols() == 1, "rls: Y size mismatch"); y_ = y; return true; } bool setY(T y) { - if (ySize_ != 1) - return false; + RLS_ASSERT(ySize_ == 1, "rls: setY(scalar) requires ySize==1"); y_(0, 0) = y; return true; } + template + bool setW(const Eigen::MatrixBase& w) + { + RLS_ASSERT(w.rows() == xSize_ && w.cols() == ySize_, "rls: W size mismatch"); + w_ = w; + return true; + } + + template + bool setU(const Eigen::MatrixBase& u) + { + static_assert(TU::ColsAtCompileTime == 1 || TU::ColsAtCompileTime == Eigen::Dynamic, + "rls: U should be a column vector"); + RLS_ASSERT(u.rows() == ySize_ && u.cols() == 1, "rls: U size mismatch"); + u_ = u; + return true; + } + + bool setU(T u) + { + RLS_ASSERT(ySize_ == 1, "rls: setU(scalar) requires ySize==1"); + u_(0, 0) = u; + return true; + } + bool update() { xt_ = x_.transpose(); @@ -88,8 +111,8 @@ class Rls kNumerator_ = p_ * x_; T denominator = lambda_ + (xt_ * p_ * x_)(0, 0); - if (std::abs(denominator) <= std::numeric_limits::epsilon()) - return false; + RLS_ASSERT(std::abs(denominator) > std::numeric_limits::epsilon(), + "rls: denominator too small, numerical instability"); k_ = kNumerator_ / denominator; output_ = w_ + k_ * e_.transpose(); From 5891385f9ea7804ca2132d6f6b21f531f2fbd2f5 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 13 Apr 2026 03:52:26 +0000 Subject: [PATCH 19/32] Fix math. --- rm_common/include/rm_common/rls.h | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h index 79f7f2c6..7d1f81cd 100644 --- a/rm_common/include/rm_common/rls.h +++ b/rm_common/include/rm_common/rls.h @@ -106,7 +106,6 @@ class Rls bool update() { xt_ = x_.transpose(); - u_ = w_.transpose() * x_; e_ = y_ - u_; kNumerator_ = p_ * x_; From 16c738b4fbc89e4ede3d6b97636f76f4ca7a915e Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 13 Apr 2026 13:10:49 +0000 Subject: [PATCH 20/32] New powerlimit. --- rm_common/include/rm_common/decision/power_limit.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 83731d13..b2cd2b9d 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -155,10 +155,7 @@ class PowerLimit allow_use_cap_ = false; if (allow_use_cap_) { - if (ros::Time::now() - start_burst_time_ > ros::Duration(0.5)) - chassis_cmd.power_limit = burst_power_; - else - expect_state_ = NORMAL; + chassis_cmd.power_limit = burst_power_; } else expect_state_ = NORMAL; From 3867decfe8e40de24f1b5f3a0649fb813f0e3b9c Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 14 Apr 2026 07:45:08 +0000 Subject: [PATCH 21/32] Fix bugs. --- rm_common/include/rm_common/decision/heat_limit.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index c7e75d8b..f42b55fe 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -153,8 +153,6 @@ class HeatLimit return shoot_frequency_; double shooter_cooling_heat = (use_local_heat_ || !referee_is_online_) ? local_shooter_cooling_heat_ : shooter_cooling_heat_; - if (!referee_is_online_) - return 5.0; if (shooter_cooling_limit_ - shooter_cooling_heat < bullet_heat_) return 0.0; else if (shooter_cooling_limit_ - shooter_cooling_heat == bullet_heat_) From 7bc78a679c96824e673085ab459cf83e41fb3d11 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 28 Apr 2026 08:54:11 +0000 Subject: [PATCH 22/32] Add new safety power. Co-authored-by: Copilot --- .../include/rm_common/decision/power_limit.h | 87 +++++++++++-------- 1 file changed, 49 insertions(+), 38 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index b2cd2b9d..7f719133 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -77,10 +78,8 @@ class PowerLimit ROS_ERROR("Gyro 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("total_burst_time", total_burst_time_)) - ROS_ERROR("total burst time no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("robot_type", robot_type_)) + ROS_WARN("Only standard and hero robot types are supported (namespace: %s)", nh.getNamespace().c_str()); } typedef enum { @@ -91,47 +90,64 @@ class PowerLimit TEST = 4, } Mode; + typedef enum + { + // Melee and Remote for hero, Burst and Health for standard + // Not used now. + Melee = 0, + Remote = 1, + Burst = 2, + Health = 3, + } RobotPerformanceSelect; + void updateSafetyPower(int safety_power) { - if (safety_power > 0) - safety_power_ = safety_power; - ROS_INFO("update safety power: %d", safety_power); + if (robot_type_ == "standard") + safety_power_ = 40 + robot_id_ * 5; + else if (robot_type_ == "hero") + safety_power_ = 45 + robot_id_ * 5; + else + safety_power_ = safety_power > 0 ? safety_power : safety_power_; + ROS_WARN("update safety power: %.0f", safety_power_); } - void updateState(uint8_t state) + + void updateBurstPower(int burst_power) { - expect_state_ = state; + burst_power_ = burst_power > 0 ? burst_power : burst_power_; + ROS_INFO("update burst power: %.0f", burst_power_); } + + void updateState(uint8_t state) + { expect_state_ = state; } + void setGameRobotData(const rm_msgs::GameRobotStatus data) { + robot_level_ = data.robot_level; robot_id_ = data.robot_id; chassis_power_limit_ = data.chassis_power_limit; } + void setChassisPowerBuffer(const rm_msgs::PowerHeatData data) - { - chassis_power_buffer_ = data.chassis_power_buffer; - } + { chassis_power_buffer_ = data.chassis_power_buffer; } + void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } + void setRefereeStatus(bool status) - { - referee_is_online_ = status; - } + { referee_is_online_ = status; } + void setStartBurstTime(const ros::Time start_burst_time) - { - start_burst_time_ = start_burst_time; - } + { start_burst_time_ = start_burst_time; } + ros::Time getStartBurstTime() const - { - return start_burst_time_; - } + { return start_burst_time_; } + uint8_t getState() - { - return expect_state_; - } + { return expect_state_; } void setGyroPower(rm_msgs::ChassisCmd& chassis_cmd) { @@ -166,7 +182,7 @@ class PowerLimit if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER) chassis_cmd.power_limit = 400; else - { // standard and hero + { if (referee_is_online_) { if (capacity_is_online_ && expect_state_ != ALLOFF) @@ -196,7 +212,10 @@ class PowerLimit normal(chassis_cmd); } else + { + updateSafetyPower(safety_power_); chassis_cmd.power_limit = safety_power_; + } } applyPosturePowerScale(chassis_cmd); } @@ -221,28 +240,20 @@ class PowerLimit chassis_cmd.power_limit = chassis_power_limit_; } 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; - } + { 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_gyro) - { setGyroPower(chassis_cmd); - } else - { setBurstPower(chassis_cmd); - } } else expect_state_ = NORMAL; @@ -257,9 +268,11 @@ class PowerLimit uint8_t expect_state_{}, cap_state_{}; + std::string robot_type_{}; int chassis_power_buffer_{}; - int robot_id_{}, chassis_power_limit_{}; - int max_power_limit_{ 70 }; + int robot_id_{}, robot_level_{}; + int chassis_power_limit_{}; + int max_power_limit_{ 220 }; float cap_energy_{}; double safety_power_{}; double capacitor_threshold_{}; @@ -268,13 +281,11 @@ class PowerLimit double enable_gyro_cap_threshold_{}, disable_gyro_cap_threshold_{}; double disable_normal_cap_threshold_{}; double extra_power_{}, burst_power_{}, gyro_power_{}; - double power_gain_{}; bool allow_gyro_cap_{ false }, allow_use_cap_{ false }; double posture_power_scale_{ 1.0 }; ros::Time start_burst_time_{}; - int total_burst_time_{}; bool referee_is_online_{ false }; bool capacity_is_online_{ false }; From c7f26f285d444963660cd88c88ca02762eaf111b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 28 Apr 2026 08:55:47 +0000 Subject: [PATCH 23/32] Fix format. --- .../include/rm_common/decision/power_limit.h | 28 ++++++++++++++----- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 7f719133..2cf4b9dc 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -118,7 +118,9 @@ class PowerLimit } void updateState(uint8_t state) - { expect_state_ = state; } + { + expect_state_ = state; + } void setGameRobotData(const rm_msgs::GameRobotStatus data) { @@ -128,7 +130,9 @@ class PowerLimit } void setChassisPowerBuffer(const rm_msgs::PowerHeatData data) - { chassis_power_buffer_ = data.chassis_power_buffer; } + { + chassis_power_buffer_ = data.chassis_power_buffer; + } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { @@ -138,16 +142,24 @@ class PowerLimit } void setRefereeStatus(bool status) - { referee_is_online_ = status; } + { + referee_is_online_ = status; + } void setStartBurstTime(const ros::Time start_burst_time) - { start_burst_time_ = start_burst_time; } + { + start_burst_time_ = start_burst_time; + } ros::Time getStartBurstTime() const - { return start_burst_time_; } + { + return start_burst_time_; + } uint8_t getState() - { return expect_state_; } + { + return expect_state_; + } void setGyroPower(rm_msgs::ChassisCmd& chassis_cmd) { @@ -244,7 +256,9 @@ class PowerLimit } void zero(rm_msgs::ChassisCmd& chassis_cmd) - { chassis_cmd.power_limit = 0.0; } + { + chassis_cmd.power_limit = 0.0; + } void burst(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { From 8a587f49017434f7d336318e6c264bae08a08a1c Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Thu, 30 Apr 2026 15:25:04 +0000 Subject: [PATCH 24/32] Add traj. --- .../include/rm_common/decision/power_limit.h | 2 +- rm_common/include/rm_common/traj_gen.h | 38 +++++++++++++++++++ 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 2cf4b9dc..30e32cb8 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -136,7 +136,7 @@ class PowerLimit void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(5); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } diff --git a/rm_common/include/rm_common/traj_gen.h b/rm_common/include/rm_common/traj_gen.h index 6d00c022..1d3e065c 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, T af) + { + 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_ + af * (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_{}; +}; From 8d5d361e11dfac544db5468c52169601d1b4b5d5 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 4 May 2026 03:34:19 +0000 Subject: [PATCH 25/32] Fix stream. Co-authored-by: Copilot --- rm_common/include/rm_common/decision/power_limit.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 30e32cb8..cab58a8b 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -108,7 +108,7 @@ class PowerLimit safety_power_ = 45 + robot_id_ * 5; else safety_power_ = safety_power > 0 ? safety_power : safety_power_; - ROS_WARN("update safety power: %.0f", safety_power_); + ROS_WARN_THROTTLE(2.0, "update safety power: %.0f", safety_power_); } void updateBurstPower(int burst_power) From 29814dc5894aeb132bf12c912b1478f7f3fdc914 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Thu, 7 May 2026 03:08:09 +0000 Subject: [PATCH 26/32] Add zipui and remove cover. Co-authored-by: Copilot --- .../include/rm_common/decision/power_limit.h | 12 +--------- rm_msgs/msg/referee/ManualToReferee.msg | 2 +- rm_referee/include/rm_referee/referee_base.h | 2 +- rm_referee/include/rm_referee/ui/flash_ui.h | 14 ------------ .../include/rm_referee/ui/trigger_change_ui.h | 22 +++++++++++++++++++ rm_referee/src/referee_base.cpp | 12 +++++----- rm_referee/src/ui/flash_ui.cpp | 14 ------------ rm_referee/src/ui/trigger_change_ui.cpp | 16 ++++++++++++++ 8 files changed, 47 insertions(+), 47 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 957ed3dd..3dcf20f1 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -78,7 +78,7 @@ class PowerLimit ROS_ERROR("Gyro 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("robot_type", robot_type_)) + if (!nh.getParam("/rm_manual/robot_type", robot_type_)) ROS_WARN("Only standard and hero robot types are supported (namespace: %s)", nh.getNamespace().c_str()); } typedef enum @@ -90,16 +90,6 @@ class PowerLimit TEST = 4, } Mode; - typedef enum - { - // Melee and Remote for hero, Burst and Health for standard - // Not used now. - Melee = 0, - Remote = 1, - Burst = 2, - Health = 3, - } RobotPerformanceSelect; - void updateSafetyPower(int safety_power) { if (robot_type_ == "standard") diff --git a/rm_msgs/msg/referee/ManualToReferee.msg b/rm_msgs/msg/referee/ManualToReferee.msg index a51c9af6..9e1d83bc 100644 --- a/rm_msgs/msg/referee/ManualToReferee.msg +++ b/rm_msgs/msg/referee/ManualToReferee.msg @@ -3,7 +3,7 @@ time start_burst_time #standard uint8 shoot_frequency -bool cover_state +bool zip_state #hero bool gimbal_eject diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 365fbef8..ff8c77ef 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -107,6 +107,7 @@ class RefereeBase CameraTriggerChangeUi* camera_trigger_change_ui_{}; FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{}; GyroTriggerChangeUi* gyro_trigger_change_ui_{}; + ZipTriggerChangeUi* zip_trigger_change_ui_{}; BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; @@ -131,7 +132,6 @@ class RefereeBase FixedUi* fixed_ui_{}; - CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; DeployFlashUi* deploy_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 4c9fe4c5..c899c644 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -64,20 +64,6 @@ class CustomizeDisplayFlashUi : public FlashGroupUi uint32_t symbol_; }; -class CoverFlashUi : public FlashUi -{ -public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; - void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; - -private: - void display(const ros::Time& time) override; - - uint8_t cover_state_; -}; - class SpinFlashUi : public FlashUi { 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 226e28d3..8164b2fc 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -103,6 +103,28 @@ class GyroTriggerChangeUi : public TriggerChangeUi int fill_width_{}; }; +class ZipTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit ZipTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "zip", graph_queue, character_queue) + { + outline_width_ = graph_->getConfig().width; + const auto config = graph_->getConfig(); + const int rect_w = std::abs(config.end_x - config.start_x); + const int rect_h = std::abs(config.end_y - config.start_y); + fill_width_ = std::max(outline_width_ + 1, std::min(rect_w, rect_h) / 1); + } + void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) override; + +private: + void update() override; + uint8_t zip_mode_{}; + int outline_width_{}; + int fill_width_{}; +}; + class ShooterTriggerChangeUi : public TriggerChangeUi { public: diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index a3e3ecd4..217f9f45 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -85,6 +85,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) new FrictionSpeedTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gyro") gyro_trigger_change_ui_ = new GyroTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "zip") + zip_trigger_change_ui_ = new ZipTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gripper") gripper_state_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "gripper", &graph_queue_, &character_queue_); @@ -153,8 +155,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) ui_nh.getParam("flash", rpc_value); for (int i = 0; i < rpc_value.size(); i++) { - if (rpc_value[i]["name"] == "cover") - cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + // if (rpc_value[i]["name"] == "cover") + // 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") @@ -501,10 +503,8 @@ void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& d gimbal_trigger_change_ui_->updateManualCmdData(data); if (target_trigger_change_ui_ && !is_adding_) target_trigger_change_ui_->updateManualCmdData(data); - if (cover_flash_ui_ && !is_adding_) - cover_flash_ui_->updateManualCmdData(data, ros::Time::now()); - // if (burst_flash_ui_ && !is_adding_) - // burst_flash_ui_->updateBurstTimeData(data); + if (zip_trigger_change_ui_ && !is_adding_) + zip_trigger_change_ui_->updateManualCmdData(data); } void RefereeBase::radarDataCallBack(const std_msgs::Int8MultiArrayConstPtr& data) { diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index ca77b696..fc1643b9 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -139,20 +139,6 @@ void CustomizeDisplayFlashUi::display(const ros::Time& time) } } -void CoverFlashUi::display(const ros::Time& time) -{ - if (!cover_state_) - graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateFlashUiForQueue(time, cover_state_, true); -} - -void CoverFlashUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, - const ros::Time& last_get_data_time) -{ - cover_state_ = data->cover_state; - display(last_get_data_time); -} - void SpinFlashUi::display(const ros::Time& time) { if (chassis_mode_ != rm_msgs::ChassisCmd::RAW) diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index dd46e50d..31ae1e6e 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -245,6 +245,22 @@ void GyroTriggerChangeUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstP update(); } +void ZipTriggerChangeUi::update() +{ + if (zip_mode_) + graph_->setWidth(fill_width_); + else + graph_->setWidth(outline_width_); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(true); +} + +void ZipTriggerChangeUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) +{ + zip_mode_ = data->zip_state; + update(); +} + void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); From d33de662042fe70d5591dcd2bbcfb20292878c59 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Thu, 7 May 2026 03:33:18 +0000 Subject: [PATCH 27/32] Add lane line offset. Co-authored-by: Copilot --- rm_referee/include/rm_referee/ui/time_change_ui.h | 3 +++ rm_referee/src/ui/time_change_ui.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 4 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 bc512091..821049f5 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -143,6 +143,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 +169,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_; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 6256855b..f85c2db2 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -208,16 +208,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); } } From ac8b72961022d01e40c8b086bf0f2af0f9fe338a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 10 May 2026 03:11:55 +0000 Subject: [PATCH 28/32] Fix zip ui. --- .../include/rm_common/decision/power_limit.h | 17 ++++++++++++++--- rm_referee/src/referee_base.cpp | 2 ++ rm_referee/src/ui/trigger_change_ui.cpp | 2 +- 3 files changed, 17 insertions(+), 4 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 3dcf20f1..30e32cb8 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -78,7 +78,7 @@ class PowerLimit ROS_ERROR("Gyro 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("/rm_manual/robot_type", robot_type_)) + if (!nh.getParam("robot_type", robot_type_)) ROS_WARN("Only standard and hero robot types are supported (namespace: %s)", nh.getNamespace().c_str()); } typedef enum @@ -90,6 +90,16 @@ class PowerLimit TEST = 4, } Mode; + typedef enum + { + // Melee and Remote for hero, Burst and Health for standard + // Not used now. + Melee = 0, + Remote = 1, + Burst = 2, + Health = 3, + } RobotPerformanceSelect; + void updateSafetyPower(int safety_power) { if (robot_type_ == "standard") @@ -98,7 +108,7 @@ class PowerLimit safety_power_ = 45 + robot_id_ * 5; else safety_power_ = safety_power > 0 ? safety_power : safety_power_; - ROS_WARN_THROTTLE(2.0, "update safety power: %.0f", safety_power_); + ROS_WARN("update safety power: %.0f", safety_power_); } void updateBurstPower(int burst_power) @@ -126,7 +136,7 @@ class PowerLimit void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(5); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } @@ -140,6 +150,7 @@ class PowerLimit { start_burst_time_ = start_burst_time; } + ros::Time getStartBurstTime() const { return start_burst_time_; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 217f9f45..a9041dc2 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -261,6 +261,8 @@ void RefereeBase::addUi() friction_speed_trigger_change_ui_->addForQueue(); if (gyro_trigger_change_ui_) gyro_trigger_change_ui_->addForQueue(); + if (zip_trigger_change_ui_) + zip_trigger_change_ui_->addForQueue(); if (bullet_time_change_ui_) { bullet_time_change_ui_->reset(); diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 31ae1e6e..b06b9eab 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -252,7 +252,7 @@ void ZipTriggerChangeUi::update() else graph_->setWidth(outline_width_); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateForQueue(true); + updateForQueue(false); } void ZipTriggerChangeUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) From 67c8179f9bb998dffd363518c3c949022bd281d9 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 11 May 2026 04:20:10 +0000 Subject: [PATCH 29/32] Fix rls. Co-authored-by: Copilot --- rm_common/include/rm_common/rls.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h index 7d1f81cd..2aba28af 100644 --- a/rm_common/include/rm_common/rls.h +++ b/rm_common/include/rm_common/rls.h @@ -106,6 +106,9 @@ class Rls bool update() { xt_ = x_.transpose(); + // Compute predicted output using current weights: u_pred = x^T * w + u_ = xt_ * w_; + // Compute prediction error: e = y - u_pred e_ = y_ - u_; kNumerator_ = p_ * x_; @@ -113,11 +116,14 @@ class Rls RLS_ASSERT(std::abs(denominator) > std::numeric_limits::epsilon(), "rls: denominator too small, numerical instability"); + // Compute gain vector: k = P*x / (lambda + x^T*P*x) k_ = kNumerator_ / denominator; + // Update parameters: w = w + k * e^T output_ = w_ + k_ * e_.transpose(); w_ = output_; - p_ = (p_ - (p_ * x_ * xt_ * p_) / denominator) / lambda_; + // Update covariance matrix: P = (P - k*x^T*P) / lambda + p_ = (p_ - k_ * xt_ * p_) / lambda_; return true; } From ecbc63a82122a1cdac8cad54c4db2742eb91b0e7 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 11 May 2026 17:49:23 +0000 Subject: [PATCH 30/32] Fix rls. --- rm_common/include/rm_common/rls.h | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h index 2aba28af..cf0f2030 100644 --- a/rm_common/include/rm_common/rls.h +++ b/rm_common/include/rm_common/rls.h @@ -107,7 +107,6 @@ class Rls { xt_ = x_.transpose(); // Compute predicted output using current weights: u_pred = x^T * w - u_ = xt_ * w_; // Compute prediction error: e = y - u_pred e_ = y_ - u_; From 6a70b6b4b5443d10d534149ecb8831623442b5ba Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 2 Jun 2026 12:44:44 +0000 Subject: [PATCH 31/32] Fix cap topic data name. --- rm_common/include/rm_common/decision/power_limit.h | 10 ---------- .../msg/referee/PowerManagementSampleAndStatusData.msg | 5 +++-- rm_referee/src/referee.cpp | 4 ++-- 3 files changed, 5 insertions(+), 14 deletions(-) diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index 30e32cb8..2c2237be 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -90,16 +90,6 @@ class PowerLimit TEST = 4, } Mode; - typedef enum - { - // Melee and Remote for hero, Burst and Health for standard - // Not used now. - Melee = 0, - Remote = 1, - Burst = 2, - Health = 3, - } RobotPerformanceSelect; - void updateSafetyPower(int safety_power) { if (robot_type_ == "standard") diff --git a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg index 9946e9aa..ff9b2baa 100644 --- a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg +++ b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg @@ -1,9 +1,10 @@ float32 chassis_power -float32 chassis_expect_power -float32 capacity_recent_charge_power +float32 chassis_error_code +float32 capacity_receive_message float32 capacity_remain_charge uint8 capacity_discharge_power uint8 state_machine_running_state + uint8 power_management_protection_info uint8 power_management_topology diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 2d98e329..df5083cf 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -699,8 +699,8 @@ int Referee::unpack(uint8_t* rx_data) uint8_t data[sizeof(rm_referee::PowerManagementSampleAndStatusData)]; memcpy(&data, rx_data + 7, sizeof(rm_referee::PowerManagementSampleAndStatusData)); sample_and_status_pub_data.chassis_power = (static_cast((data[0] << 8) | data[1]) / 100.); - sample_and_status_pub_data.chassis_expect_power = (static_cast((data[2] << 8) | data[3]) / 100.); - sample_and_status_pub_data.capacity_recent_charge_power = + sample_and_status_pub_data.chassis_error_code = (static_cast((data[2] << 8) | data[3]) / 100.); + sample_and_status_pub_data.capacity_receive_message = (static_cast((data[4] << 8) | data[5]) / 100.); sample_and_status_pub_data.capacity_remain_charge = (static_cast((data[6] << 8) | data[7]) / 10000.); From c51cdef6dbd522d652c17d85cca6c28772232bf4 Mon Sep 17 00:00:00 2001 From: CH <148587897+2194555@users.noreply.github.com> Date: Sat, 6 Jun 2026 16:35:26 +0800 Subject: [PATCH 32/32] Fix format. --- rm_referee/include/rm_referee/ui/trigger_change_ui.h | 1 + 1 file changed, 1 insertion(+) 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 085d24d4..2efe329d 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -123,6 +123,7 @@ class ZipTriggerChangeUi : public TriggerChangeUi uint8_t zip_mode_{}; int outline_width_{}; int fill_width_{}; + class HeroLegTriggerChangeUi : public TriggerChangeUi { public: