Merge pull request #240 from GuraMumei/master

Update rm_referee and add new ui

rm_msgs/CMakeLists.txt

@@ -87,6 +87,7 @@ add_message_files(
         Buff.msg
         RadarToSentry.msg
         SentryCmd.msg
+        VisualizeStateData.msg
         PowerManagementSampleAndStatusData.msg
         PowerManagementInitializationExceptionData.msg
         PowerManagementProcessStackOverflowData.msg

rm_msgs/msg/referee/VisualizeStateData.msg

@@ -0,0 +1 @@
+bool[] state

rm_msgs/srv/SetLimitVel.srv

@@ -1,3 +1,5 @@
 float32 limit_vel
+float32 slide_window
 ---
 float32 current_limit_vel
+float32 current_slide_window

rm_referee/include/rm_referee/common/data.h

@@ -47,6 +47,8 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "std_msgs/UInt32.h"
+#include "rm_msgs/VisualizeStateData.h"
 
 #include "rm_referee/common/protocol.h"
 

rm_referee/include/rm_referee/referee_base.h

@@ -64,6 +64,8 @@ class RefereeBase
   virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data);
   virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data);
   virtual void radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& data);
+  virtual void customizeDisplayCmdCallBack(const std_msgs::UInt32ConstPtr& data);
+  virtual void visualizeStateDataCallBack(const rm_msgs::VisualizeStateDataConstPtr& data);
 
   // send  ui
   void sendSerialDataCallback();
@@ -94,6 +96,8 @@ class RefereeBase
   ros::Subscriber sentry_state_sub_;
   ros::Subscriber drone_pose_sub_;
   ros::Subscriber shoot_cmd_sub_;
+  ros::Subscriber customize_display_cmd_sub_;
+  ros::Subscriber visualize_state_data_sub_;
 
   ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
   ShooterTriggerChangeUi* shooter_trigger_change_ui_{};
@@ -120,14 +124,15 @@ class RefereeBase
   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_{};
+  VisualizeStateTriggerChangeUi* visualize_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_{};
+  CustomizeDisplayFlashUi* customize_display_flash_ui_{};
 
   InteractiveSender* interactive_data_sender_{};
   CustomInfoSender* custom_info_sender{};

rm_referee/include/rm_referee/ui/flash_ui.h

@@ -40,12 +40,12 @@ class FlashGroupUi : public GroupUiBase
   std::string graph_name_;
 };
 
-class EngineerActionFlashUi : public FlashGroupUi
+class CustomizeDisplayFlashUi : 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)
+  explicit CustomizeDisplayFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                   std::deque<Graph>* character_queue)
+    : FlashGroupUi(rpc_value, base, "customize_display", graph_queue, character_queue)
   {
     if (rpc_value.hasMember("data"))
     {
@@ -57,7 +57,7 @@ class EngineerActionFlashUi : public FlashGroupUi
       }
     }
   }
-  void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time);
+  void updateCmdData(const uint32_t& data);
 
 private:
   void display(const ros::Time& time) override;

rm_referee/include/rm_referee/ui/trigger_change_ui.h

@@ -252,4 +252,27 @@ class FrictionSpeedTriggerChangeUi : public TriggerChangeUi
   double wheel_speed_;
 };
 
+class VisualizeStateTriggerChangeUi : public TriggerChangeGroupUi
+{
+public:
+  explicit VisualizeStateTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& name,
+                                         std::deque<Graph>* graph_queue, std::deque<Graph>* character_queue)
+    : TriggerChangeGroupUi(rpc_value, base, name, 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(i), new Graph(data[i]["config"], base_, id_++)));
+      }
+    }
+  };
+  void updateUiColor(const std::vector<bool>& data);
+
+private:
+  void update() override;
+};
+
 }  // namespace rm_referee

rm_referee/src/referee_base.cpp

@@ -50,6 +50,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       "/sentry_target_to_referee", 1, &RefereeBase::sentryAttackingTargetCallback, this);
   RefereeBase::radar_to_referee_sub_ =
       nh.subscribe<rm_msgs::RadarToSentry>("/radar_to_referee", 1, &RefereeBase::radarToRefereeCallBack, this);
+  RefereeBase::customize_display_cmd_sub_ =
+      nh.subscribe<std_msgs::UInt32>("/customize_display_ui", 1, &RefereeBase::customizeDisplayCmdCallBack, this);
+  RefereeBase::visualize_state_data_sub_ =
+      nh.subscribe<rm_msgs::VisualizeStateData>("/visualize_state", 1, &RefereeBase::visualizeStateDataCallBack, this);
 
   XmlRpc::XmlRpcValue rpc_value;
   send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
@@ -88,6 +92,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       if (rpc_value[i]["name"] == "stone")
         stone_num_trigger_change_ui_ =
             new StringTriggerChangeUi(rpc_value[i], base_, "stone_num", &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "visualize_state")
