Merge branch 'master' into follow-feedforward

rm_common/include/rm_common/decision/command_sender.h

@@ -161,7 +161,7 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
       max_angular_z_.init(xml_rpc_value);
     std::string topic;
     nh.getParam("power_limit_topic", topic);
-    target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.);
+    target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 3.);
     chassis_power_limit_subscriber_ =
         nh.subscribe<rm_msgs::ChassisCmd>(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this);
   }
@@ -186,6 +186,15 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
       vel_direction_ = 1.;
     msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_;
   };
+  void setAngularZVel(double scale, double limit)
+  {
+    if (track_data_.v_yaw > target_vel_yaw_threshold_)
+      vel_direction_ = -1.;
+    if (track_data_.v_yaw < -target_vel_yaw_threshold_)
+      vel_direction_ = 1.;
+    double angular_z = max_angular_z_.output(power_limit_) > limit ? limit : max_angular_z_.output(power_limit_);
+    msg_.angular.z = scale * angular_z * vel_direction_;
+  };
   void set2DVel(double scale_x, double scale_y, double scale_z)
   {
     setLinearXVel(scale_x);
@@ -824,6 +833,11 @@ class DoubleBarrelCommandSender
     shooter_ID1_cmd_sender_->updateSuggestFireData(data);
     shooter_ID2_cmd_sender_->updateSuggestFireData(data);
   }
+  void updateShootBeforehandCmd(const rm_msgs::ShootBeforehandCmd& data)
+  {
+    shooter_ID1_cmd_sender_->updateShootBeforehandCmd(data);
+    shooter_ID2_cmd_sender_->updateShootBeforehandCmd(data);
+  }
 
   void setMode(int mode)
   {

rm_msgs/CMakeLists.txt

@@ -68,7 +68,7 @@ add_message_files(
         SupplyProjectileAction.msg
         DartRemainingTime.msg
         RobotHurt.msg
-        CurrentSentryPosData.msg
+        SentryAttackingTarget.msg
         ShootData.msg
         BulletAllowance.msg
         RfidStatus.msg

rm_msgs/msg/referee/EngineerUi.msg

@@ -4,5 +4,5 @@ string ex_arm_state
 uint32 stone_num
 
 string step_queue_name
-bool symbol
+uint32 symbol
 time stamp

rm_msgs/msg/referee/EventData.msg

@@ -1,3 +1,15 @@
-uint32 event_data
+bool forward_supply_station_state
+bool inside_supply_station_state
+bool supplier_zone_state
+bool power_rune_activation_point_state
+bool small_power_rune_state
+bool large_power_rune_state
+uint8 ring_elevated_ground_state
+uint8 r3_state
+uint8 r4_state
+uint8 base_shield_value
+uint16 be_hit_time
+uint8 be_hit_target
+uint8 central_point_state
 
 time stamp

rm_msgs/msg/referee/RfidStatus.msg

@@ -1,3 +1,23 @@
-uint32 rfid_status
+bool base_buff_point_state
+bool own_ring_elevated_ground_state
+bool enemy_ring_elevated_ground_state
+bool own_r3_state
+bool enemy_r3_state
+bool own_r4_state
+bool enemy_r4_state
+bool power_rune_activation_point_state
+bool forward_own_launch_ramp_buff_point_state
+bool behind_own_launch_ramp_buff_point_state
+bool forward_enemy_launch_ramp_buff_point_state
+bool behind_enemy_launch_ramp_buff_point_state
+bool own_outpost_buff_point
+bool own_side_restoration_zone
+bool own_sentry_patrol_zones
+bool enemy_sentry_patrol_zones
+bool own_large_resource_island_point
+bool enemy_large_resource_island_point
+bool own_exchange_zone
+bool central_buff_point
+uint32 reverse
 
 time stamp

rm_msgs/msg/referee/SentryAttackingTarget.msg

@@ -0,0 +1,3 @@
+uint8 target_robot_ID
+float32 target_position_x
+float32 target_position_y

rm_referee/include/rm_referee/common/data.h

@@ -81,13 +81,12 @@
 #include <rm_msgs/ClientMapReceiveData.h>
 #include <rm_msgs/SupplyProjectileAction.h>
 #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/TrackData.h"
+#include "rm_msgs/SentryAttackingTarget.h"
 #include <rm_msgs/PowerManagementSampleAndStatusData.h>
 #include <rm_msgs/PowerManagementSystemExceptionData.h>
 #include <rm_msgs/PowerManagementInitializationExceptionData.h>

rm_referee/include/rm_referee/common/protocol.h

@@ -94,7 +94,9 @@ typedef enum
   CLIENT_CHARACTER_CMD = 0x0110,
   SENTRY_CMD = 0x0120,
   RADAR_CMD = 0x0121,
-  CURRENT_SENTRY_POSITION_CMD = 0x0200  // send radar->sentry
+  BULLET_NUM_SHARE_CMD = 0x0200,  // send robot->aerial
+  SENTRY_TO_RADAR_CMD = 0x0201    // send sentry->radar
+  // send radar->sentry
 } DataCmdId;
 
 typedef enum
@@ -282,7 +284,19 @@ typedef struct
 
 typedef struct
 {
-  uint32_t event_type;
+  uint8_t forward_supply_station_state : 1;
+  uint8_t inside_supply_station_state : 1;
+  uint8_t supplier_zone_state : 1;
+  uint8_t power_rune_activation_point_state : 1;
+  uint8_t small_power_rune_state : 1;
+  uint8_t large_power_rune_state : 1;
+  uint8_t ring_elevated_ground_state : 2;
+  uint8_t r3_state : 2;
+  uint8_t r4_state : 2;
+  uint8_t base_shield_value : 7;
+  uint16_t be_hit_time : 9;
+  uint8_t be_hit_target : 2;
+  uint8_t central_point_state : 2;
 } __packed EventData;
 
 typedef struct
@@ -371,7 +385,27 @@ typedef struct
 
 typedef struct
 {
-  uint32_t rfid_status;
+  uint8_t base_buff_point_state : 1;
+  uint8_t own_ring_elevated_ground_state : 1;
+  uint8_t enemy_ring_elevated_ground_state : 1;
+  uint8_t own_r3_state : 1;
+  uint8_t enemy_r3_state : 1;
+  uint8_t own_r4_state : 1;
+  uint8_t enemy_r4_state : 1;
+  uint8_t power_rune_activation_point_state : 1;
+  uint8_t forward_own_launch_ramp_buff_point_state : 1;
+  uint8_t behind_own_launch_ramp_buff_point_state : 1;
+  uint8_t forward_enemy_launch_ramp_buff_point_state : 1;
+  uint8_t behind_enemy_launch_ramp_buff_point_state : 1;
+  uint8_t own_outpost_buff_point : 1;
+  uint8_t own_side_restoration_zone : 1;
+  uint8_t own_sentry_patrol_zones : 1;
+  uint8_t enemy_sentry_patrol_zones : 1;
+  uint8_t own_large_resource_island_point : 1;
+  uint8_t enemy_large_resource_island_point : 1;
+  uint8_t own_exchange_zone : 1;
+  uint8_t central_buff_point : 1;
+  uint32_t reverse : 12;
 } __packed RfidStatus;
 
 typedef struct
@@ -535,6 +569,14 @@ typedef struct
   float target_position_y;
 } __packed ClientMapReceiveData;
 
