Update rm_referee for engineer, radar and sentry.

rm_referee/include/rm_referee/common/protocol.h

@@ -134,7 +134,8 @@ 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,
+  REFEREE_SERVER = 0x8080
 } ClientId;
 
 typedef enum
@@ -386,16 +387,6 @@ 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
 {
@@ -511,6 +502,18 @@ typedef struct
   uint8_t data;
 } __packed InteractiveData;
 
+typedef struct
+{
+  InteractiveDataHeader header;
+  uint32_t sentry_info;
+} __packed SentryInfo;
+
+typedef struct
+{
+  InteractiveDataHeader header;
+  uint8_t radar_info;
+} __packed RadarInfo;
+
 typedef struct
 {
   uint8_t data[30];

rm_referee/include/rm_referee/referee_base.h

@@ -57,6 +57,8 @@ class RefereeBase
   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 sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data);
+  virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data);
 
   // send  ui
   void sendSerialDataCallback();
@@ -82,6 +84,8 @@ class RefereeBase
   ros::Subscriber map_sentry_sub_;
   ros::Subscriber sentry_deviate_sub_;
   ros::Subscriber radar_to_sentry_sub_;
+  ros::Subscriber sentry_cmd_sub_;
+  ros::Subscriber radar_cmd_sub_;
 
   ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
   ShooterTriggerChangeUi* shooter_trigger_change_ui_{};
@@ -102,6 +106,9 @@ class RefereeBase
   JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{},
       *engineer_joint3_time_change_ui{};
   TargetDistanceTimeChangeUi* target_distance_time_change_ui_{};
+  StringTriggerChangeUi *step_name_trigger_change_ui_{}, *servo_mode_trigger_change_ui_{},
+      *reversal_state_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, *joint_temperature_trigger_change_ui_{},
+      *drag_state_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{};
 
   FixedUi* fixed_ui_{};
 
@@ -115,6 +122,7 @@ class RefereeBase
 
   ros::Time radar_interactive_data_last_send_;
   ros::Time sentry_interactive_data_last_send_;
+  ros::Time sentry_cmd_data_last_send_, radar_cmd_data_last_send_;
   UiBase* interactive_data_sender_{};
 
   Base& base_;

rm_referee/include/rm_referee/ui/trigger_change_ui.h

@@ -226,4 +226,16 @@ class CameraTriggerChangeUi : public TriggerChangeUi
   std::string current_camera_{}, camera1_name_{}, camera2_name_{};
 };
 
+class StringTriggerChangeUi : public TriggerChangeUi
+{
+public:
+  explicit StringTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& name,
+                                 std::deque<Graph>* graph_queue, std::deque<Graph>* character_queue)
+    : TriggerChangeUi(rpc_value, base, name, graph_queue, character_queue){};
+  void updateStringUiData(const std::string& data);
+
+private:
+  void update() override;
+  std::string data_;
+};
 }  // namespace rm_referee

rm_referee/include/rm_referee/ui/ui_base.h

@@ -47,6 +47,8 @@ class UiBase
   void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data);
   void sendCustomInfoData(std::wstring data);
   void transferInt(const int data);
+  void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data);
+  void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data);
 
   void sendSerial(const ros::Time& time, int data_len);
   void clearTxBuffer();

rm_referee/src/referee_base.cpp

@@ -39,7 +39,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       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);
-  //"/bullet_allowance_data"
+  RefereeBase::sentry_cmd_sub_ =
+      nh.subscribe<rm_msgs::SentryInfo>("/sentry_cmd", 1, &RefereeBase::sendSentryCmdCallback, this);
+  RefereeBase::radar_cmd_sub_ =
+      nh.subscribe<rm_msgs::RadarInfo>("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this);
 
   XmlRpc::XmlRpcValue rpc_value;
   send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
@@ -66,6 +69,22 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
             new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "camera")
         camera_trigger_change_ui_ = new CameraTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      //      if (rpc_value[i]["name"] == "drag")
+      //        drag_state_trigger_change_ui_ =
+      //            new StringTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "drag");
+      if (rpc_value[i]["name"] == "gripper")
+        gripper_state_trigger_change_ui_ =
+            new StringTriggerChangeUi(rpc_value[i], base_, "gripper", &graph_queue_, &character_queue_);
+      //      if (rpc_value[i]["name"] == "step")
+      //        step_name_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "step");
+      //      if (rpc_value[i]["name"] == "reversal")
+      //        reversal_state_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "reversal");
+      //      if (rpc_value[i]["name"] == "stone")
+      //        stone_num_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "stone");
+      //      if (rpc_value[i]["name"] == "temperature")
+      //        joint_temperature_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "temperature");
+      //      if (rpc_value[i]["name"] == "servo_mode")
+      //        servo_mode_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "mode");
     }
 
     ui_nh.getParam("time_change", rpc_value);
