Fix fixed ui and lane line bug.

rm_referee/include/rm_referee/referee_base.h

@@ -89,7 +89,7 @@ class RefereeBase
   TargetTriggerChangeUi* target_trigger_change_ui_{};
   TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{};
   CameraTriggerChangeUi* camera_trigger_change_ui_{};
-  RemainBulletTimeChangeUi* remain_bullet_time_change_ui_{};
+  BulletTimeChangeUi* bullet_time_change_ui_{};
 
   CapacitorTimeChangeUi* capacitor_time_change_ui_{};
   EffortTimeChangeUi* effort_time_change_ui_{};

rm_referee/include/rm_referee/ui/time_change_ui.h

@@ -274,16 +274,17 @@ class JointPositionTimeChangeUi : public TimeChangeUi
   double max_val_, min_val_, current_val_, length_;
 };
 
-class RemainBulletTimeChangeUi : public TimeChangeUi
+class BulletTimeChangeUi : public TimeChangeUi
 {
 public:
-  explicit RemainBulletTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
-                                    std::deque<Graph>* character_queue)
+  explicit BulletTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                              std::deque<Graph>* character_queue)
     : TimeChangeUi(rpc_value, base, "remaining_bullet", graph_queue, character_queue){};
   void updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time);
+  void reset();
 
 private:
   void updateConfig() override;
-  int bullet_allowance_num_17_mm_, bullet_allowance_num_42_mm_;
+  int bullet_allowance_num_17_mm_, bullet_allowance_num_42_mm_, bullet_num_17_mm_{ 0 }, bullet_num_42_mm_{ 0 };
 };
 }  // namespace rm_referee

rm_referee/include/rm_referee/ui/ui_base.h

@@ -108,11 +108,14 @@ class FixedUi : public GroupUiBase
   {
     for (int i = 0; i < static_cast<int>(rpc_value.size()); i++)
     {
-      if (rpc_value[i]["config"].hasMember("type") || rpc_value[i]["config"]["type"] != "string")
-        graph_vector_.insert(
+      if (rpc_value[i]["config"]["type"] == "string")
+      {
+        ROS_INFO_STREAM("string FixedUi:" << rpc_value[i]["name"]);
+        character_vector_.insert(
             std::pair<std::string, Graph*>(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++)));
+      }
       else
-        character_vector_.insert(
+        graph_vector_.insert(
             std::pair<std::string, Graph*>(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++)));
     }
   };

rm_referee/src/referee_base.cpp

@@ -97,9 +97,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       if (rpc_value[i]["name"] == "engineer_joint3")
         engineer_joint3_time_change_ui =
             new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint3");
-      if (rpc_value[i]["name"] == "remaining_bullet")
-        remain_bullet_time_change_ui_ =
-            new RemainBulletTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "bullet")
+        bullet_time_change_ui_ = new BulletTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
     }
 
     ui_nh.getParam("fixed", rpc_value);
@@ -175,8 +174,11 @@ void RefereeBase::addUi()
     engineer_joint2_time_change_ui->addForQueue();
   if (engineer_joint3_time_change_ui)
     engineer_joint3_time_change_ui->addForQueue();
-  if (remain_bullet_time_change_ui_)
-    remain_bullet_time_change_ui_->addForQueue();
+  if (bullet_time_change_ui_)
+  {
+    bullet_time_change_ui_->reset();
+    bullet_time_change_ui_->addForQueue();
+  }
   add_ui_times_++;
 }
 