+typedef struct
+{
+  InteractiveDataHeader header_data;
+  uint8_t target_robot_ID;
+  float target_position_x;
+  float target_position_y;
+} __packed SentryAttackingTargetData;
+
 typedef struct
 {
   int16_t mouse_x;
@@ -559,11 +601,9 @@ typedef struct
 typedef struct
 {
   InteractiveDataHeader header_data;
-  float position_x;
-  float position_y;
-  float position_z;
-  float position_yaw;
-} __packed CurrentSentryPosData;
+  uint8_t bullet_42_mm_num;
+  uint8_t bullet_17_mm_num;
+} __packed BulletNumData;
 
 typedef struct
 {

rm_referee/include/rm_referee/referee.h

@@ -55,7 +55,6 @@ class Referee
     game_status_pub_ = nh.advertise<rm_msgs::GameStatus>("game_status", 1);
     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);
@@ -75,6 +74,7 @@ class Referee
     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);
+    sentry_to_radar_pub_ = nh.advertise<rm_msgs::SentryAttackingTarget>("sentry_target_to_radar", 1);
 
     ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management");
     power_management_sample_and_status_data_pub_ =
@@ -102,7 +102,7 @@ class Referee
   ros::Publisher game_status_pub_;
   ros::Publisher power_heat_data_pub_;
   ros::Publisher game_robot_hp_pub_;
-  ros::Publisher current_sentry_pos_pub_;
+  ros::Publisher sentry_to_radar_pub_;
   ros::Publisher event_data_pub_;
   ros::Publisher dart_status_pub_;
   ros::Publisher buff_pub_;

rm_referee/include/rm_referee/referee_base.h

@@ -12,6 +12,7 @@
 #include "rm_referee/ui/trigger_change_ui.h"
 #include "rm_referee/ui/time_change_ui.h"
 #include "rm_referee/ui/flash_ui.h"
+#include "rm_referee/ui/interactive_data.h"
 
 namespace rm_referee
 {
@@ -38,6 +39,7 @@ class RefereeBase
   virtual void updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data);
   virtual void supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data);
   virtual void updateShootDataDataCallBack(const rm_msgs::ShootData& msg);
+  virtual void updateBulletRemainData(const rm_referee::BulletNumData& data);
 
   // sub call back
   virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state);
@@ -56,12 +58,12 @@ class RefereeBase
   virtual void balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data);
   virtual void radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data);
   virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data);
-  virtual void sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data);
-  virtual void sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data);
+  virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data);
   virtual void sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data);
   virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data);
   virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data);
   virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data);
+  virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data);
 
   // send  ui
   void sendSerialDataCallback();
