Merge pull request #224 from GuraMumei/master

Add ui

rm_msgs/msg/referee/ClientMapSendData.msg

@@ -1,14 +1,17 @@
 uint8 KEY_A = 65
+uint8 KEY_E = 69
 uint8 KEY_H = 72
 uint8 KEY_K = 75
 uint8 KEY_L = 76
 uint8 KEY_N = 78
 uint8 KEY_O = 79
+uint8 KEY_Q = 81
+uint8 KEY_R = 82
+uint8 KEY_W = 87
 uint8 KEY_U = 85
 
 float32 target_position_x
 float32 target_position_y
-float32 target_position_z
 uint8 command_keyboard
 uint16 target_robot_ID
 uint8 cmd_source

rm_referee/include/rm_referee/referee_base.h

@@ -36,6 +36,8 @@ class RefereeBase
                                        const ros::Time& last_get_data_time);
   virtual void eventDataCallBack(const rm_msgs::EventData& event_data, const ros::Time& last_get_data_time);
   virtual void updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data);
+  virtual void supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data);
+  virtual void updateShootDataDataCallBack(const rm_msgs::ShootData& msg);
 
   // sub call back
   virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state);
@@ -59,6 +61,7 @@ class RefereeBase
   virtual void sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data);
   virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data);
   virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data);
+  virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data);
 
   // send  ui
   void sendSerialDataCallback();
@@ -86,15 +89,16 @@ class RefereeBase
   ros::Subscriber sentry_cmd_sub_;
   ros::Subscriber radar_cmd_sub_;
   ros::Subscriber sentry_state_sub_;
+  ros::Subscriber drone_pose_sub_;
 
   ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
   ShooterTriggerChangeUi* shooter_trigger_change_ui_{};
   GimbalTriggerChangeUi* gimbal_trigger_change_ui_{};
   TargetTriggerChangeUi* target_trigger_change_ui_{};
   TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{};
   CameraTriggerChangeUi* camera_trigger_change_ui_{};
-  BulletTimeChangeUi* bullet_time_change_ui_{};
 
+  BulletTimeChangeUi* bullet_time_change_ui_{};
   CapacitorTimeChangeUi* capacitor_time_change_ui_{};
   EffortTimeChangeUi* effort_time_change_ui_{};
   ProgressTimeChangeUi* progress_time_change_ui_{};
@@ -106,6 +110,7 @@ class RefereeBase
   JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{},
       *engineer_joint3_time_change_ui{};
   TargetDistanceTimeChangeUi* target_distance_time_change_ui_{};
+  DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_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_{};
@@ -115,6 +120,7 @@ class RefereeBase
   CoverFlashUi* cover_flash_ui_{};
   SpinFlashUi* spin_flash_ui_{};
   HeroHitFlashUi* hero_hit_flash_ui_{};
+  ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{};
 
   InteractiveSender* interactive_data_sender_{};
   InteractiveSender* enemy_hero_state_sender_{};

rm_referee/include/rm_referee/ui/flash_ui.h

@@ -64,4 +64,20 @@ class HeroHitFlashUi : public FlashUi
   bool hitted_;
   rm_msgs::GameRobotHp last_hp_msg_;
 };
+
+class ExceedBulletSpeedFlashUi : public FlashUi
+{
+public:
+  explicit ExceedBulletSpeedFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                    std::deque<Graph>* character_queue)
+    : FlashUi(rpc_value, base, "exceed_bullet_speed", graph_queue, character_queue)
+  {
+  }
+  void updateShootData(const rm_msgs::ShootData& msg);
+
+private:
+  void display(const ros::Time& time) override;
+  rm_msgs::ShootData shoot_data_;
+};
+
 }  // namespace rm_referee

rm_referee/include/rm_referee/ui/time_change_ui.h

@@ -300,4 +300,38 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi
   void updateConfig() override;
   double target_distance_;
 };
+
+class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi
+{
+public:
+  explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                         std::deque<Graph>* character_queue)
+    : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue)
+  {
+    if (rpc_value.hasMember("data"))
+    {
+      XmlRpc::XmlRpcValue& data = rpc_value["data"];
+      ori_x_ = static_cast<int>(data["ori_x"]);
+      ori_y_ = static_cast<int>(data["ori_y"]);
+    }
+    else
+      ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined.");
+
+    graph_vector_.insert(
+        std::pair<std::string, Graph*>(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++)));
+    graph_vector_.insert(
+        std::pair<std::string, Graph*>(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++)));
+    graph_vector_.insert(
+        std::pair<std::string, Graph*>(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++)));
+  };
+  void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data);
+
+private:
+  void updateConfig() override;
+  int ori_x_, ori_y_;
+  double angle_;
+  int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_,
+      right_line_y2_;
+};
+
 }  // namespace rm_referee

rm_referee/include/rm_referee/ui/trigger_change_ui.h

@@ -238,4 +238,5 @@ class StringTriggerChangeUi : public TriggerChangeUi
   void update() override;
   std::string data_;
 };
+
 }  // namespace rm_referee

rm_referee/src/referee.cpp

@@ -215,6 +215,7 @@ int Referee::unpack(uint8_t* rx_data)
           supply_projectile_action_data.supply_robot_id = supply_projectile_action_ref.supply_robot_id;
           supply_projectile_action_data.stamp = last_get_data_time_;
 
+          referee_ui_.supplyBulletDataCallBack(supply_projectile_action_data);
           supply_projectile_action_pub_.publish(supply_projectile_action_data);
           break;
         }
@@ -331,6 +332,7 @@ int Referee::unpack(uint8_t* rx_data)
           shoot_data.shooter_id = shoot_data_ref.shooter_id;
           shoot_data.stamp = last_get_data_time_;
 
