Merge pull request #254 from 321Aurora/deploy-ui

Achieve deploy ui and add deploy mode in chassis cmd

Author: YoujianWu

rm_msgs/msg/ChassisCmd.msg

@@ -3,6 +3,7 @@ uint8 FOLLOW = 1
 uint8 TWIST = 2
 uint8 UP_SLOPE = 3
 uint8 FALLEN = 4
+uint8 DEPLOY = 5
 
 uint8 mode
 geometry_msgs/Accel accel

rm_referee/include/rm_referee/referee_base.h

@@ -130,6 +130,7 @@ class RefereeBase
 
   CoverFlashUi* cover_flash_ui_{};
   SpinFlashUi* spin_flash_ui_{};
+  DeployFlashUi* deploy_flash_ui_{};
   HeroHitFlashUi* hero_hit_flash_ui_{};
   ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{};
   CustomizeDisplayFlashUi* customize_display_flash_ui_{};

rm_referee/include/rm_referee/ui/flash_ui.h

@@ -91,6 +91,21 @@ class SpinFlashUi : public FlashUi
   uint8_t chassis_mode_;
 };
 
+class DeployFlashUi : public FlashUi
+{
+public:
+  explicit DeployFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                         std::deque<Graph>* character_queue)
+    : FlashUi(rpc_value, base, "deploy", graph_queue, character_queue){};
+  void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data, const ros::Time& last_get_data_time);
+  void updateChassisVelData(const geometry_msgs::Twist::ConstPtr& data);
+
+private:
+  void display(const ros::Time& time) override;
+  uint8_t chassis_mode_;
+  double angular_z_{ 0. };
+};
+
 class HeroHitFlashUi : public FlashGroupUi
 {
 public:

rm_referee/src/referee_base.cpp

@@ -150,6 +150,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
         cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "spin")
         spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "deploy")
+        deploy_flash_ui_ = new DeployFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "hero_hit")
         hero_hit_flash_ui_ = new HeroHitFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "exceed_bullet_speed")
@@ -432,11 +434,15 @@ void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& da
     chassis_trigger_change_ui_->updateChassisCmdData(data);
   if (spin_flash_ui_ && !is_adding_)
     spin_flash_ui_->updateChassisCmdData(data, ros::Time::now());
+  if (deploy_flash_ui_ && !is_adding_)
+    deploy_flash_ui_->updateChassisCmdData(data, ros::Time::now());
   if (rotation_time_change_ui_ && !is_adding_)
     rotation_time_change_ui_->updateChassisCmdData(data);
 }
 void RefereeBase::vel2DCmdDataCallback(const geometry_msgs::Twist::ConstPtr& data)
 {
+  if (deploy_flash_ui_ && !is_adding_)
+    deploy_flash_ui_->updateChassisVelData(data);
 }
 void RefereeBase::shootStateCallback(const rm_msgs::ShootState::ConstPtr& data)
 {

rm_referee/src/ui/flash_ui.cpp

@@ -166,6 +166,24 @@ void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data,
   display(last_get_data_time);
 }
 
+void DeployFlashUi::display(const ros::Time& time)
+{
+  if (!(chassis_mode_ == rm_msgs::ChassisCmd::RAW && angular_z_ == 0.0))
+    graph_->setOperation(rm_referee::GraphOperation::DELETE);
+  FlashUi::updateFlashUiForQueue(time, (chassis_mode_ == rm_msgs::ChassisCmd::RAW && angular_z_ == 0.0), false);
+}
+
+void DeployFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data, const ros::Time& last_get_data_time)
+{
+  chassis_mode_ = data->mode;
+  display(last_get_data_time);
+}
+
+void DeployFlashUi::updateChassisVelData(const geometry_msgs::Twist::ConstPtr& data)
+{
+  angular_z_ = data->angular.z;
+}
+
 void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg)
 {
   if (base_.robot_id_ > 100)