Merge pull request #229 from chen-gr/master

Add engineer action ui.
rm-controls · Jul 1, 2024 · 2ff031a · 2ff031a
2 parents 8a8cb7e + 5a633d7
commit 2ff031a
Show file tree

Hide file tree

Showing 5 changed files with 93 additions and 1 deletion.
diff --git a/rm_msgs/msg/referee/EngineerUi.msg b/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
diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h
@@ -121,6 +121,7 @@ class RefereeBase
   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_{};

diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/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:

diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp
@@ -138,6 +138,10 @@ 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 (nh.hasParam("interactive_data"))
@@ -440,6 +444,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());
 }
 void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data)
 {

diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp
@@ -33,6 +33,50 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once
   UiBase::updateForQueue();
 }
 
+void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph)
+{
+    if (once)
+    {
+        if (state)
+            graph->setOperation(rm_referee::GraphOperation::ADD);
+        else
+            graph->setOperation(rm_referee::GraphOperation::DELETE);
+    }
+    else if (time - last_send_ > delay_)
+    {
+        ROS_INFO("%f  %.3f", last_send_.toSec(), delay_.toSec());
+        if (state)
+            graph->setOperation(rm_referee::GraphOperation::ADD);
+        else
+            graph->setOperation(rm_referee::GraphOperation::DELETE);
+    }
+    if (graph->isRepeated())
+        return;
+    graph->updateLastConfig();
+    last_send_ = time;
+    if (graph->isString())
+        character_queue_->push_back(*graph);
+    else
+        graph_queue_->push_back(*graph);
+}
+
+void EngineerActionFlashUi::updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time &last_get_data_time)
+{
+    symbol_ = data->symbol;
+    display(last_get_data_time);
+}
+
+void EngineerActionFlashUi::display(const ros::Time &time)
+{
+    for (auto graph : graph_vector_)
+    {
+        bool state = false;
+        if (std::to_string(static_cast<int>(symbol_)) == graph.first)
+            state = true;
+        FlashGroupUi::updateFlashUiForQueue(time, state, true, graph.second);
+    }
+}
+
 void CoverFlashUi::display(const ros::Time& time)
 {
   if (!cover_state_)