+        visualize_state_trigger_change_ui_ =
+            new VisualizeStateTriggerChangeUi(rpc_value[i], base_, "visualize_state", &graph_queue_, &character_queue_);
     }
 
     ui_nh.getParam("time_change", rpc_value);
@@ -148,17 +155,16 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       if (rpc_value[i]["name"] == "exceed_bullet_speed")
         exceed_bullet_speed_flash_ui_ =
             new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
-      if (rpc_value[i]["name"] == "engineer_action")
-        engineer_action_flash_ui_ = new EngineerActionFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "customize_display")
+        customize_display_flash_ui_ =
+            new CustomizeDisplayFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
     }
   }
   if (nh.hasParam("interactive_data"))
   {
     nh.getParam("interactive_data", rpc_value);
     for (int i = 0; i < rpc_value.size(); i++)
     {
-      //      if (rpc_value[i]["name"] == "enemy_hero_state")
-      //        enemy_hero_state_sender_ = new CustomInfoSender(rpc_value[i], base_);
       if (rpc_value[i]["name"] == "custom_info")
         custom_info_sender = new CustomInfoSender(rpc_value[i], base_);
       if (rpc_value[i]["name"] == "bullet_num_share")
@@ -247,6 +253,8 @@ void RefereeBase::addUi()
     target_distance_time_change_ui_->addForQueue();
   if (friend_bullets_time_change_group_ui_)
     friend_bullets_time_change_group_ui_->addForQueue();
+  if (visualize_state_trigger_change_ui_)
+    visualize_state_trigger_change_ui_->addForQueue();
   add_ui_times_++;
 }
 
@@ -453,8 +461,8 @@ void RefereeBase::engineerUiDataCallback(const rm_msgs::EngineerUi::ConstPtr& da
     stone_num_trigger_change_ui_->updateStringUiData(std::to_string(data->stone_num));
   if (servo_mode_trigger_change_ui_ && !is_adding_)
     servo_mode_trigger_change_ui_->updateStringUiData(data->control_mode);
-  if (engineer_action_flash_ui_ && !is_adding_)
-    engineer_action_flash_ui_->updateEngineerUiCmdData(data, ros::Time::now());
+  if (customize_display_flash_ui_ && !is_adding_)
+    customize_display_flash_ui_->updateCmdData(data->symbol);
 }
 void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data)
 {
@@ -585,4 +593,21 @@ void RefereeBase::radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& d
     radar_to_sentry_->updateRadarToSentryData(data);
 }
 
+void RefereeBase::customizeDisplayCmdCallBack(const std_msgs::UInt32ConstPtr& data)
+{
+  if (customize_display_flash_ui_ && !is_adding_)
+    customize_display_flash_ui_->updateCmdData(data->data);
+}
+
+void RefereeBase::visualizeStateDataCallBack(const rm_msgs::VisualizeStateDataConstPtr& data)
+{
+  if (visualize_state_trigger_change_ui_ && !is_adding_)
+  {
+    std::vector<bool> state;
+    for (auto state_data : data->state)
+      state.push_back(state_data);
+    visualize_state_trigger_change_ui_->updateUiColor(state);
+  }
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/flash_ui.cpp

@@ -122,14 +122,13 @@ void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool
     graph_queue_->push_back(*graph);
 }
 
-void EngineerActionFlashUi::updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data,
-                                                    const ros::Time& last_get_data_time)
+void CustomizeDisplayFlashUi::updateCmdData(const uint32_t& data)
 {
-  symbol_ = data->symbol;
-  display(last_get_data_time);
+  symbol_ = data;
+  display(ros::Time::now());
 }
 
-void EngineerActionFlashUi::display(const ros::Time& time)
+void CustomizeDisplayFlashUi::display(const ros::Time& time)
 {
   for (auto graph : graph_vector_)
   {

rm_referee/src/ui/trigger_change_ui.cpp

@@ -450,4 +450,21 @@ void FrictionSpeedTriggerChangeUi::update()
   updateForQueue(true);
 }
 
+void VisualizeStateTriggerChangeUi::updateUiColor(const std::vector<bool>& data)
+{
+  for (int i = 0; i < static_cast<int>(data.size()); i++)
+  {
+    graph_vector_.find(std::to_string(i))
+        ->second->setColor(data[i] ? rm_referee::GraphColor::GREEN : rm_referee::GraphColor::PINK);
+  }
+  update();
+}
+
+void VisualizeStateTriggerChangeUi::update()
+{
+  for (auto graph : graph_vector_)
+    graph.second->setOperation(rm_referee::GraphOperation::UPDATE);
+  updateForQueue(true);
+}
+
 }  // namespace rm_referee