+          referee_ui_.updateShootDataDataCallBack(shoot_data);
           shoot_data_pub_.publish(shoot_data);
           break;
         }

rm_referee/src/referee_base.cpp

@@ -44,6 +44,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       nh.subscribe<rm_msgs::RadarInfo>("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this);
   RefereeBase::sentry_state_sub_ =
       nh.subscribe<std_msgs::String>("/sentry_state", 1, &RefereeBase::sendSentryStateCallback, this);
+  RefereeBase::drone_pose_sub_ =
+      nh.subscribe<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 1, &RefereeBase::dronePoseCallBack, this);
 
   XmlRpc::XmlRpcValue rpc_value;
   send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
@@ -123,6 +125,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       if (rpc_value[i]["name"] == "target_distance")
         target_distance_time_change_ui_ =
             new TargetDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "drone_towards")
+        drone_towards_time_change_group_ui_ =
+            new DroneTowardsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
     }
 
     ui_nh.getParam("fixed", rpc_value);
@@ -137,6 +142,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
         spin_flash_ui_ = new SpinFlashUi(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")
+        exceed_bullet_speed_flash_ui_ =
+            new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
     }
   }
   if (nh.hasParam("interactive_data"))
@@ -209,6 +217,8 @@ void RefereeBase::addUi()
     engineer_joint2_time_change_ui->addForQueue();
   if (engineer_joint3_time_change_ui)
     engineer_joint3_time_change_ui->addForQueue();
+  if (drone_towards_time_change_group_ui_)
+    drone_towards_time_change_group_ui_->addForQueue();
   //  if (drag_state_trigger_change_ui_)
   //    drag_state_trigger_change_ui_->addForQueue();
   if (gripper_state_trigger_change_ui_)
@@ -533,4 +543,20 @@ void RefereeBase::sendSentryStateCallback(const std_msgs::StringConstPtr& data)
     sentry_state_sender_->sendCustomInfoData(converter.from_bytes(data->data));
 }
 
+void RefereeBase::supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data)
+{
+}
+
+void RefereeBase::dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data)
+{
+  if (drone_towards_time_change_group_ui_ && !is_adding_)
+    drone_towards_time_change_group_ui_->updateTowardsData(data);
+}
+
+void RefereeBase::updateShootDataDataCallBack(const rm_msgs::ShootData& msg)
+{
+  if (exceed_bullet_speed_flash_ui_ && !is_adding_)
+    exceed_bullet_speed_flash_ui_->updateShootData(msg);
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/flash_ui.cpp

@@ -86,5 +86,17 @@ void HeroHitFlashUi::display(const ros::Time& time)
     FlashUi::updateFlashUiForQueue(time, hitted_, false);
 }
 
+void ExceedBulletSpeedFlashUi::display(const ros::Time& time)
+{
+  if (shoot_data_.bullet_speed <= 30)
+    graph_->setOperation(rm_referee::GraphOperation::DELETE);
+  FlashUi::updateFlashUiForQueue(time, shoot_data_.bullet_speed > 30, true);
+}
+
+void ExceedBulletSpeedFlashUi::updateShootData(const rm_msgs::ShootData& msg)
+{
+  shoot_data_ = msg;
+}
+
 }  // namespace rm_referee
 // namespace rm_referee

rm_referee/src/ui/time_change_ui.cpp

@@ -398,4 +398,46 @@ void TargetDistanceTimeChangeUi::updateConfig()
   UiBase::transferInt(std::floor(target_distance_ * 1000));
 }
 
+void DroneTowardsTimeChangeGroupUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data)
+{
+  angle_ = yawFromQuat(data->pose.orientation) - M_PI / 2;
+  mid_line_x2_ = ori_x_ + 60 * cos(angle_ - M_PI / 2);
+  mid_line_y2_ = ori_y_ + 60 * sin(angle_ - M_PI / 2);
+  mid_line_x1_ = ori_x_ + 60 * cos(angle_ + M_PI / 2);
+  mid_line_y1_ = ori_y_ + 60 * sin(angle_ + M_PI / 2);
+  left_line_x2_ = ori_x_ + 40 * cos(angle_ + (5 * M_PI) / 6);
+  left_line_y2_ = ori_y_ + 40 * sin(angle_ + (5 * M_PI) / 6);
+  right_line_x2_ = ori_x_ + 40 * cos(angle_ + M_PI / 6);
+  right_line_y2_ = ori_y_ + 40 * sin(angle_ + M_PI / 6);
+  updateForQueue();
+}
+
+void DroneTowardsTimeChangeGroupUi::updateConfig()
+{
+  for (auto it : graph_vector_)
+  {
+    if (it.first == "drone_towards_mid")
+    {
+      it.second->setStartX(mid_line_x2_);
+      it.second->setStartY(mid_line_y2_);
+      it.second->setEndX(mid_line_x1_);
+      it.second->setEndY(mid_line_y1_);
+    }
+    else if (it.first == "drone_towards_left")
+    {
+      it.second->setStartX(left_line_x2_);
+      it.second->setStartY(left_line_y2_);
+      it.second->setEndX(mid_line_x1_);
+      it.second->setEndY(mid_line_y1_);
+    }
+    else if (it.first == "drone_towards_right")
+    {
+      it.second->setStartX(right_line_x2_);
+      it.second->setStartY(right_line_y2_);
+      it.second->setEndX(mid_line_x1_);
+      it.second->setEndY(mid_line_y1_);
+    }
+  }
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/trigger_change_ui.cpp

@@ -436,4 +436,5 @@ void StringTriggerChangeUi::update()
   graph_->setOperation(rm_referee::GraphOperation::UPDATE);
   updateForQueue(true);
 }
+
 }  // namespace rm_referee