Merge pull request #201 from GuraMumei/add_publisher_and_msg_for_sentry

Add publisher and msg for sentry

rm_msgs/CMakeLists.txt

@@ -27,6 +27,7 @@ add_message_files(
         MultiDofCmd.msg
         TagMsg.msg
         TagMsgArray.msg
+        SentryDeviate.msg
 )
 
 add_service_files(
@@ -63,6 +64,7 @@ add_message_files(
         SupplyProjectileAction.msg
         DartRemainingTime.msg
         RobotHurt.msg
+        CurrentSentryPosData.msg
         ShootData.msg
         BulletAllowance.msg
         RfidStatus.msg

rm_msgs/msg/SentryDeviate.msg

@@ -0,0 +1,3 @@
+float32 deviate_x
+float32 deviate_y
+bool isDeviate

rm_msgs/msg/referee/CurrentSentryPosData.msg

@@ -0,0 +1,4 @@
+float32 x
+float32 y
+float32 z
+float32 yaw

rm_referee/include/rm_referee/common/data.h

@@ -80,6 +80,8 @@
 #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/PowerManagementSampleAndStatusData.h>
 #include <rm_msgs/PowerManagementSystemExceptionData.h>
 #include <rm_msgs/PowerManagementInitializationExceptionData.h>

rm_referee/include/rm_referee/common/protocol.h

@@ -76,7 +76,7 @@ typedef enum
   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 +89,7 @@ typedef enum
   CLIENT_GRAPH_FIVE_CMD = 0x0103,
   CLIENT_GRAPH_SEVEN_CMD = 0x0104,
   CLIENT_CHARACTER_CMD = 0x0110,
+  CURRENT_SENTRY_POSITION_CMD = 0x0200  // send radar->sentry
 } DataCmdId;
 
 typedef enum
@@ -540,6 +541,15 @@ typedef struct
   int8_t delta_y[49];
 } __packed MapSentryData;
 
+typedef struct
+{
+  InteractiveDataHeader header_data;
+  float position_x;
+  float position_y;
+  float position_z;
+  float position_yaw;
+} __packed CurrentSentryPosData;
+
 typedef struct
 {
   uint8_t chassis_power_high_8_bit;

rm_referee/include/rm_referee/referee.h

@@ -55,6 +55,7 @@ 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);
     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_ =
@@ -97,6 +98,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 event_data_pub_;
   ros::Publisher dart_status_pub_;
   ros::Publisher icra_buff_debuff_zone_status_pub_;

rm_referee/include/rm_referee/referee_base.h

@@ -50,6 +50,8 @@ 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);
 
   // send graph_type ui
   void sendGraphQueueCallback();
@@ -72,6 +74,8 @@ class RefereeBase
   ros::Subscriber balance_state_sub_;
   ros::Subscriber radar_receive_sub_;
   ros::Subscriber map_sentry_sub_;
+  ros::Subscriber sentry_deviate_sub_;
+  ros::Subscriber radar_to_sentry_sub_;
 
   ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
   ShooterTriggerChangeUi* shooter_trigger_change_ui_{};

rm_referee/include/rm_referee/ui/ui_base.h

@@ -40,6 +40,7 @@ class UiBase
   void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data);
   void sendRadarInteractiveData(rm_referee::ClientMapReceiveData& data);
   void sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data);
+  void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data);
 
   void sendSerial(const ros::Time& time, int data_len);
   void clearTxBuffer();

rm_referee/src/referee.cpp

@@ -425,6 +425,19 @@ int Referee::unpack(uint8_t* rx_data)
         {
           rm_referee::InteractiveData interactive_data_ref;  // local variable temporarily before moving referee data
           memcpy(&interactive_data_ref, rx_data + 7, sizeof(rm_referee::InteractiveData));
+          // TODO: case cmd_id
+          if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::CURRENT_SENTRY_POSITION_CMD)
+          {
+            rm_referee::CurrentSentryPosData current_sentry_pos_ref;
+            rm_msgs::CurrentSentryPosData current_sentry_pos_data;
+            memcpy(&current_sentry_pos_ref, rx_data + 7, sizeof(rm_referee::CurrentSentryPosData));
+            current_sentry_pos_data.x = current_sentry_pos_ref.position_x;
+            current_sentry_pos_data.y = current_sentry_pos_ref.position_y;
+            current_sentry_pos_data.z = current_sentry_pos_ref.position_z;
+            current_sentry_pos_data.yaw = current_sentry_pos_ref.position_yaw;
+
+            current_sentry_pos_pub_.publish(current_sentry_pos_data);
+          }
           break;
         }
         case rm_referee::CLIENT_MAP_CMD:
