Merge pull request #228 from GuraMumei/master

Update hero ui

rm_referee/include/rm_referee/referee_base.h

@@ -62,6 +62,7 @@ class RefereeBase
   virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data);
   virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data);
   virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data);
+  virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data);
 
   // send  ui
   void sendSerialDataCallback();
@@ -90,13 +91,15 @@ class RefereeBase
   ros::Subscriber radar_cmd_sub_;
   ros::Subscriber sentry_state_sub_;
   ros::Subscriber drone_pose_sub_;
+  ros::Subscriber shoot_cmd_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_{};
+  FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{};
 
   BulletTimeChangeUi* bullet_time_change_ui_{};
   CapacitorTimeChangeUi* capacitor_time_change_ui_{};

rm_referee/include/rm_referee/ui/trigger_change_ui.h

@@ -239,4 +239,17 @@ class StringTriggerChangeUi : public TriggerChangeUi
   std::string data_;
 };
 
+class FrictionSpeedTriggerChangeUi : public TriggerChangeUi
+{
+public:
+  explicit FrictionSpeedTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                        std::deque<Graph>* character_queue)
+    : TriggerChangeUi(rpc_value, base, "friction_speed", graph_queue, character_queue){};
+  void updateFrictionSpeedUiData(const rm_msgs::ShootCmdConstPtr& data);
+
+private:
+  void update() override;
+  double wheel_speed_;
+};
+
 }  // namespace rm_referee

rm_referee/src/referee_base.cpp

@@ -46,6 +46,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       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);
+  RefereeBase::shoot_cmd_sub_ = nh.subscribe<rm_msgs::ShootCmd>("/controllers/shooter_controller/command", 1,
+                                                                &RefereeBase::shootCmdCallBack, this);
 
   XmlRpc::XmlRpcValue rpc_value;
   send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
@@ -72,6 +74,9 @@ 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"] == "friction_speed")
+        friction_speed_trigger_change_ui_ =
+            new FrictionSpeedTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "gripper")
         gripper_state_trigger_change_ui_ =
             new StringTriggerChangeUi(rpc_value[i], base_, "gripper", &graph_queue_, &character_queue_);
@@ -216,14 +221,14 @@ void RefereeBase::addUi()
     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_)
     gripper_state_trigger_change_ui_->addForQueue();
   if (stone_num_trigger_change_ui_)
     stone_num_trigger_change_ui_->addForQueue();
   if (servo_mode_trigger_change_ui_)
     servo_mode_trigger_change_ui_->addForQueue();
+  if (friction_speed_trigger_change_ui_)
+    friction_speed_trigger_change_ui_->addForQueue();
   if (bullet_time_change_ui_)
   {
     bullet_time_change_ui_->reset();
@@ -566,4 +571,10 @@ void RefereeBase::updateShootDataDataCallBack(const rm_msgs::ShootData& msg)
     exceed_bullet_speed_flash_ui_->updateShootData(msg);
 }
 
+void RefereeBase::shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data)
+{
+  if (friction_speed_trigger_change_ui_ && !is_adding_)
+    friction_speed_trigger_change_ui_->updateFrictionSpeedUiData(data);
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/flash_ui.cpp

@@ -10,21 +10,23 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once
 {
   if (once)
   {
+    //    ROS_INFO("test");
     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());
+    //    ROS_INFO("%f  %.3f", last_send_.toSec(), delay_.toSec());
     if (state)
       graph_->setOperation(rm_referee::GraphOperation::ADD);
     else
       graph_->setOperation(rm_referee::GraphOperation::DELETE);
     //    graph_->setOperation(graph_->getOperation() == rm_referee::GraphOperation::ADD ?
     //                             rm_referee::GraphOperation::DELETE :
     //                             rm_referee::GraphOperation::ADD);
+    //    ROS_INFO("delay");
   }
   if (graph_->isRepeated())
     return;
@@ -109,25 +111,23 @@ void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg)
   if (base_.robot_id_ > 100)
   {
     hitted_ =
-        (last_hp_msg_.red_outpost_hp - msg.red_outpost_hp > 175 || last_hp_msg_.red_base_hp - msg.red_base_hp > 175);
+        (last_hp_msg_.red_outpost_hp - msg.red_outpost_hp > 190 || last_hp_msg_.red_base_hp - msg.red_base_hp > 190);
   }
   else
   {
-    hitted_ = (last_hp_msg_.blue_outpost_hp - msg.blue_outpost_hp > 175 ||
-               last_hp_msg_.blue_base_hp - msg.blue_base_hp > 175);
+    hitted_ = (last_hp_msg_.blue_outpost_hp - msg.blue_outpost_hp > 190 ||
+               last_hp_msg_.blue_base_hp - msg.blue_base_hp > 190);
   }
   last_hp_msg_ = msg;
+  display(ros::Time::now());
 }
 
 void HeroHitFlashUi::display(const ros::Time& time)
 {
   if (hitted_)
-  {
-    graph_->setOperation(rm_referee::GraphOperation::ADD);
     FlashUi::updateFlashUiForQueue(time, true, true);
-  }
   else
-    FlashUi::updateFlashUiForQueue(time, hitted_, false);
+    FlashUi::updateFlashUiForQueue(time, false, false);
 }
 
 void ExceedBulletSpeedFlashUi::display(const ros::Time& time)

rm_referee/src/ui/time_change_ui.cpp

@@ -326,12 +326,14 @@ void BulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data,
 {
   if (data.bullet_allowance_num_17_mm >= 0 && data.bullet_allowance_num_17_mm < 1000)
   {
-    bullet_num_17_mm_ += (bullet_allowance_num_17_mm_ - data.bullet_allowance_num_17_mm);
+    if (bullet_allowance_num_17_mm_ > data.bullet_allowance_num_17_mm)
+      bullet_num_17_mm_ += (bullet_allowance_num_17_mm_ - data.bullet_allowance_num_17_mm);
     bullet_allowance_num_17_mm_ = data.bullet_allowance_num_17_mm;
   }
   if (data.bullet_allowance_num_42_mm >= 0 && data.bullet_allowance_num_42_mm < 1000)
   {
-    bullet_num_42_mm_ += (bullet_allowance_num_42_mm_ - data.bullet_allowance_num_42_mm);
+    if (bullet_allowance_num_42_mm_ > data.bullet_allowance_num_42_mm)
+      bullet_num_42_mm_ += (bullet_allowance_num_42_mm_ - data.bullet_allowance_num_42_mm);
     bullet_allowance_num_42_mm_ = data.bullet_allowance_num_42_mm;
   }
   updateForQueue();

rm_referee/src/ui/trigger_change_ui.cpp

@@ -437,4 +437,17 @@ void StringTriggerChangeUi::update()
   updateForQueue(true);
 }
 
+void FrictionSpeedTriggerChangeUi::updateFrictionSpeedUiData(const rm_msgs::ShootCmdConstPtr& data)
+{
+  wheel_speed_ = data->wheel_speed;
+  update();
+}
+
+void FrictionSpeedTriggerChangeUi::update()
+{
+  graph_->setRadius(std::floor(wheel_speed_));
+  graph_->setOperation(rm_referee::GraphOperation::UPDATE);
+  updateForQueue(true);
+}
+
 }  // namespace rm_referee