Merge branch 'rm-controls:master' into new_dbus

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
-      }
+      return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND;
     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
-      }
+      return rm_msgs::ShootCmd::SPEED_30M_PER_SECOND;
     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 rm_msgs::ShootCmd::SPEED_16M_PER_SECOND;
     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

rm_common/include/rm_common/decision/service_caller.h

@@ -192,6 +192,16 @@ class SwitchDetectionCaller : public ServiceCallerBase<rm_msgs::StatusChange>
     service_.request.armor_target = rm_msgs::StatusChangeRequest::ARMOR_ALL;
     callService();
   }
+
+  explicit SwitchDetectionCaller(ros::NodeHandle& nh, std::string service_name)
+    : ServiceCallerBase<rm_msgs::StatusChange>(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<rm_msgs::StatusChange>
 private:
   bool is_set_{};
 };
+
 }  // namespace rm_common

rm_msgs/CMakeLists.txt

@@ -76,6 +76,10 @@ add_message_files(
         ClientMapSendData.msg
         ClientMapReceiveData.msg
         MapSentryData.msg
+        GameRobotPosData.msg
+        SentryInfo.msg
+        RadarInfo.msg
+        Buff.msg
         PowerManagementSampleAndStatusData.msg
         PowerManagementInitializationExceptionData.msg
         PowerManagementProcessStackOverflowData.msg

rm_msgs/msg/detection/TrackData.msg

@@ -3,6 +3,7 @@ uint8 id
 bool tracking
 uint8 armors_num
 geometry_msgs/Point position
+geometry_msgs/Quaternion rotation
 float64 yaw
 geometry_msgs/Vector3 velocity
 float64 v_yaw

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

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

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

rm_msgs/msg/referee/DartRemainingTime.msg

@@ -1,3 +1,4 @@
 uint8 dart_remaining_time
+uint8 dart_aim_state
 
 time stamp

rm_msgs/msg/referee/GameRobotPosData.msg

@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 yaw

rm_msgs/msg/referee/GameRobotStatus.msg

@@ -25,15 +25,8 @@ uint8 robot_id
 uint8 robot_level
 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

rm_msgs/msg/referee/RadarInfo.msg

@@ -0,0 +1 @@
+uint8 radar_info

rm_msgs/msg/referee/SentryInfo.msg

@@ -0,0 +1 @@
+uint32 sentry_info

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

rm_referee/include/rm_referee/common/data.h

@@ -82,6 +82,10 @@
 #include <rm_msgs/IcraBuffDebuffZoneStatus.h>
 #include <rm_msgs/SentryDeviate.h>
 #include <rm_msgs/CurrentSentryPosData.h>
+#include <rm_msgs/GameRobotPosData.h>
+#include "rm_msgs/SentryInfo.h"
+#include "rm_msgs/RadarInfo.h"
+#include "rm_msgs/Buff.h"
 #include <rm_msgs/PowerManagementSampleAndStatusData.h>
 #include <rm_msgs/PowerManagementSystemExceptionData.h>
 #include <rm_msgs/PowerManagementInitializationExceptionData.h>

rm_referee/include/rm_referee/common/protocol.h

@@ -65,18 +65,21 @@ 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
   ROBOT_COMMAND_CMD = 0x0304,      // controller
   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
@@ -154,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
@@ -279,7 +286,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,11 +296,13 @@ 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
@@ -302,15 +311,8 @@ typedef struct
   uint8_t robot_level;
   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 +334,6 @@ typedef struct
 {
   float x;
   float y;
-  float z;
   float yaw;
 } __packed GameRobotPos;
 
@@ -375,7 +376,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 +386,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 +523,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 +551,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 +563,13 @@ typedef struct
   float position_yaw;
 } __packed CurrentSentryPosData;
 
+typedef struct
+{
+  uint16_t sender_id;
+  uint16_t receiver_id;
+  uint8_t user_data[30];
+} __packed CustomInfo;
+
 typedef struct
 {
   uint8_t chassis_power_high_8_bit;

rm_referee/include/rm_referee/referee.h

@@ -56,6 +56,7 @@ class Referee
     power_heat_data_pub_ = nh.advertise<rm_msgs::PowerHeatData>("power_heat_data", 1);
     game_robot_hp_pub_ = nh.advertise<rm_msgs::GameRobotHp>("game_robot_hp", 1);
     current_sentry_pos_pub_ = nh.advertise<rm_msgs::CurrentSentryPosData>("current_sentry_pos", 1);
+    buff_pub_ = nh.advertise<rm_msgs::Buff>("robot_buff", 1);
     event_data_pub_ = nh.advertise<rm_msgs::EventData>("event_data", 1);
     dart_status_pub_ = nh.advertise<rm_msgs::DartStatus>("dart_status_data", 1);
     icra_buff_debuff_zone_status_pub_ =
@@ -71,6 +72,9 @@ class Referee
     robots_position_pub_ = nh.advertise<rm_msgs::RobotsPositionData>("robot_position", 1);
     radar_mark_pub_ = nh.advertise<rm_msgs::RadarMarkData>("radar_mark", 1);
     client_map_send_data_pub_ = nh.advertise<rm_msgs::ClientMapSendData>("client_map_send_data", 1);
+    game_robot_pos_pub_ = nh.advertise<rm_msgs::GameRobotPosData>("game_robot_pos", 1);
+    sentry_info_pub_ = nh.advertise<rm_msgs::SentryInfo>("sentry_info", 1);
+    radar_info_pub_ = nh.advertise<rm_msgs::RadarInfo>("radar_info", 1);
 
     ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management");
     power_management_sample_and_status_data_pub_ =
@@ -101,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_;
@@ -112,6 +117,9 @@ 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 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_;