@@ -460,6 +473,18 @@ int Referee::unpack(uint8_t* rx_data)
           client_map_send_data_pub_.publish(client_map_send_data);
           break;
         }
+        /*
+         m_msgs::GameRobotPos 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.z = game_robot_pos_ref.z;
+game_robot_pos_data.yaw = game_robot_pos_ref.yaw;
+
+game_robot_pos_pub_.publish(game_robot_pos_data);
+
+         */
         case rm_referee::POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD:
         {
           rm_msgs::PowerManagementSampleAndStatusData sample_and_status_pub_data;

rm_referee/src/referee_base.cpp

@@ -34,6 +34,11 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       nh.subscribe<rm_msgs::MapSentryData>("/map_sentry_data", 10, &RefereeBase::mapSentryCallback, this);
   RefereeBase::radar_receive_sub_ =
       nh.subscribe<rm_msgs::ClientMapReceiveData>("/rm_radar", 10, &RefereeBase::radarReceiveCallback, this);
+  RefereeBase::sentry_deviate_sub_ =
+      nh.subscribe<rm_msgs::SentryDeviate>("/deviate", 10, &RefereeBase::sentryDeviateCallback, this);
+  RefereeBase::radar_to_sentry_sub_ = nh.subscribe<rm_msgs::CurrentSentryPosData>(
+      "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this);
+
   XmlRpc::XmlRpcValue rpc_value;
   send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
   add_ui_frequency_ = getParam(nh, "add_ui_frequency", 5);
@@ -344,6 +349,9 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data
   if (balance_pitch_time_change_group_ui_)
     balance_pitch_time_change_group_ui_->calculatePointPosition(data, ros::Time::now());
 }
+void RefereeBase::sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data)
+{
+}
 void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data)
 {
   rm_referee::ClientMapReceiveData send_data;
@@ -357,4 +365,10 @@ void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data)
 {
   interactive_data_sender_->sendMapSentryData(data);
 }
+
+void RefereeBase::sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data)
+{
+  interactive_data_sender_->sendCurrentSentryData(data);
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/ui_base.cpp

@@ -138,6 +138,28 @@ void UiBase::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char
   sendSerial(ros::Time::now(), sizeof(InteractiveData));
 }
 
+void UiBase::sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data)
+{
+  int data_len;
+  uint8_t tx_data[sizeof(CurrentSentryPosData)] = { 0 };
+  auto current_sentry_pos_data = (CurrentSentryPosData*)tx_data;
+  data_len = static_cast<int>(sizeof(rm_referee::CurrentSentryPosData));
+
+  for (int i = 0; i < 128; i++)
+    tx_buffer_[i] = 0;
+
+  current_sentry_pos_data->header_data.data_cmd_id = DataCmdId::CURRENT_SENTRY_POSITION_CMD;
+  current_sentry_pos_data->header_data.sender_id = base_.robot_id_;
+  current_sentry_pos_data->header_data.receiver_id = base_.robot_id_ < 100 ? RobotId::RED_SENTRY : RobotId::BLUE_SENTRY;
+  current_sentry_pos_data->position_x = data->x;
+  current_sentry_pos_data->position_y = data->y;
+  current_sentry_pos_data->position_z = data->z;
+  current_sentry_pos_data->position_yaw = data->yaw;
+
+  pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(InteractiveData));
+  sendSerial(ros::Time::now(), data_len);
+}
+
 void UiBase::sendUi(const ros::Time& time)
 {
   if (base_.robot_id_ == 0 || base_.client_id_ == 0)