@@ -304,8 +306,8 @@ void RefereeBase::robotHurtDataCallBack(const rm_msgs::RobotHurt& data, const ro
 void RefereeBase::bulletRemainDataCallBack(const rm_msgs::BulletAllowance& bullet_allowance,
                                            const ros::Time& last_get_data_time)
 {
-  if (remain_bullet_time_change_ui_ && !is_adding_)
-    remain_bullet_time_change_ui_->updateBulletData(bullet_allowance, last_get_data_time);
+  if (bullet_time_change_ui_ && !is_adding_)
+    bullet_time_change_ui_->updateBulletData(bullet_allowance, last_get_data_time);
 }
 void RefereeBase::interactiveDataCallBack(const rm_referee::InteractiveData& data, const ros::Time& last_get_data_time)
 {

rm_referee/src/ui/time_change_ui.cpp

@@ -214,12 +214,14 @@ void LaneLineTimeChangeGroupUi::updateConfig()
 
 void LaneLineTimeChangeGroupUi::updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time)
 {
-  if (!tf_buffer_.canTransform(reference_frame_, "odom", ros::Time(0)))
+  if (!tf_buffer_.canTransform(reference_frame_, "yaw", ros::Time(0)))
     return;
   try
   {
     double roll, pitch, yaw;
-    quatToRPY(tf_buffer_.lookupTransform(reference_frame_, "odom", ros::Time(0)).transform.rotation, roll, pitch, yaw);
+    quatToRPY(tf_buffer_.lookupTransform(reference_frame_, "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
+    tf2::Quaternion pitch_ori_rot;
+    pitch_ori_rot.setRPY(0, 0, yaw);
     pitch_angle_ = pitch;
   }
   catch (tf2::TransformException& ex)
@@ -322,19 +324,29 @@ void JointPositionTimeChangeUi::updateJointStateData(const sensor_msgs::JointSta
   updateForQueue();
 }
 
-void RemainBulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time)
+void BulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data, const ros::Time& time)
 {
+  if (bullet_allowance_num_17_mm_ > data.bullet_allowance_num_17_mm && data.bullet_allowance_num_17_mm >= 0)
+    bullet_num_17_mm_ += (bullet_allowance_num_17_mm_ - data.bullet_allowance_num_17_mm);
+  if (bullet_allowance_num_42_mm_ > data.bullet_allowance_num_42_mm && data.bullet_allowance_num_42_mm >= 0)
+    bullet_num_42_mm_ += (bullet_allowance_num_42_mm_ - data.bullet_allowance_num_42_mm);
   bullet_allowance_num_17_mm_ = data.bullet_allowance_num_17_mm;
   bullet_allowance_num_42_mm_ = data.bullet_allowance_num_42_mm;
   updateForQueue();
 }
 
-void RemainBulletTimeChangeUi::updateConfig()
+void BulletTimeChangeUi::reset()
+{
+  bullet_num_17_mm_ = 0;
+  bullet_num_42_mm_ = 0;
+}
+
+void BulletTimeChangeUi::updateConfig()
 {
   std::string bullet_allowance_num;
   if (base_.robot_id_ == RED_HERO || base_.robot_id_ == BLUE_HERO)
   {
-    graph_->setRadius(bullet_allowance_num_42_mm_);
+    graph_->setRadius(bullet_num_42_mm_);
     if (bullet_allowance_num_42_mm_ > 5)
       graph_->setColor(rm_referee::GraphColor::GREEN);
     else if (bullet_allowance_num_42_mm_ < 3)
@@ -344,15 +356,14 @@ void RemainBulletTimeChangeUi::updateConfig()
   }
   else
   {
-    graph_->setRadius(bullet_allowance_num_17_mm_);  // TODO:need use uint32, now only < 1024
+    graph_->setRadius(bullet_num_17_mm_);  // TODO:need use uint32, now only < 1024
     if (bullet_allowance_num_17_mm_ > 50)
       graph_->setColor(rm_referee::GraphColor::GREEN);
     else if (bullet_allowance_num_17_mm_ < 10)
       graph_->setColor(rm_referee::GraphColor::PINK);
     else
       graph_->setColor(rm_referee::GraphColor::YELLOW);
   }
-
   graph_->setColor(rm_referee::GraphColor::YELLOW);
 }
 

rm_referee/src/ui/ui_base.cpp

@@ -61,6 +61,12 @@ void GroupUiBase::addForQueue(int add_times)
       graph_queue_->push_back(*graph.second);
       last_send_ = ros::Time::now();
     }
+    for (auto graph : character_vector_)
+    {
+      graph.second->setOperation(rm_referee::GraphOperation::ADD);
+      character_queue_->push_back(*graph.second);
+      last_send_ = ros::Time::now();
+    }
   }
 }
 
@@ -441,6 +447,8 @@ void FixedUi::updateForQueue()
   {
     for (auto it : graph_vector_)
       it.second->updateLastConfig();
+    for (auto it : character_vector_)
+      it.second->updateLastConfig();
 
     if (base_.robot_id_ == 0 || base_.client_id_ == 0)
       return;