@@ -85,18 +87,21 @@ class RefereeBase
   ros::Subscriber balance_state_sub_;
   ros::Subscriber radar_receive_sub_;
   ros::Subscriber map_sentry_sub_;
+  ros::Subscriber sentry_to_radar_sub_;
   ros::Subscriber radar_to_sentry_sub_;
   ros::Subscriber sentry_cmd_sub_;
   ros::Subscriber radar_cmd_sub_;
   ros::Subscriber sentry_state_sub_;
   ros::Subscriber drone_pose_sub_;
+  ros::Subscriber shoot_cmd_sub_;
 
   ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
   ShooterTriggerChangeUi* shooter_trigger_change_ui_{};
   GimbalTriggerChangeUi* gimbal_trigger_change_ui_{};
   TargetTriggerChangeUi* target_trigger_change_ui_{};
   TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{};
   CameraTriggerChangeUi* camera_trigger_change_ui_{};
+  FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{};
 
   BulletTimeChangeUi* bullet_time_change_ui_{};
   CapacitorTimeChangeUi* capacitor_time_change_ui_{};
@@ -110,26 +115,30 @@ class RefereeBase
   JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{},
       *engineer_joint3_time_change_ui{};
   TargetDistanceTimeChangeUi* target_distance_time_change_ui_{};
+  FriendBulletsTimeChangeGroupUi* friend_bullets_time_change_group_ui_{};
 
   DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{};
   StringTriggerChangeUi *servo_mode_trigger_change_ui_{}, *stone_num_trigger_change_ui_{},
       *joint_temperature_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{};
 
-
   FixedUi* fixed_ui_{};
 
   CoverFlashUi* cover_flash_ui_{};
   SpinFlashUi* spin_flash_ui_{};
   HeroHitFlashUi* hero_hit_flash_ui_{};
   ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{};
+  EngineerActionFlashUi* engineer_action_flash_ui_{};
 
   InteractiveSender* interactive_data_sender_{};
-  InteractiveSender* enemy_hero_state_sender_{};
-  InteractiveSender* sentry_state_sender_{};
+  CustomInfoSender* enemy_hero_state_sender_{};
+  CustomInfoSender* sentry_state_sender_{};
+  BulletNumShare* bullet_num_share_{};
+  SentryToRadar* sentry_to_radar_{};
 
   GroupUiBase* graph_queue_sender_{};
   std::deque<Graph> graph_queue_;
   std::deque<Graph> character_queue_;
+  //  std::deque<std::tuple<>> interactive_data_queue_;
 
   ros::Time radar_interactive_data_last_send_;
   ros::Time sentry_interactive_data_last_send_;

rm_referee/include/rm_referee/ui/flash_ui.h

@@ -22,6 +22,47 @@ class FlashUi : public UiBase
   void updateFlashUiForQueue(const ros::Time& time, bool state, bool once);
 };
 
+class FlashGroupUi : public GroupUiBase
+{
+public:
+  explicit FlashGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name,
+                        std::deque<Graph>* graph_queue, std::deque<Graph>* character_queue)
+    : GroupUiBase(rpc_value, base, graph_queue, character_queue)
+  {
+    graph_name_ = graph_name;
+  }
+  virtual void display(const ros::Time& time){};
+  virtual void updateConfig(){};
+  void updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph);
+
+protected:
+  std::string graph_name_;
+};
+
+class EngineerActionFlashUi : public FlashGroupUi
+{
+public:
+  explicit EngineerActionFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                 std::deque<Graph>* character_queue)
+    : FlashGroupUi(rpc_value, base, "engineer_action", graph_queue, character_queue)
+  {
+    if (rpc_value.hasMember("data"))
+    {
+      XmlRpc::XmlRpcValue& data = rpc_value["data"];
+      for (int i = 0; i < static_cast<int>(rpc_value["data"].size()); i++)
+      {
+        graph_vector_.insert(std::pair<std::string, Graph*>(std::to_string(static_cast<int>(data[i]["flag"])),
+                                                            new Graph(data[i]["config"], base_, id_++)));
+      }
+    }
+  }
+  void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time);
+
+private:
+  void display(const ros::Time& time) override;
+  uint32_t symbol_;
+};
+
 class CoverFlashUi : public FlashUi
 {
 public:

rm_referee/include/rm_referee/ui/graph.h

@@ -53,6 +53,25 @@ class Graph
   {
     config_.radius = radius;
   }
+  void setIntNum(int num)
+  {
+    int a = num & 1023;
+    int b = (num >> 10) & 2047;
+    int c = num >> 21;
+    config_.radius = a;
+    config_.end_x = b;
+    config_.end_y = c;
+  }
+  void setFloatNum(float data)
+  {
+    int num = static_cast<int>(data * 1000);
+    int a = num & 1023;
+    int b = (num >> 10) & 2047;
+    int c = num >> 21;
+    config_.radius = a;
+    config_.end_x = b;
+    config_.end_y = c;
+  }
   void setStartX(int start_x)
   {
     config_.start_x = start_x;