From 4e6b5152e3fe2ca7c7a92d87e6d904749060d4b8 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Mon, 18 Dec 2023 16:41:37 +0800 Subject: [PATCH 01/15] Fixed some bug. --- rm_referee/src/referee_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index a47f2ae5..759c5368 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -35,7 +35,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::radar_receive_sub_ = nh.subscribe("/rm_radar", 10, &RefereeBase::radarReceiveCallback, this); RefereeBase::sentry_deviate_sub_ = - nh.subscribe("/odometry", 10, &RefereeBase::sentryDeviateCallback, this); + nh.subscribe("/deviate", 10, &RefereeBase::sentryDeviateCallback, this); RefereeBase::radar_to_sentry_sub_ = nh.subscribe( "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this); From 9e12642b782c965fe7e69668a7f09446744ddc04 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 1 Feb 2024 16:11:37 +0800 Subject: [PATCH 02/15] Add srv for sentry. --- rm_msgs/CMakeLists.txt | 1 + rm_msgs/srv/CloseToTarget.srv | 3 +++ 2 files changed, 4 insertions(+) create mode 100644 rm_msgs/srv/CloseToTarget.srv diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index a64f7f7e..6a2a4a6f 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ add_service_files( CameraStatus.srv EnableImuTrigger.srv EnableGyro.srv + CloseToTarget.srv ) add_message_files( diff --git a/rm_msgs/srv/CloseToTarget.srv b/rm_msgs/srv/CloseToTarget.srv new file mode 100644 index 00000000..64e0e2fb --- /dev/null +++ b/rm_msgs/srv/CloseToTarget.srv @@ -0,0 +1,3 @@ +geometry_msgs/PointStamped target_pos +--- +geometry_msgs/PoseStamped move_point \ No newline at end of file From d002ead68745d50d1fe5a65e6403047415bb6a9b Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 25 Feb 2024 21:57:56 +0800 Subject: [PATCH 03/15] Update protocol. --- rm_msgs/CMakeLists.txt | 1 + rm_msgs/msg/referee/ClientMapSendData.msg | 1 + rm_msgs/msg/referee/DartClientCmd.msg | 1 - rm_msgs/msg/referee/DartRemainingTime.msg | 1 + rm_msgs/msg/referee/GameRobotPosData.msg | 3 ++ rm_msgs/msg/referee/GameRobotStatus.msg | 13 ++--- .../msg/referee/SupplyProjectileAction.msg | 2 +- rm_referee/include/rm_referee/common/data.h | 1 + .../include/rm_referee/common/protocol.h | 48 +++++++++++------ rm_referee/include/rm_referee/referee.h | 2 + rm_referee/src/referee.cpp | 51 ++++++++++++------- rm_referee/src/ui/ui_base.cpp | 1 + 12 files changed, 79 insertions(+), 46 deletions(-) create mode 100644 rm_msgs/msg/referee/GameRobotPosData.msg diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 6a2a4a6f..2498337f 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -77,6 +77,7 @@ add_message_files( ClientMapSendData.msg ClientMapReceiveData.msg MapSentryData.msg + GameRobotPosData.msg PowerManagementSampleAndStatusData.msg PowerManagementInitializationExceptionData.msg PowerManagementProcessStackOverflowData.msg diff --git a/rm_msgs/msg/referee/ClientMapSendData.msg b/rm_msgs/msg/referee/ClientMapSendData.msg index 0963377f..628adb38 100644 --- a/rm_msgs/msg/referee/ClientMapSendData.msg +++ b/rm_msgs/msg/referee/ClientMapSendData.msg @@ -11,5 +11,6 @@ float32 target_position_y float32 target_position_z uint8 command_keyboard uint16 target_robot_ID +uint8 cmd_source time stamp diff --git a/rm_msgs/msg/referee/DartClientCmd.msg b/rm_msgs/msg/referee/DartClientCmd.msg index c37bef17..1c5ab248 100644 --- a/rm_msgs/msg/referee/DartClientCmd.msg +++ b/rm_msgs/msg/referee/DartClientCmd.msg @@ -1,5 +1,4 @@ uint8 dart_launch_opening_status -uint8 dart_attack_target uint16 target_change_time uint8 first_dart_speed uint8 second_dart_speed diff --git a/rm_msgs/msg/referee/DartRemainingTime.msg b/rm_msgs/msg/referee/DartRemainingTime.msg index 9c39e652..77fe568f 100644 --- a/rm_msgs/msg/referee/DartRemainingTime.msg +++ b/rm_msgs/msg/referee/DartRemainingTime.msg @@ -1,3 +1,4 @@ uint8 dart_remaining_time +uint8 dart_aim_state time stamp diff --git a/rm_msgs/msg/referee/GameRobotPosData.msg b/rm_msgs/msg/referee/GameRobotPosData.msg new file mode 100644 index 00000000..bfa5c4c9 --- /dev/null +++ b/rm_msgs/msg/referee/GameRobotPosData.msg @@ -0,0 +1,3 @@ +float32 x +float32 y +float32 yaw \ No newline at end of file diff --git a/rm_msgs/msg/referee/GameRobotStatus.msg b/rm_msgs/msg/referee/GameRobotStatus.msg index f7c2be7a..d17b0106 100644 --- a/rm_msgs/msg/referee/GameRobotStatus.msg +++ b/rm_msgs/msg/referee/GameRobotStatus.msg @@ -22,18 +22,11 @@ uint8 BLUE_OUTPUT = 110 uint8 BLUE_BASE = 111 uint8 robot_id -uint8 robot_level +uint8 robot_exp uint16 remain_hp uint16 max_hp -uint16 shooter_id_1_17_mm_cooling_rate -uint16 shooter_id_1_17_mm_cooling_limit -uint16 shooter_id_1_17_mm_speed_limit -uint16 shooter_id_2_17_mm_cooling_rate -uint16 shooter_id_2_17_mm_cooling_limit -uint16 shooter_id_2_17_mm_speed_limit -uint16 shooter_id_1_42_mm_cooling_rate -uint16 shooter_id_1_42_mm_cooling_limit -uint16 shooter_id_1_42_mm_speed_limit +uint16 shooter_cooling_rate +uint16 shooter_cooling_limit uint16 chassis_power_limit uint8 mains_power_gimbal_output uint8 mains_power_chassis_output diff --git a/rm_msgs/msg/referee/SupplyProjectileAction.msg b/rm_msgs/msg/referee/SupplyProjectileAction.msg index 0b03619b..27cdb2ad 100644 --- a/rm_msgs/msg/referee/SupplyProjectileAction.msg +++ b/rm_msgs/msg/referee/SupplyProjectileAction.msg @@ -1,4 +1,4 @@ -uint8 supply_projectile_id +uint8 reserved uint8 supply_robot_id uint8 supply_projectile_step uint8 supply_projectile_num diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index daff6734..627ae1d6 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -82,6 +82,7 @@ #include #include #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 f309e110..042767cf 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -65,6 +65,8 @@ typedef enum DART_CLIENT_CMD = 0x020A, ROBOTS_POS_CMD = 0X020B, RADAR_MARK_CMD = 0X020C, + SENTRY_INFO_CMD = 0x020D, + RADAR_INFO_CMD = 0x020E, INTERACTIVE_DATA_CMD = 0x0301, CUSTOM_CONTROLLER_CMD = 0x0302, // controller TARGET_POS_CMD = 0x0303, // send aerial->server @@ -72,11 +74,12 @@ typedef enum CLIENT_MAP_CMD = 0x0305, CUSTOM_CLIENT_CMD = 0x0306, // controller MAP_SENTRY_CMD = 0x0307, // send sentry->aerial + CUSTOM_INFO_CMD = 0x0308, POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD = 0X8301, POWER_MANAGEMENT_INITIALIZATION_EXCEPTION_CMD = 0X8302, POWER_MANAGEMENT_SYSTEM_EXCEPTION_CMD = 0X8303, POWER_MANAGEMENT_PROCESS_STACK_OVERFLOW_CMD = 0X8304, - POWER_MANAGEMENT_UNKNOWN_EXCEPTION_CMD = 0X8305, + POWER_MANAGEMENT_UNKNOWN_EXCEPTION_CMD = 0X8305 } RefereeCmdId; typedef enum @@ -89,6 +92,8 @@ typedef enum CLIENT_GRAPH_FIVE_CMD = 0x0103, CLIENT_GRAPH_SEVEN_CMD = 0x0104, CLIENT_CHARACTER_CMD = 0x0110, + SENTRY_CMD = 0x0120, + RADAR_CMD = 0x0121, CURRENT_SENTRY_POSITION_CMD = 0x0200 // send radar->sentry } DataCmdId; @@ -129,7 +134,7 @@ typedef enum BLUE_STANDARD_3_CLIENT = 0x0167, BLUE_STANDARD_4_CLIENT = 0x0168, BLUE_STANDARD_5_CLIENT = 0x0169, - BLUE_AERIAL_CLIENT = 0x016A, + BLUE_AERIAL_CLIENT = 0x016A } ClientId; typedef enum @@ -279,7 +284,7 @@ typedef struct typedef struct { - uint8_t supply_projectile_id; + uint8_t reserved; uint8_t supply_robot_id; uint8_t supply_projectile_step; uint8_t supply_projectile_num; @@ -289,28 +294,23 @@ typedef struct { uint8_t level; uint8_t foul_robot_id; + uint8_t count; } __packed RefereeWarning; typedef struct { uint8_t dart_remaining_time; + uint8_t dart_aim_state; } __packed DartRemainingTime; typedef struct { uint8_t robot_id; - uint8_t robot_level; + uint8_t robot_exp; uint16_t remain_hp; uint16_t max_hp; - uint16_t shooter_id_1_17_mm_cooling_rate; - uint16_t shooter_id_1_17_mm_cooling_limit; - uint16_t shooter_id_1_17_mm_speed_limit; - uint16_t shooter_id_2_17_mm_cooling_rate; - uint16_t shooter_id_2_17_mm_cooling_limit; - uint16_t shooter_id_2_17_mm_speed_limit; - uint16_t shooter_id_1_42_mm_cooling_rate; - uint16_t shooter_id_1_42_mm_cooling_limit; - uint16_t shooter_id_1_42_mm_speed_limit; + uint16_t shooter_cooling_rate; + uint16_t shooter_cooling_limit; uint16_t chassis_power_limit; uint8_t mains_power_gimbal_output : 1; uint8_t mains_power_chassis_output : 1; @@ -332,7 +332,6 @@ typedef struct { float x; float y; - float z; float yaw; } __packed GameRobotPos; @@ -375,7 +374,7 @@ typedef struct typedef struct { uint8_t dart_launch_opening_status; - uint8_t dart_attack_target; + // uint8_t dart_attack_target; uint16_t target_change_time; uint8_t first_dart_speed; uint8_t second_dart_speed; @@ -385,6 +384,16 @@ typedef struct uint16_t operate_launch_cmd_time; } __packed DartClientCmd; +typedef struct +{ + uint32_t sentry_info; +} __packed SentryInfo; + +typedef struct +{ + uint8_t radar_info; +} __packed RadarInfo; + /*********************** Interactive data between robots----0x0301 ********************/ typedef struct { @@ -512,6 +521,7 @@ typedef struct float target_position_z; uint8_t command_keyboard; uint16_t target_robot_ID; + uint8_t cmd_source; } __packed ClientMapSendData; typedef struct @@ -539,6 +549,7 @@ typedef struct uint16_t start_position_y; int8_t delta_x[49]; int8_t delta_y[49]; + uint16_t sender_id; } __packed MapSentryData; typedef struct @@ -550,6 +561,13 @@ typedef struct float position_yaw; } __packed CurrentSentryPosData; +typedef struct +{ + uint16_t sender_id; + uint16_t receiver_id; + uint16_t user_data[30]; +} __packed CustomInfo; + typedef struct { uint8_t chassis_power_high_8_bit; diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index 23f8fab4..b8963af2 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -71,6 +71,7 @@ class Referee robots_position_pub_ = nh.advertise("robot_position", 1); radar_mark_pub_ = nh.advertise("radar_mark", 1); client_map_send_data_pub_ = nh.advertise("client_map_send_data", 1); + game_robot_pos_pub_ = nh.advertise("game_robot_pos", 1); ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management"); power_management_sample_and_status_data_pub_ = @@ -112,6 +113,7 @@ class Referee ros::Publisher client_map_receive_pub_; ros::Publisher robots_position_pub_; ros::Publisher radar_mark_pub_; + ros::Publisher game_robot_pos_pub_; ros::Publisher client_map_send_data_pub_; ros::Publisher power_management_sample_and_status_data_pub_; ros::Publisher power_management_initialization_exception_pub_; diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index e99daff5..4aad8d9c 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -207,7 +207,7 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::SupplyProjectileAction supply_projectile_action_data; memcpy(&supply_projectile_action_ref, rx_data + 7, sizeof(rm_referee::SupplyProjectileAction)); - supply_projectile_action_data.supply_projectile_id = supply_projectile_action_ref.supply_projectile_id; + supply_projectile_action_data.reserved = supply_projectile_action_ref.reserved; supply_projectile_action_data.supply_projectile_num = supply_projectile_action_ref.supply_projectile_num; supply_projectile_action_data.supply_projectile_step = supply_projectile_action_ref.supply_projectile_step; supply_projectile_action_data.supply_robot_id = supply_projectile_action_ref.supply_robot_id; @@ -229,6 +229,7 @@ int Referee::unpack(uint8_t* rx_data) memcpy(&dart_remaining_time_ref, rx_data + 7, sizeof(rm_referee::DartRemainingTime)); dart_remaining_time_data.dart_remaining_time = dart_remaining_time_ref.dart_remaining_time; + dart_remaining_time_data.dart_aim_state = dart_remaining_time_ref.dart_aim_state; dart_remaining_time_data.stamp = last_get_data_time_; dart_remaining_time_pub_.publish(dart_remaining_time_data); @@ -241,29 +242,16 @@ int Referee::unpack(uint8_t* rx_data) memcpy(&game_robot_status_ref, rx_data + 7, sizeof(rm_referee::GameRobotStatus)); game_robot_status_data.remain_hp = game_robot_status_ref.remain_hp; + game_robot_status_data.robot_exp = game_robot_status_ref.robot_exp; game_robot_status_data.max_hp = game_robot_status_ref.max_hp; + game_robot_status_data.shooter_cooling_limit = game_robot_status_ref.shooter_cooling_limit; + game_robot_status_data.shooter_cooling_rate = game_robot_status_ref.shooter_cooling_rate; + game_robot_status_data.chassis_power_limit = game_robot_status_ref.chassis_power_limit; game_robot_status_data.mains_power_chassis_output = game_robot_status_ref.mains_power_chassis_output; game_robot_status_data.mains_power_gimbal_output = game_robot_status_ref.mains_power_gimbal_output; game_robot_status_data.mains_power_shooter_output = game_robot_status_ref.mains_power_shooter_output; - game_robot_status_data.chassis_power_limit = game_robot_status_ref.chassis_power_limit; - game_robot_status_data.shooter_id_1_17_mm_cooling_limit = - game_robot_status_ref.shooter_id_1_17_mm_cooling_limit; - game_robot_status_data.shooter_id_1_17_mm_cooling_rate = - game_robot_status_ref.shooter_id_1_17_mm_cooling_rate; - game_robot_status_data.shooter_id_2_17_mm_cooling_limit = - game_robot_status_ref.shooter_id_2_17_mm_cooling_limit; - game_robot_status_data.shooter_id_2_17_mm_cooling_rate = - game_robot_status_ref.shooter_id_2_17_mm_cooling_rate; - game_robot_status_data.shooter_id_1_42_mm_cooling_limit = - game_robot_status_ref.shooter_id_1_42_mm_cooling_limit; - game_robot_status_data.shooter_id_1_42_mm_cooling_rate = - game_robot_status_ref.shooter_id_1_42_mm_cooling_rate; - game_robot_status_data.shooter_id_1_17_mm_speed_limit = game_robot_status_ref.shooter_id_1_17_mm_speed_limit; - game_robot_status_data.shooter_id_2_17_mm_speed_limit = game_robot_status_ref.shooter_id_2_17_mm_speed_limit; - game_robot_status_data.shooter_id_1_42_mm_speed_limit = game_robot_status_ref.shooter_id_1_42_mm_speed_limit; game_robot_status_data.robot_id = game_robot_status_ref.robot_id; base_.robot_id_ = game_robot_status_ref.robot_id; - game_robot_status_data.robot_level = game_robot_status_ref.robot_level; game_robot_status_data.stamp = last_get_data_time_; referee_ui_.robotStatusDataCallBack(game_robot_status_data, last_get_data_time_); @@ -292,7 +280,13 @@ int Referee::unpack(uint8_t* rx_data) case rm_referee::RefereeCmdId::ROBOT_POS_CMD: { rm_referee::GameRobotPos game_robot_pos_ref; + rm_msgs::GameRobotPosData game_robot_pos_data; memcpy(&game_robot_pos_ref, rx_data + 7, sizeof(rm_referee::GameRobotPos)); + + game_robot_pos_data.x = game_robot_pos_ref.x; + game_robot_pos_data.y = game_robot_pos_ref.y; + game_robot_pos_data.yaw = game_robot_pos_ref.yaw; + game_robot_pos_pub_.publish(game_robot_pos_data); break; } case rm_referee::RefereeCmdId::BUFF_CMD: @@ -370,7 +364,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_attack_target = dart_client_cmd_ref.dart_attack_target; + // dart_client_cmd_data.dart_attack_target = dart_client_cmd_ref.dart_attack_target; dart_client_cmd_data.dart_launch_opening_status = dart_client_cmd_ref.dart_launch_opening_status; dart_client_cmd_data.first_dart_speed = dart_client_cmd_ref.first_dart_speed; dart_client_cmd_data.second_dart_speed = dart_client_cmd_ref.second_dart_speed; @@ -457,6 +451,12 @@ int Referee::unpack(uint8_t* rx_data) } break; } + case rm_referee::CUSTOM_INFO_CMD: + { + rm_referee::CustomInfo custom_info; + memcpy(&custom_info, rx_data + 7, sizeof(rm_referee::CustomInfo)); + break; + } case rm_referee::TARGET_POS_CMD: { rm_referee::ClientMapSendData client_map_send_data_ref; @@ -468,11 +468,24 @@ int Referee::unpack(uint8_t* rx_data) client_map_send_data.target_position_z = client_map_send_data_ref.target_position_z; client_map_send_data.command_keyboard = client_map_send_data_ref.command_keyboard; client_map_send_data.target_robot_ID = client_map_send_data_ref.target_robot_ID; + client_map_send_data.cmd_source = client_map_send_data_ref.cmd_source; client_map_send_data.stamp = last_get_data_time_; client_map_send_data_pub_.publish(client_map_send_data); break; } + case rm_referee::SENTRY_INFO_CMD: + { + rm_referee::SentryInfo sentry_info; + memcpy(&sentry_info, rx_data + 7, sizeof(rm_referee::SentryInfo)); + break; + } + case rm_referee::RADAR_INFO_CMD: + { + rm_referee::RadarInfo radar_info; + memcpy(&radar_info, rx_data + 7, sizeof(rm_referee::RadarInfo)); + break; + } case rm_referee::POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD: { rm_msgs::PowerManagementSampleAndStatusData sample_and_status_pub_data; diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index ec3af275..eaadd50b 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -207,6 +207,7 @@ void UiBase::sendMapSentryData(const rm_referee::MapSentryData& data) map_sentry_data->delta_x[i] = data.delta_x[i]; map_sentry_data->delta_y[i] = data.delta_y[i]; } + map_sentry_data->sender_id = base_.robot_id_; pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::MAP_SENTRY_CMD, sizeof(rm_referee::MapSentryData)); tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::MapSentryData) + k_tail_length_); From 06fc0fe0d7a9cedbc25a64be94cc7f0eb9f70b32 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 28 Feb 2024 20:10:30 +0800 Subject: [PATCH 04/15] Update heat_limit.h. --- .../include/rm_common/decision/heat_limit.h | 52 ++----------------- 1 file changed, 4 insertions(+), 48 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 17bc6ee4..4d93d889 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -64,7 +64,7 @@ class HeatLimit ROS_ERROR("Heat coeff no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("type", type_)) ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str()); - nh.param("safe_speed_limit", shooter_speed_limit_, 15); + // nh.param("safe_speed_limit", shooter_speed_limit_, 15); if (type_ == "ID1_42MM") bullet_heat_ = 100.; else @@ -81,24 +81,8 @@ class HeatLimit void setStatusOfShooter(const rm_msgs::GameRobotStatus data) { - if (type_ == "ID1_17MM") - { - shooter_cooling_limit_ = data.shooter_id_1_17_mm_cooling_limit; - shooter_cooling_rate_ = data.shooter_id_1_17_mm_cooling_rate; - shooter_speed_limit_ = data.shooter_id_1_17_mm_speed_limit; - } - else if (type_ == "ID2_17MM") - { - shooter_cooling_limit_ = data.shooter_id_2_17_mm_cooling_limit; - shooter_cooling_rate_ = data.shooter_id_2_17_mm_cooling_rate; - shooter_speed_limit_ = data.shooter_id_2_17_mm_speed_limit; - } - else if (type_ == "ID1_42MM") - { - shooter_cooling_limit_ = data.shooter_id_1_42_mm_cooling_limit; - shooter_cooling_rate_ = data.shooter_id_1_42_mm_cooling_rate; - shooter_speed_limit_ = data.shooter_id_1_42_mm_speed_limit; - } + shooter_cooling_limit_ = data.shooter_cooling_limit; + shooter_cooling_rate_ = data.shooter_cooling_rate; } void setCoolingHeatOfShooter(const rm_msgs::PowerHeatData data) @@ -145,39 +129,11 @@ class HeatLimit { updateExpectShootFrequency(); if (type_ == "ID1_17MM") - switch (shooter_speed_limit_) - { - case 15: - return rm_msgs::ShootCmd::SPEED_15M_PER_SECOND; - case 18: - return rm_msgs::ShootCmd::SPEED_18M_PER_SECOND; - case 30: return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND; - default: - return rm_msgs::ShootCmd::SPEED_15M_PER_SECOND; // Safety speed - } else if (type_ == "ID2_17MM") - switch (shooter_speed_limit_) - { - case 15: - return rm_msgs::ShootCmd::SPEED_15M_PER_SECOND; - case 18: - return rm_msgs::ShootCmd::SPEED_18M_PER_SECOND; - case 30: return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND; - default: - return rm_msgs::ShootCmd::SPEED_15M_PER_SECOND; // Safety speed - } else if (type_ == "ID1_42MM") - switch (shooter_speed_limit_) - { - case 10: - return rm_msgs::ShootCmd::SPEED_10M_PER_SECOND; - case 16: return rm_msgs::ShootCmd::SPEED_16M_PER_SECOND; - default: - return rm_msgs::ShootCmd::SPEED_10M_PER_SECOND; // Safety speed - } return -1; // TODO unsafe! } @@ -238,7 +194,7 @@ class HeatLimit high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}; bool referee_is_online_; - int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_, shooter_speed_limit_; + int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_; }; } // namespace rm_common From c1ca39e55b41a057929d5e46fcab1f5af319e9f8 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 29 Feb 2024 04:58:51 +0800 Subject: [PATCH 05/15] Update GameRobotStatus. --- rm_referee/include/rm_referee/common/protocol.h | 2 +- rm_referee/src/referee.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 042767cf..98f18fa2 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -306,7 +306,7 @@ typedef struct typedef struct { uint8_t robot_id; - uint8_t robot_exp; + uint8_t robot_level; uint16_t remain_hp; uint16_t max_hp; uint16_t shooter_cooling_rate; diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 4aad8d9c..23d3c70a 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -242,7 +242,7 @@ int Referee::unpack(uint8_t* rx_data) memcpy(&game_robot_status_ref, rx_data + 7, sizeof(rm_referee::GameRobotStatus)); game_robot_status_data.remain_hp = game_robot_status_ref.remain_hp; - game_robot_status_data.robot_exp = game_robot_status_ref.robot_exp; + game_robot_status_data.robot_level = game_robot_status_ref.robot_level; game_robot_status_data.max_hp = game_robot_status_ref.max_hp; game_robot_status_data.shooter_cooling_limit = game_robot_status_ref.shooter_cooling_limit; game_robot_status_data.shooter_cooling_rate = game_robot_status_ref.shooter_cooling_rate; From cf397ef608f2428cd954eb5b6651c5ef815db0da Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 28 Feb 2024 22:01:47 +0800 Subject: [PATCH 06/15] Update GameRobotStatus. --- rm_msgs/msg/referee/GameRobotStatus.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_msgs/msg/referee/GameRobotStatus.msg b/rm_msgs/msg/referee/GameRobotStatus.msg index d17b0106..b09a90f1 100644 --- a/rm_msgs/msg/referee/GameRobotStatus.msg +++ b/rm_msgs/msg/referee/GameRobotStatus.msg @@ -22,7 +22,7 @@ uint8 BLUE_OUTPUT = 110 uint8 BLUE_BASE = 111 uint8 robot_id -uint8 robot_exp +uint8 robot_level uint16 remain_hp uint16 max_hp uint16 shooter_cooling_rate From 6ea6accd06e12b6893004c198c724db904ad2efc Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sat, 9 Mar 2024 16:50:22 +0800 Subject: [PATCH 07/15] Update new msg and topic, optimize interactive data send. --- rm_msgs/CMakeLists.txt | 3 + rm_msgs/msg/referee/Buff.msg | 5 ++ rm_msgs/msg/referee/RadarInfo.msg | 1 + rm_msgs/msg/referee/SentryInfo.msg | 1 + rm_referee/include/rm_referee/common/data.h | 3 + rm_referee/include/rm_referee/referee.h | 6 ++ rm_referee/include/rm_referee/referee_base.h | 6 +- rm_referee/src/referee_base.cpp | 61 ++++++++------------ 8 files changed, 46 insertions(+), 40 deletions(-) create mode 100644 rm_msgs/msg/referee/Buff.msg create mode 100644 rm_msgs/msg/referee/RadarInfo.msg create mode 100644 rm_msgs/msg/referee/SentryInfo.msg diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 2498337f..fa5e22d0 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -78,6 +78,9 @@ add_message_files( ClientMapReceiveData.msg MapSentryData.msg GameRobotPosData.msg + SentryInfo.msg + RadarInfo.msg + Buff.msg PowerManagementSampleAndStatusData.msg PowerManagementInitializationExceptionData.msg PowerManagementProcessStackOverflowData.msg diff --git a/rm_msgs/msg/referee/Buff.msg b/rm_msgs/msg/referee/Buff.msg new file mode 100644 index 00000000..63e1622f --- /dev/null +++ b/rm_msgs/msg/referee/Buff.msg @@ -0,0 +1,5 @@ +uint8 recovery_buff +uint8 cooling_buff +uint8 defence_buff +uint8 vulnerability_buff +uint16 attack_buff \ No newline at end of file diff --git a/rm_msgs/msg/referee/RadarInfo.msg b/rm_msgs/msg/referee/RadarInfo.msg new file mode 100644 index 00000000..6d39c725 --- /dev/null +++ b/rm_msgs/msg/referee/RadarInfo.msg @@ -0,0 +1 @@ +uint8 radar_info \ No newline at end of file diff --git a/rm_msgs/msg/referee/SentryInfo.msg b/rm_msgs/msg/referee/SentryInfo.msg new file mode 100644 index 00000000..3fd479e5 --- /dev/null +++ b/rm_msgs/msg/referee/SentryInfo.msg @@ -0,0 +1 @@ +uint32 sentry_info \ No newline at end of file diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index 627ae1d6..ed2b2f4d 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -83,6 +83,9 @@ #include #include #include +#include "rm_msgs/SentryInfo.h" +#include "rm_msgs/RadarInfo.h" +#include "rm_msgs/Buff.h" #include #include #include diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index b8963af2..1895bf04 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -56,6 +56,7 @@ class Referee power_heat_data_pub_ = nh.advertise("power_heat_data", 1); game_robot_hp_pub_ = nh.advertise("game_robot_hp", 1); current_sentry_pos_pub_ = nh.advertise("current_sentry_pos", 1); + buff_pub_ = nh.advertise("robot_buff", 1); event_data_pub_ = nh.advertise("event_data", 1); dart_status_pub_ = nh.advertise("dart_status_data", 1); icra_buff_debuff_zone_status_pub_ = @@ -72,6 +73,8 @@ class Referee radar_mark_pub_ = nh.advertise("radar_mark", 1); client_map_send_data_pub_ = nh.advertise("client_map_send_data", 1); game_robot_pos_pub_ = nh.advertise("game_robot_pos", 1); + sentry_info_pub_ = nh.advertise("sentry_info", 1); + radar_info_pub_ = nh.advertise("radar_info", 1); ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management"); power_management_sample_and_status_data_pub_ = @@ -102,6 +105,7 @@ class Referee ros::Publisher current_sentry_pos_pub_; ros::Publisher event_data_pub_; ros::Publisher dart_status_pub_; + ros::Publisher buff_pub_; ros::Publisher icra_buff_debuff_zone_status_pub_; ros::Publisher supply_projectile_action_pub_; ros::Publisher dart_remaining_time_pub_; @@ -114,6 +118,8 @@ class Referee ros::Publisher robots_position_pub_; ros::Publisher radar_mark_pub_; ros::Publisher game_robot_pos_pub_; + ros::Publisher sentry_info_pub_; + ros::Publisher radar_info_pub_; ros::Publisher client_map_send_data_pub_; ros::Publisher power_management_sample_and_status_data_pub_; ros::Publisher power_management_initialization_exception_pub_; diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index b6799a69..80e77afb 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -105,10 +105,8 @@ class RefereeBase std::deque graph_queue_; std::deque character_queue_; - rm_referee::ClientMapReceiveData radar_receive_data_; - rm_referee::MapSentryData map_sentry_data_; - ros::Time interactive_data_last_send_; - bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; + ros::Time radar_interactive_data_last_send_; + ros::Time sentry_interactive_data_last_send_; UiBase* interactive_data_sender_{}; Base& base_; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 759c5368..61c4dbc8 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -194,31 +194,7 @@ void RefereeBase::sendSerialDataCallback() while (character_queue_.size() > 8) character_queue_.pop_front(); } - - if (send_radar_receive_data_) - { - if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) - return; - else - { - interactive_data_sender_->sendRadarInteractiveData(radar_receive_data_); - interactive_data_last_send_ = ros::Time::now(); - send_radar_receive_data_ = false; - } - } - else if (send_map_sentry_data_) - { - if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) - return; - else - { - interactive_data_sender_->sendMapSentryData(map_sentry_data_); - interactive_data_last_send_ = ros::Time::now(); - send_map_sentry_data_ = false; - } - } - else - sendQueue(); + sendQueue(); } else sendQueue(); @@ -401,23 +377,36 @@ void RefereeBase::sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& da void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { - radar_receive_data_.target_position_x = data->target_position_x; - radar_receive_data_.target_position_y = data->target_position_y; - radar_receive_data_.target_robot_ID = data->target_robot_ID; - - send_radar_receive_data_ = true; + rm_referee::ClientMapReceiveData radar_receive_data; + radar_receive_data.target_position_x = data->target_position_x; + radar_receive_data.target_position_y = data->target_position_y; + radar_receive_data.target_robot_ID = data->target_robot_ID; + if (ros::Time::now() - radar_interactive_data_last_send_ <= ros::Duration(0.1)) + return; + else + { + interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); + radar_interactive_data_last_send_ = ros::Time::now(); + } } void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data) { - map_sentry_data_.intention = data->intention; - map_sentry_data_.start_position_x = data->start_position_x; - map_sentry_data_.start_position_y = data->start_position_y; + rm_referee::MapSentryData map_sentry_data; + map_sentry_data.intention = data->intention; + map_sentry_data.start_position_x = data->start_position_x; + map_sentry_data.start_position_y = data->start_position_y; for (int i = 0; i < 49; i++) { - map_sentry_data_.delta_x[i] = data->delta_x[i]; - map_sentry_data_.delta_y[i] = data->delta_y[i]; + map_sentry_data.delta_x[i] = data->delta_x[i]; + map_sentry_data.delta_y[i] = data->delta_y[i]; + } + if (ros::Time::now() - sentry_interactive_data_last_send_ <= ros::Duration(1.0)) + return; + else + { + interactive_data_sender_->sendMapSentryData(map_sentry_data); + sentry_interactive_data_last_send_ = ros::Time::now(); } - send_map_sentry_data_ = true; } void RefereeBase::sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data) From 2f6ae067f6805fe9949def224ae62ee2c0898123 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 17 Mar 2024 18:50:21 +0800 Subject: [PATCH 08/15] Set vision service for twin camera. --- rm_common/include/rm_common/decision/service_caller.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rm_common/include/rm_common/decision/service_caller.h b/rm_common/include/rm_common/decision/service_caller.h index 16bb1a8a..457a2089 100644 --- a/rm_common/include/rm_common/decision/service_caller.h +++ b/rm_common/include/rm_common/decision/service_caller.h @@ -192,6 +192,16 @@ class SwitchDetectionCaller : public ServiceCallerBase service_.request.armor_target = rm_msgs::StatusChangeRequest::ARMOR_ALL; callService(); } + + explicit SwitchDetectionCaller(ros::NodeHandle& nh, std::string service_name) + : ServiceCallerBase(nh, service_name) + { + service_.request.target = rm_msgs::StatusChangeRequest::ARMOR; + service_.request.exposure = rm_msgs::StatusChangeRequest::EXPOSURE_LEVEL_0; + service_.request.armor_target = rm_msgs::StatusChangeRequest::ARMOR_ALL; + callService(); + } + void setEnemyColor(const int& robot_id_, const std::string& robot_color_) { if (robot_id_ != 0) @@ -261,4 +271,5 @@ class SwitchDetectionCaller : public ServiceCallerBase private: bool is_set_{}; }; + } // namespace rm_common From e5ca6da75ddb025088576d6c7a5e56bfd1bbaf09 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Fri, 22 Mar 2024 21:44:03 +0800 Subject: [PATCH 09/15] Add bullet remain check and enemy hero rise warning. --- .../include/rm_referee/common/protocol.h | 16 +++--- rm_referee/include/rm_referee/referee_base.h | 7 +++ rm_referee/include/rm_referee/ui/flash_ui.h | 18 +++++++ rm_referee/include/rm_referee/ui/graph.h | 4 ++ .../include/rm_referee/ui/time_change_ui.h | 13 +++++ rm_referee/include/rm_referee/ui/ui_base.h | 11 ++++- rm_referee/src/referee.cpp | 3 ++ rm_referee/src/referee_base.cpp | 49 +++++++++++++++++-- rm_referee/src/ui/flash_ui.cpp | 35 +++++++++++++ rm_referee/src/ui/graph.cpp | 9 +++- rm_referee/src/ui/time_change_ui.cpp | 35 +++++++++++++ rm_referee/src/ui/ui_base.cpp | 33 ++++++++++++- 12 files changed, 218 insertions(+), 15 deletions(-) diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 98f18fa2..2743b6a7 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -159,12 +159,14 @@ typedef enum typedef enum { - LINE = 0, - RECTANGLE = 1, - CIRCLE = 2, - ELLIPSE = 3, - ARC = 4, - STRING = 7 + LINE, + RECTANGLE, + CIRCLE, + ELLIPSE, + ARC, + FLOAT_NUM, + INT_NUM, + STRING } GraphType; typedef enum @@ -565,7 +567,7 @@ typedef struct { uint16_t sender_id; uint16_t receiver_id; - uint16_t user_data[30]; + uint8_t user_data[30]; } __packed CustomInfo; typedef struct diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 80e77afb..d5da30c2 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -24,11 +24,16 @@ class RefereeBase // unpack call back virtual void robotStatusDataCallBack(const rm_msgs::GameRobotStatus& game_robot_status_data, const ros::Time& last_get_data_time); + virtual void updateEnemyHeroState(const rm_msgs::GameRobotHp& game_robot_hp_data, const ros::Time& last_get_data_time); virtual void gameStatusDataCallBack(const rm_msgs::GameStatus& game_status_data, const ros::Time& last_get_data_time); virtual void capacityDataCallBack(const rm_msgs::PowerManagementSampleAndStatusData& data, ros::Time& last_get_data_time); virtual void powerHeatDataCallBack(const rm_msgs::PowerHeatData& power_heat_data, const ros::Time& last_get_data_time); virtual void robotHurtDataCallBack(const rm_msgs::RobotHurt& robot_hurt_data, const ros::Time& last_get_data_time); + virtual void bulletRemainDataCallBack(const rm_msgs::BulletAllowance& bullet_allowance, + const ros::Time& last_get_data_time); + virtual void updateHeroStateDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data, + const ros::Time& last_get_data_time); virtual void interactiveDataCallBack(const rm_referee::InteractiveData& interactive_data, const ros::Time& last_get_data_time); virtual void eventDataCallBack(const rm_msgs::EventData& event_data, const ros::Time& last_get_data_time); @@ -84,6 +89,7 @@ class RefereeBase TargetTriggerChangeUi* target_trigger_change_ui_{}; TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; + RemainBulletTimeChangeUi* remain_bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; EffortTimeChangeUi* effort_time_change_ui_{}; @@ -100,6 +106,7 @@ class RefereeBase CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; + HeroStateFlashUi* hero_state_flash_ui_{}; GroupUiBase* graph_queue_sender_{}; std::deque graph_queue_; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 7be7495e..adfee5e0 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -48,4 +48,22 @@ class SpinFlashUi : public FlashUi void display(const ros::Time& time) override; uint8_t chassis_mode_; }; + +class HeroStateFlashUi : public FlashUi +{ +public: + explicit HeroStateFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashUi(rpc_value, base, "hero_state", graph_queue, character_queue) + { + ros::NodeHandle nh; + timer_ = nh.createTimer(ros::Duration(delay_), std::bind(&HeroStateFlashUi::delayDisplay, this), false, false); + }; + void updateHeroStateData(const rm_msgs::GameRobotHp& data, const ros::Time& last_get_data_time); + +private: + void delayDisplay(); + ros::Timer timer_; + bool enemy_hero_die_{ false }; +}; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index 5e93b7c2..eba8cbab 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -49,6 +49,10 @@ class Graph { config_.end_y = end_y; } + void setRadius(int radius) + { + config_.radius = radius; + } void setStartX(int start_x) { config_.start_x = start_x; 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 a249a44b..c14970ad 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -273,4 +273,17 @@ class JointPositionTimeChangeUi : public TimeChangeUi std::string name_, direction_; double max_val_, min_val_, current_val_, length_; }; + +class RemainBulletTimeChangeUi : public TimeChangeUi +{ +public: + explicit RemainBulletTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeUi(rpc_value, base, "remaining_bullet", graph_queue, character_queue){}; + void updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time); + +private: + void updateConfig() override; + int bullet_allowance_num_17_mm_, bullet_allowance_num_42_mm_; +}; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 0296faf8..20e2a634 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -45,6 +45,7 @@ class UiBase void sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data); void sendMapSentryData(const rm_referee::MapSentryData& data); void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data); + void sendCustomInfoData(std::wstring data); void sendSerial(const ros::Time& time, int data_len); void clearTxBuffer(); @@ -106,8 +107,14 @@ class FixedUi : public GroupUiBase : GroupUiBase(rpc_value, base, graph_queue, character_queue) { for (int i = 0; i < static_cast(rpc_value.size()); i++) - graph_vector_.insert( - std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); + { + if (rpc_value[i]["config"].hasMember("type") || rpc_value[i]["config"]["type"] != "string") + graph_vector_.insert( + std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); + else + character_vector_.insert( + std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); + } }; void updateForQueue() override; int update_fixed_ui_times = 0; diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 23d3c70a..191f461e 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -140,6 +140,8 @@ int Referee::unpack(uint8_t* rx_data) game_robot_hp_data.red_base_hp = game_robot_hp_ref.red_base_hp; game_robot_hp_data.stamp = last_get_data_time_; + referee_ui_.updateEnemyHeroState(game_robot_hp_data, last_get_data_time_); + referee_ui_.updateHeroStateDataCallBack(game_robot_hp_data, last_get_data_time_); game_robot_hp_pub_.publish(game_robot_hp_data); break; } @@ -342,6 +344,7 @@ int Referee::unpack(uint8_t* rx_data) 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.stamp = last_get_data_time_; + referee_ui_.bulletRemainDataCallBack(bullet_allowance_data, last_get_data_time_); bullet_allowance_pub_.publish(bullet_allowance_data); break; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 61c4dbc8..568c9efb 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -38,6 +38,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/deviate", 10, &RefereeBase::sentryDeviateCallback, this); RefereeBase::radar_to_sentry_sub_ = nh.subscribe( "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this); + //"/bullet_allowance_data" XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -96,6 +97,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "engineer_joint3") engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint3"); + if (rpc_value[i]["name"] == "remaining_bullet") + remain_bullet_time_change_ui_ = + new RemainBulletTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -108,6 +112,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"] == "hero_state") + hero_state_flash_ui_ = new HeroStateFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } @@ -134,11 +140,11 @@ void RefereeBase::addUi() ROS_INFO_THROTTLE(1.0, "Adding ui... %.1f%%", (add_ui_times_ / static_cast(add_ui_max_times_)) * 100); if (chassis_trigger_change_ui_) - chassis_trigger_change_ui_->addForQueue(3); + chassis_trigger_change_ui_->addForQueue(2); if (gimbal_trigger_change_ui_) - gimbal_trigger_change_ui_->addForQueue(3); + gimbal_trigger_change_ui_->addForQueue(2); if (shooter_trigger_change_ui_) - shooter_trigger_change_ui_->addForQueue(3); + shooter_trigger_change_ui_->addForQueue(2); if (target_trigger_change_ui_) target_trigger_change_ui_->addForQueue(); if (target_view_angle_trigger_change_ui_) @@ -169,6 +175,8 @@ void RefereeBase::addUi() engineer_joint2_time_change_ui->addForQueue(); if (engineer_joint3_time_change_ui) engineer_joint3_time_change_ui->addForQueue(); + if (remain_bullet_time_change_ui_) + remain_bullet_time_change_ui_->addForQueue(); add_ui_times_++; } @@ -247,6 +255,35 @@ void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, if (fixed_ui_ && !is_adding_) fixed_ui_->updateForQueue(); } +void RefereeBase::updateEnemyHeroState(const rm_msgs::GameRobotHp& game_robot_hp_data, + const ros::Time& last_get_data_time) +{ + std::wstring data; + if (base_.robot_id_ < 100 && base_.robot_id_ != RED_SENTRY) + { + if (game_robot_hp_data.blue_1_robot_hp > 0) + data = L"敌英雄存活:" + std::to_wstring(game_robot_hp_data.blue_1_robot_hp); + else + data = L"敌英雄死亡"; + } + else if (base_.robot_id_ >= 100 && base_.robot_id_ != BLUE_SENTRY) + { + if (game_robot_hp_data.red_1_robot_hp > 0) + data = L"敌英雄存活:" + std::to_wstring(game_robot_hp_data.red_1_robot_hp); + else + data = L"敌英雄死亡"; + } + else + return; + interactive_data_sender_->sendCustomInfoData(data); +} + +void RefereeBase::updateHeroStateDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data, + const ros::Time& last_get_data_time) +{ + if (hero_state_flash_ui_) + hero_state_flash_ui_->updateHeroStateData(game_robot_hp_data, last_get_data_time); +} void RefereeBase::gameStatusDataCallBack(const rm_msgs::GameStatus& data, const ros::Time& last_get_data_time) { } @@ -264,6 +301,12 @@ void RefereeBase::powerHeatDataCallBack(const rm_msgs::PowerHeatData& data, cons void RefereeBase::robotHurtDataCallBack(const rm_msgs::RobotHurt& data, const ros::Time& last_get_data_time) { } +void RefereeBase::bulletRemainDataCallBack(const rm_msgs::BulletAllowance& bullet_allowance, + const ros::Time& last_get_data_time) +{ + if (remain_bullet_time_change_ui_ && !is_adding_) + remain_bullet_time_change_ui_->updateBulletData(bullet_allowance, last_get_data_time); +} void RefereeBase::interactiveDataCallBack(const rm_referee::InteractiveData& data, const ros::Time& last_get_data_time) { } diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index 3b8421b8..07e22fc0 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -54,4 +54,39 @@ void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, chassis_mode_ = data->mode; display(last_get_data_time); } + +void HeroStateFlashUi::updateHeroStateData(const rm_msgs::GameRobotHp& data, const ros::Time& last_get_data_time) +{ + if (base_.robot_id_ < 100) + { + if (data.blue_1_robot_hp > 0 && enemy_hero_die_) + { + FlashUi::updateFlashUiForQueue(last_get_data_time, true, true); + timer_.start(); + } + if (data.blue_1_robot_hp == 0) + enemy_hero_die_ = false; + else + enemy_hero_die_ = true; + } + else if (base_.robot_id_ >= 100) + { + if (data.red_1_robot_hp > 0 && enemy_hero_die_) + { + FlashUi::updateFlashUiForQueue(last_get_data_time, true, true); + timer_.start(); + } + if (data.red_1_robot_hp == 0) + enemy_hero_die_ = false; + else + enemy_hero_die_ = true; + } +} +void HeroStateFlashUi::delayDisplay() +{ + graph_->setOperation(rm_referee::GraphOperation::DELETE); + FlashUi::updateFlashUiForQueue(ros::Time::now(), false, true); + timer_.stop(); +} } // namespace rm_referee +// namespace rm_referee diff --git a/rm_referee/src/ui/graph.cpp b/rm_referee/src/ui/graph.cpp index 1a6061f6..632e8835 100644 --- a/rm_referee/src/ui/graph.cpp +++ b/rm_referee/src/ui/graph.cpp @@ -15,7 +15,8 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base { config_.graphic_type = rm_referee::GraphType::STRING; } - if (config_.graphic_type == getType("string")) + if (config_.graphic_type == getType("string") || config_.graphic_type == getType("int_num") || + config_.graphic_type == getType("float_num")) { if (config.hasMember("size")) config_.start_angle = static_cast(config["size"]); @@ -127,6 +128,12 @@ rm_referee::GraphType Graph::getType(const std::string& type) return rm_referee::GraphType::ARC; else if (type == "string") return rm_referee::GraphType::STRING; + else if (type == "int_num") + return rm_referee::GraphType::INT_NUM; + else if (type == "float_num") + return rm_referee::GraphType::FLOAT_NUM; + else if (type == "line") + return rm_referee::GraphType::LINE; else return rm_referee::GraphType::LINE; } diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 30360ff7..09b100ea 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -321,4 +321,39 @@ void JointPositionTimeChangeUi::updateJointStateData(const sensor_msgs::JointSta current_val_ = data->position[i]; updateForQueue(); } + +void RemainBulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time) +{ + bullet_allowance_num_17_mm_ = data.bullet_allowance_num_17_mm; + bullet_allowance_num_42_mm_ = data.bullet_allowance_num_42_mm; + updateForQueue(); +} + +void RemainBulletTimeChangeUi::updateConfig() +{ + std::string bullet_allowance_num; + if (base_.robot_id_ == RED_HERO || base_.robot_id_ == BLUE_HERO) + { + graph_->setRadius(bullet_allowance_num_42_mm_); + if (bullet_allowance_num_42_mm_ > 5) + graph_->setColor(rm_referee::GraphColor::GREEN); + else if (bullet_allowance_num_42_mm_ < 3) + graph_->setColor(rm_referee::GraphColor::PINK); + else + graph_->setColor(rm_referee::GraphColor::YELLOW); + } + else + { + graph_->setRadius(bullet_allowance_num_17_mm_); // TODO:need use uint32, now only < 1024 + if (bullet_allowance_num_17_mm_ > 50) + graph_->setColor(rm_referee::GraphColor::GREEN); + else if (bullet_allowance_num_17_mm_ < 10) + graph_->setColor(rm_referee::GraphColor::PINK); + else + graph_->setColor(rm_referee::GraphColor::YELLOW); + } + + graph_->setColor(rm_referee::GraphColor::YELLOW); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index eaadd50b..2f4366ac 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -222,6 +222,37 @@ void UiBase::sendMapSentryData(const rm_referee::MapSentryData& data) clearTxBuffer(); } +void UiBase::sendCustomInfoData(std::wstring data) +{ + uint8_t tx_data[sizeof(rm_referee::CustomInfo)] = { 0 }; + auto custom_info = (rm_referee::CustomInfo*)tx_data; + uint16_t characters[15]; + for (int i = 0; i < 15; i++) + { + if (i < static_cast(data.size())) + characters[i] = static_cast(data[i]); + } + for (int i = 0; i < 15; i++) + { + custom_info->user_data[2 * i] = characters[i] & 0xFF; + custom_info->user_data[2 * i + 1] = (characters[i] >> 8) & 0xFF; + } + custom_info->sender_id = base_.robot_id_; + custom_info->receiver_id = base_.client_id_; + pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::CUSTOM_INFO_CMD, sizeof(rm_referee::CustomInfo)); + tx_len_ = + k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::ClientMapReceiveData) + k_tail_length_); + + try + { + base_.serial_.write(tx_buffer_, tx_len_); + } + catch (serial::PortNotOpenedException& e) + { + } + clearTxBuffer(); +} + void UiBase::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) { uint8_t tx_data[sizeof(rm_referee::ClientMapReceiveData)] = { 0 }; @@ -235,7 +266,6 @@ void UiBase::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& da pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::CLIENT_MAP_CMD, sizeof(rm_referee::ClientMapReceiveData)); tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::ClientMapReceiveData) + k_tail_length_); - try { base_.serial_.write(tx_buffer_, tx_len_); @@ -243,7 +273,6 @@ void UiBase::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& da catch (serial::PortNotOpenedException& e) { } - clearTxBuffer(); } From 91e0fcd36ecd2d1048f56d8ec1bf8a2e8a656e39 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 26 Mar 2024 15:55:13 +0800 Subject: [PATCH 10/15] Fix fixed ui and lane line bug. --- rm_referee/include/rm_referee/referee_base.h | 2 +- .../include/rm_referee/ui/time_change_ui.h | 9 ++++--- rm_referee/include/rm_referee/ui/ui_base.h | 9 ++++--- rm_referee/src/referee_base.cpp | 16 ++++++------ rm_referee/src/ui/time_change_ui.cpp | 25 +++++++++++++------ rm_referee/src/ui/ui_base.cpp | 8 ++++++ 6 files changed, 47 insertions(+), 22 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index d5da30c2..42c3264e 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -89,7 +89,7 @@ class RefereeBase TargetTriggerChangeUi* target_trigger_change_ui_{}; TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; - RemainBulletTimeChangeUi* remain_bullet_time_change_ui_{}; + BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; EffortTimeChangeUi* effort_time_change_ui_{}; 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 c14970ad..df382ddf 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -274,16 +274,17 @@ class JointPositionTimeChangeUi : public TimeChangeUi double max_val_, min_val_, current_val_, length_; }; -class RemainBulletTimeChangeUi : public TimeChangeUi +class BulletTimeChangeUi : public TimeChangeUi { public: - explicit RemainBulletTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) + explicit BulletTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "remaining_bullet", graph_queue, character_queue){}; void updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time); + void reset(); private: void updateConfig() override; - int bullet_allowance_num_17_mm_, bullet_allowance_num_42_mm_; + int bullet_allowance_num_17_mm_, bullet_allowance_num_42_mm_, bullet_num_17_mm_{ 0 }, bullet_num_42_mm_{ 0 }; }; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 20e2a634..285d75a7 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -108,11 +108,14 @@ class FixedUi : public GroupUiBase { for (int i = 0; i < static_cast(rpc_value.size()); i++) { - if (rpc_value[i]["config"].hasMember("type") || rpc_value[i]["config"]["type"] != "string") - graph_vector_.insert( + if (rpc_value[i]["config"]["type"] == "string") + { + ROS_INFO_STREAM("string FixedUi:" << rpc_value[i]["name"]); + character_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); + } else - character_vector_.insert( + graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); } }; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 568c9efb..ab2764e2 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -97,9 +97,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "engineer_joint3") engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint3"); - if (rpc_value[i]["name"] == "remaining_bullet") - remain_bullet_time_change_ui_ = - new RemainBulletTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "bullet") + bullet_time_change_ui_ = new BulletTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -175,8 +174,11 @@ void RefereeBase::addUi() engineer_joint2_time_change_ui->addForQueue(); if (engineer_joint3_time_change_ui) engineer_joint3_time_change_ui->addForQueue(); - if (remain_bullet_time_change_ui_) - remain_bullet_time_change_ui_->addForQueue(); + if (bullet_time_change_ui_) + { + bullet_time_change_ui_->reset(); + bullet_time_change_ui_->addForQueue(); + } add_ui_times_++; } @@ -304,8 +306,8 @@ void RefereeBase::robotHurtDataCallBack(const rm_msgs::RobotHurt& data, const ro void RefereeBase::bulletRemainDataCallBack(const rm_msgs::BulletAllowance& bullet_allowance, const ros::Time& last_get_data_time) { - if (remain_bullet_time_change_ui_ && !is_adding_) - remain_bullet_time_change_ui_->updateBulletData(bullet_allowance, last_get_data_time); + if (bullet_time_change_ui_ && !is_adding_) + bullet_time_change_ui_->updateBulletData(bullet_allowance, last_get_data_time); } void RefereeBase::interactiveDataCallBack(const rm_referee::InteractiveData& data, const ros::Time& last_get_data_time) { diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 09b100ea..6aa64ec5 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -214,12 +214,14 @@ void LaneLineTimeChangeGroupUi::updateConfig() void LaneLineTimeChangeGroupUi::updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time) { - if (!tf_buffer_.canTransform(reference_frame_, "odom", ros::Time(0))) + if (!tf_buffer_.canTransform(reference_frame_, "yaw", ros::Time(0))) return; try { double roll, pitch, yaw; - quatToRPY(tf_buffer_.lookupTransform(reference_frame_, "odom", ros::Time(0)).transform.rotation, roll, pitch, yaw); + quatToRPY(tf_buffer_.lookupTransform(reference_frame_, "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw); + tf2::Quaternion pitch_ori_rot; + pitch_ori_rot.setRPY(0, 0, yaw); pitch_angle_ = pitch; } catch (tf2::TransformException& ex) @@ -322,19 +324,29 @@ void JointPositionTimeChangeUi::updateJointStateData(const sensor_msgs::JointSta updateForQueue(); } -void RemainBulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time) +void BulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time) { + if (bullet_allowance_num_17_mm_ > data.bullet_allowance_num_17_mm && data.bullet_allowance_num_17_mm >= 0) + bullet_num_17_mm_ += (bullet_allowance_num_17_mm_ - data.bullet_allowance_num_17_mm); + if (bullet_allowance_num_42_mm_ > data.bullet_allowance_num_42_mm && data.bullet_allowance_num_42_mm >= 0) + bullet_num_42_mm_ += (bullet_allowance_num_42_mm_ - data.bullet_allowance_num_42_mm); bullet_allowance_num_17_mm_ = data.bullet_allowance_num_17_mm; bullet_allowance_num_42_mm_ = data.bullet_allowance_num_42_mm; updateForQueue(); } -void RemainBulletTimeChangeUi::updateConfig() +void BulletTimeChangeUi::reset() +{ + bullet_num_17_mm_ = 0; + bullet_num_42_mm_ = 0; +} + +void BulletTimeChangeUi::updateConfig() { std::string bullet_allowance_num; if (base_.robot_id_ == RED_HERO || base_.robot_id_ == BLUE_HERO) { - graph_->setRadius(bullet_allowance_num_42_mm_); + graph_->setRadius(bullet_num_42_mm_); if (bullet_allowance_num_42_mm_ > 5) graph_->setColor(rm_referee::GraphColor::GREEN); else if (bullet_allowance_num_42_mm_ < 3) @@ -344,7 +356,7 @@ void RemainBulletTimeChangeUi::updateConfig() } else { - graph_->setRadius(bullet_allowance_num_17_mm_); // TODO:need use uint32, now only < 1024 + graph_->setRadius(bullet_num_17_mm_); // TODO:need use uint32, now only < 1024 if (bullet_allowance_num_17_mm_ > 50) graph_->setColor(rm_referee::GraphColor::GREEN); else if (bullet_allowance_num_17_mm_ < 10) @@ -352,7 +364,6 @@ void RemainBulletTimeChangeUi::updateConfig() else graph_->setColor(rm_referee::GraphColor::YELLOW); } - graph_->setColor(rm_referee::GraphColor::YELLOW); } diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 2f4366ac..9b09ba13 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -61,6 +61,12 @@ void GroupUiBase::addForQueue(int add_times) graph_queue_->push_back(*graph.second); last_send_ = ros::Time::now(); } + for (auto graph : character_vector_) + { + graph.second->setOperation(rm_referee::GraphOperation::ADD); + character_queue_->push_back(*graph.second); + last_send_ = ros::Time::now(); + } } } @@ -441,6 +447,8 @@ void FixedUi::updateForQueue() { for (auto it : graph_vector_) it.second->updateLastConfig(); + for (auto it : character_vector_) + it.second->updateLastConfig(); if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; From f6fa05bc1ca4e95a53722d27f0f4298a76361ba4 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 26 Mar 2024 16:14:59 +0800 Subject: [PATCH 11/15] Delete code. --- rm_referee/src/ui/time_change_ui.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 6aa64ec5..0ed3b74d 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -220,8 +220,6 @@ void LaneLineTimeChangeGroupUi::updateJointStateData(const sensor_msgs::JointSta { double roll, pitch, yaw; quatToRPY(tf_buffer_.lookupTransform(reference_frame_, "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw); - tf2::Quaternion pitch_ori_rot; - pitch_ori_rot.setRPY(0, 0, yaw); pitch_angle_ = pitch; } catch (tf2::TransformException& ex) From bacd13c811179d381d71146ea4da8f853823a5a9 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 26 Mar 2024 17:23:49 +0800 Subject: [PATCH 12/15] Modify format. --- rm_common/include/rm_common/decision/heat_limit.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index 4d93d889..72b45a10 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -129,11 +129,11 @@ class HeatLimit { updateExpectShootFrequency(); if (type_ == "ID1_17MM") - return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND; + return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND; else if (type_ == "ID2_17MM") - return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND; + return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND; else if (type_ == "ID1_42MM") - return rm_msgs::ShootCmd::SPEED_16M_PER_SECOND; + return rm_msgs::ShootCmd::SPEED_16M_PER_SECOND; return -1; // TODO unsafe! } From d034fed0abd6232b726853e541d6a84bc2d82a71 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 26 Mar 2024 17:40:33 +0800 Subject: [PATCH 13/15] Modify format. --- rm_msgs/srv/CloseToTarget.srv | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 rm_msgs/srv/CloseToTarget.srv diff --git a/rm_msgs/srv/CloseToTarget.srv b/rm_msgs/srv/CloseToTarget.srv deleted file mode 100644 index 64e0e2fb..00000000 --- a/rm_msgs/srv/CloseToTarget.srv +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/PointStamped target_pos ---- -geometry_msgs/PoseStamped move_point \ No newline at end of file From 444828e420aa7517513826a631feb59009b43311 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 26 Mar 2024 17:43:24 +0800 Subject: [PATCH 14/15] Modify format. --- rm_msgs/msg/referee/Buff.msg | 2 +- rm_msgs/msg/referee/GameRobotPosData.msg | 2 +- rm_msgs/msg/referee/RadarInfo.msg | 2 +- rm_msgs/msg/referee/SentryInfo.msg | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_msgs/msg/referee/Buff.msg b/rm_msgs/msg/referee/Buff.msg index 63e1622f..7bb1e31b 100644 --- a/rm_msgs/msg/referee/Buff.msg +++ b/rm_msgs/msg/referee/Buff.msg @@ -2,4 +2,4 @@ uint8 recovery_buff uint8 cooling_buff uint8 defence_buff uint8 vulnerability_buff -uint16 attack_buff \ No newline at end of file +uint16 attack_buff diff --git a/rm_msgs/msg/referee/GameRobotPosData.msg b/rm_msgs/msg/referee/GameRobotPosData.msg index bfa5c4c9..180007e1 100644 --- a/rm_msgs/msg/referee/GameRobotPosData.msg +++ b/rm_msgs/msg/referee/GameRobotPosData.msg @@ -1,3 +1,3 @@ float32 x float32 y -float32 yaw \ No newline at end of file +float32 yaw diff --git a/rm_msgs/msg/referee/RadarInfo.msg b/rm_msgs/msg/referee/RadarInfo.msg index 6d39c725..ee153411 100644 --- a/rm_msgs/msg/referee/RadarInfo.msg +++ b/rm_msgs/msg/referee/RadarInfo.msg @@ -1 +1 @@ -uint8 radar_info \ No newline at end of file +uint8 radar_info diff --git a/rm_msgs/msg/referee/SentryInfo.msg b/rm_msgs/msg/referee/SentryInfo.msg index 3fd479e5..5295edf2 100644 --- a/rm_msgs/msg/referee/SentryInfo.msg +++ b/rm_msgs/msg/referee/SentryInfo.msg @@ -1 +1 @@ -uint32 sentry_info \ No newline at end of file +uint32 sentry_info From ddb6f750a6d3e8c9c9bd2244c8c098ba92dc0551 Mon Sep 17 00:00:00 2001 From: FurudoErika <115915981+GuraMumei@users.noreply.github.com> Date: Tue, 26 Mar 2024 18:02:47 +0800 Subject: [PATCH 15/15] Update CMakeLists.txt --- rm_msgs/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index fa5e22d0..a760773d 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -38,7 +38,6 @@ add_service_files( CameraStatus.srv EnableImuTrigger.srv EnableGyro.srv - CloseToTarget.srv ) add_message_files(