@@ -178,6 +197,10 @@ void RefereeBase::addUi()
     engineer_joint2_time_change_ui->addForQueue();
   if (engineer_joint3_time_change_ui)
     engineer_joint3_time_change_ui->addForQueue();
+  //  if (drag_state_trigger_change_ui_)
+  //    drag_state_trigger_change_ui_->addForQueue();
+  if (gripper_state_trigger_change_ui_)
+    gripper_state_trigger_change_ui_->addForQueue();
   if (bullet_time_change_ui_)
   {
     bullet_time_change_ui_->reset();
@@ -390,6 +413,10 @@ void RefereeBase::engineerUiDataCallback(const rm_msgs::EngineerUi::ConstPtr& da
 {
   /*if (progress_time_change_ui_ && !is_adding_)
     progress_time_change_ui_->updateEngineerUiData(data, ros::Time::now());*/
+  /*  if (drag_state_trigger_change_ui_ && !is_adding_)
+      drag_state_trigger_change_ui_->updateStringUiData(data->drag_state);*/
+  if (gripper_state_trigger_change_ui_ && !is_adding_)
+    gripper_state_trigger_change_ui_->updateStringUiData(data->gripper_state);
 }
 void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data)
 {
@@ -466,5 +493,25 @@ void RefereeBase::sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataC
 {
   interactive_data_sender_->sendCurrentSentryData(data);
 }
+void RefereeBase::sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data)
+{
+  if (ros::Time::now() - sentry_cmd_data_last_send_ <= ros::Duration(0.15))
+    return;
+  else
+  {
+    interactive_data_sender_->sendSentryCmdData(data);
+    sentry_cmd_data_last_send_ = ros::Time::now();
+  }
+}
+void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data)
+{
+  if (ros::Time::now() - radar_cmd_data_last_send_ <= ros::Duration(0.15))
+    return;
+  else
+  {
+    interactive_data_sender_->sendRadarCmdData(data);
+    radar_cmd_data_last_send_ = ros::Time::now();
+  }
+}
 
 }  // namespace rm_referee

rm_referee/src/ui/trigger_change_ui.cpp

@@ -422,4 +422,18 @@ void CameraTriggerChangeUi::update()
   graph_->setOperation(rm_referee::GraphOperation::UPDATE);
   updateForQueue(true);
 }
+
+void StringTriggerChangeUi::updateStringUiData(const std::string& data)
+{
+  data_ = data;
+  update();
+}
+
+void StringTriggerChangeUi::update()
+{
+  graph_->setContent(data_);
+  graph_->setColor(rm_referee::GraphColor::GREEN);
+  graph_->setOperation(rm_referee::GraphOperation::UPDATE);
+  updateForQueue(true);
+}
 }  // namespace rm_referee

rm_referee/src/ui/ui_base.cpp

@@ -320,6 +320,36 @@ void UiBase::sendSingleGraph(const ros::Time& time, Graph* graph)
   sendSerial(time, data_len);
 }
 
+void UiBase::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data)
+{
+  int data_len;
+  rm_referee::SentryInfo tx_data;
+  data_len = static_cast<int>(sizeof(rm_referee::SentryInfo));
+
+  tx_data.header.sender_id = base_.robot_id_;
+  tx_data.header.receiver_id = REFEREE_SERVER;
+  tx_data.sentry_info = data->sentry_info;
+
+  tx_data.header.data_cmd_id = rm_referee::DataCmdId::SENTRY_CMD;
+  pack(tx_buffer_, reinterpret_cast<uint8_t*>(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len);
+  sendSerial(ros::Time::now(), data_len);
+}
+
+void UiBase::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data)
+{
+  int data_len;
+  rm_referee::RadarInfo tx_data;
+  data_len = static_cast<int>(sizeof(rm_referee::RadarInfo));
+
+  tx_data.header.sender_id = base_.robot_id_;
+  tx_data.header.receiver_id = REFEREE_SERVER;
+  tx_data.radar_info = data->radar_info;
+
+  tx_data.header.data_cmd_id = rm_referee::DataCmdId::RADAR_CMD;
+  pack(tx_buffer_, reinterpret_cast<uint8_t*>(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len);
+  sendSerial(ros::Time::now(), data_len);
+}
+
 void GroupUiBase::display(bool check_repeat)
 {
   if (check_repeat)
@@ -434,7 +464,7 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr
   tx_data.config[3] = graph3->getConfig();
   tx_data.config[4] = graph4->getConfig();
   tx_data.config[5] = graph5->getConfig();
-  tx_data.config[6] = graph5->getConfig();
+  tx_data.config[6] = graph6->getConfig();
 
   tx_data.header.data_cmd_id = rm_referee::DataCmdId::CLIENT_GRAPH_SEVEN_CMD;
   pack(tx_buffer_, reinterpret_cast<uint8_t*>(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len);