Add target distance ui

rm_referee/include/rm_referee/common/data.h

@@ -46,6 +46,7 @@
 #include <std_msgs/Int8MultiArray.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include "rm_referee/common/protocol.h"
 
@@ -86,6 +87,7 @@
 #include "rm_msgs/SentryInfo.h"
 #include "rm_msgs/RadarInfo.h"
 #include "rm_msgs/Buff.h"
+#include "rm_msgs/TrackData.h"
 #include <rm_msgs/PowerManagementSampleAndStatusData.h>
 #include <rm_msgs/PowerManagementSystemExceptionData.h>
 #include <rm_msgs/PowerManagementInitializationExceptionData.h>

rm_referee/include/rm_referee/referee_base.h

@@ -101,6 +101,7 @@ class RefereeBase
   PitchAngleTimeChangeUi* pitch_angle_time_change_ui_{};
   JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{},
       *engineer_joint3_time_change_ui{};
+  TargetDistanceTimeChangeUi* target_distance_time_change_ui_{};
 
   FixedUi* fixed_ui_{};
 
@@ -122,5 +123,6 @@ class RefereeBase
   double send_ui_queue_delay_;
   bool add_ui_flag_ = false, is_adding_ = false;
   ros::NodeHandle nh_;
+  std::string dbus_topic_;
 };
 }  // namespace rm_referee

rm_referee/include/rm_referee/ui/time_change_ui.h

@@ -287,4 +287,17 @@ class BulletTimeChangeUi : public TimeChangeUi
   void updateConfig() override;
   int bullet_allowance_num_17_mm_, bullet_allowance_num_42_mm_, bullet_num_17_mm_{ 0 }, bullet_num_42_mm_{ 0 };
 };
+
+class TargetDistanceTimeChangeUi : public TimeChangeUi
+{
+public:
+  explicit TargetDistanceTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                      std::deque<Graph>* character_queue)
+    : TimeChangeUi(rpc_value, base, "target_distance", graph_queue, character_queue){};
+  void updateTargetDistanceData(const rm_msgs::TrackData::ConstPtr& data);
+
+private:
+  void updateConfig() override;
+  double target_distance_;
+};
 }  // namespace rm_referee

rm_referee/include/rm_referee/ui/ui_base.h

@@ -46,6 +46,7 @@ class UiBase
   void sendMapSentryData(const rm_referee::MapSentryData& data);
   void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data);
   void sendCustomInfoData(std::wstring data);
+  void transferInt(const int data);
 
   void sendSerial(const ros::Time& time, int data_len);
   void clearTxBuffer();

rm_referee/src/referee.cpp

@@ -140,7 +140,7 @@ int Referee::unpack(uint8_t* rx_data)
           game_robot_hp_data.red_base_hp = game_robot_hp_ref.red_base_hp;
           game_robot_hp_data.stamp = last_get_data_time_;
 
-          //          referee_ui_.updateEnemyHeroState(game_robot_hp_data, last_get_data_time_);
+          referee_ui_.updateEnemyHeroState(game_robot_hp_data, last_get_data_time_);
           referee_ui_.updateHeroStateDataCallBack(game_robot_hp_data, last_get_data_time_);
           game_robot_hp_pub_.publish(game_robot_hp_data);
           break;

rm_referee/src/referee_base.cpp

@@ -8,11 +8,12 @@ namespace rm_referee
 {
 RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
 {
+  dbus_topic_ = getParam(nh, "dbus_topic", std::string("/rm_ecat_hw/dbus"));
   RefereeBase::joint_state_sub_ =
       nh.subscribe<sensor_msgs::JointState>("/joint_states", 10, &RefereeBase::jointStateCallback, this);
   RefereeBase::actuator_state_sub_ =
       nh.subscribe<rm_msgs::ActuatorState>("/actuator_states", 10, &RefereeBase::actuatorStateCallback, this);
-  RefereeBase::dbus_sub_ = nh.subscribe<rm_msgs::DbusData>("/dbus_data", 10, &RefereeBase::dbusDataCallback, this);
+  RefereeBase::dbus_sub_ = nh.subscribe<rm_msgs::DbusData>(dbus_topic_, 10, &RefereeBase::dbusDataCallback, this);
   RefereeBase::chassis_cmd_sub_ =
       nh.subscribe<rm_msgs::ChassisCmd>("/cmd_chassis", 10, &RefereeBase::chassisCmdDataCallback, this);
   RefereeBase::vel2D_cmd_sub_ =
@@ -99,6 +100,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
             new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint3");
       if (rpc_value[i]["name"] == "bullet")
         bullet_time_change_ui_ = new BulletTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "target_distance")
+        target_distance_time_change_ui_ =
+            new TargetDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
     }
 
     ui_nh.getParam("fixed", rpc_value);
@@ -179,6 +183,8 @@ void RefereeBase::addUi()
     bullet_time_change_ui_->reset();
     bullet_time_change_ui_->addForQueue();
   }
+  if (target_distance_time_change_ui_)
+    target_distance_time_change_ui_->addForQueue();
   add_ui_times_++;
 }
 
@@ -410,6 +416,8 @@ void RefereeBase::trackCallBack(const rm_msgs::TrackDataConstPtr& data)
 {
   if (target_view_angle_trigger_change_ui_ && !is_adding_)
     target_view_angle_trigger_change_ui_->updateTrackID(data->id);
+  if (target_distance_time_change_ui_ && !is_adding_)
+    target_distance_time_change_ui_->updateTargetDistanceData(data);
 }
 void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data)
 {

rm_referee/src/ui/time_change_ui.cpp

@@ -326,14 +326,12 @@ void BulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data,
 {
   if (data.bullet_allowance_num_17_mm >= 0 && data.bullet_allowance_num_17_mm < 1000)
   {
-    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_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)
   {
-    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_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();
@@ -371,4 +369,33 @@ void BulletTimeChangeUi::updateConfig()
   graph_->setColor(rm_referee::GraphColor::YELLOW);
 }
 
+void TargetDistanceTimeChangeUi::updateTargetDistanceData(const rm_msgs::TrackData::ConstPtr& data)
+{
+  if (data->id == 0)
+    return;
+  geometry_msgs::PointStamped output;
+  geometry_msgs::PointStamped input;
+  input.point.x = data->position.x;
+  input.point.y = data->position.y;
+  input.point.z = data->position.z;
+  //  tf_buffer_.transform(input, output, "base_link");
+  try
+  {
+    geometry_msgs::TransformStamped transform_stamped = tf_buffer_.lookupTransform("base_link", "odom", ros::Time(0));
+    tf2::doTransform(input, output, transform_stamped);
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_ERROR("Failed to transform point: %s", ex.what());
+  }
+  target_distance_ = std::sqrt((output.point.x) * (output.point.x) + (output.point.y) * (output.point.y) +
+                               (output.point.z) * (output.point.z));
+  updateForQueue();
+}
+
+void TargetDistanceTimeChangeUi::updateConfig()
+{
+  UiBase::transferInt(std::floor(target_distance_ * 1000));
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/ui_base.cpp

@@ -459,6 +459,14 @@ void FixedUi::updateForQueue()
   }
 }
 
+void UiBase::transferInt(const int data)
+{
+  int a = data & 1023;
+  int b = data >> 10;
+  graph_->setRadius(a);
+  graph_->setEndX(b);
+}
+
 void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const
 {
   memset(tx_buffer, 0, k_frame_length_);