Merge branch 'rm-controls:master' into new_protocol

Author: chen-gr

rm_common/include/rm_common/decision/command_sender.h

@@ -505,6 +505,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   {
     deploy_flag_ = flag;
   }
+  bool getDeployState()
+  {
+    return deploy_flag_;
+  }
   void setSpeedDesAndWheelSpeedDes()
   {
     switch (heat_limit_->getSpeedLimit())
@@ -582,7 +586,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double total_extra_wheel_speed_{};
   double deploy_wheel_speed_{};
   bool auto_wheel_speed_ = false;
-  bool deploy_flag_{};
+  bool deploy_flag_ = false;
   rm_msgs::TrackData track_data_;
   rm_msgs::GimbalDesError gimbal_des_error_;
   rm_msgs::ShootBeforehandCmd shoot_beforehand_cmd_;
@@ -591,6 +595,24 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   uint8_t armor_type_{};
 };
 
+class UseLioCommandSender : public CommandSenderBase<std_msgs::Bool>
+{
+public:
+  explicit UseLioCommandSender(ros::NodeHandle& nh) : CommandSenderBase<std_msgs::Bool>(nh)
+  {
+  }
+
+  void setUseLio(bool flag)
+  {
+    msg_.data = flag;
+  }
+  bool getUseLio() const
+  {
+    return msg_.data;
+  }
+  void setZero() override{};
+};
+
 class BalanceCommandSender : public CommandSenderBase<std_msgs::UInt8>
 {
 public:

rm_referee/include/rm_referee/referee_base.h

@@ -66,6 +66,7 @@ class RefereeBase
   virtual void radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& data);
   virtual void customizeDisplayCmdCallBack(const std_msgs::UInt32ConstPtr& data);
   virtual void visualizeStateDataCallBack(const rm_msgs::VisualizeStateDataConstPtr& data);
+  virtual void disBase2TargetDataCallBack(const std_msgs::Float32ConstPtr& data);
 
   // send  ui
   void sendSerialDataCallback();
@@ -98,6 +99,7 @@ class RefereeBase
   ros::Subscriber shoot_cmd_sub_;
   ros::Subscriber customize_display_cmd_sub_;
   ros::Subscriber visualize_state_data_sub_;
+  ros::Subscriber dis_base2target_data_sub_;
 
   ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
   ShooterTriggerChangeUi* shooter_trigger_change_ui_{};
@@ -115,6 +117,7 @@ class RefereeBase
   RotationTimeChangeUi* rotation_time_change_ui_{};
   LaneLineTimeChangeGroupUi* lane_line_time_change_ui_{};
   BalancePitchTimeChangeGroupUi* balance_pitch_time_change_group_ui_{};
+  DistanceBaseTimeChangeUi* distance_base_time_change_ui_{};
   PitchAngleTimeChangeUi* pitch_angle_time_change_ui_{};
   ImageTransmissionAngleTimeChangeUi* image_transmission_angle_time_change_ui_{};
   JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{},

rm_referee/include/rm_referee/ui/time_change_ui.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <std_msgs/Float32.h>
+
 #include "rm_referee/ui/ui_base.h"
 
 namespace rm_referee
@@ -238,6 +240,19 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi
   double bottom_angle_;
 };
 
+class DistanceBaseTimeChangeUi : public TimeChangeUi
+{
+public:
+  explicit DistanceBaseTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
+                                    std::deque<Graph>* character_queue)
+    : TimeChangeUi(rpc_value, base, "distance", graph_queue, character_queue){};
+  void updateDistanceBaseData(const std_msgs::Float32ConstPtr data, const ros::Time& time);
+
+private:
+  void updateConfig() override;
+  double distance_base_ = 0.;
+};
+
 class PitchAngleTimeChangeUi : public TimeChangeUi
 {
 public:

rm_referee/src/referee_base.cpp

@@ -54,7 +54,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       nh.subscribe<std_msgs::UInt32>("/customize_display_ui", 1, &RefereeBase::customizeDisplayCmdCallBack, this);
   RefereeBase::visualize_state_data_sub_ =
       nh.subscribe<rm_msgs::VisualizeStateData>("/visualize_state", 1, &RefereeBase::visualizeStateDataCallBack, this);
-
+  RefereeBase::dis_base2target_data_sub_ =
+      nh.subscribe<std_msgs::Float32>("/dis_baselink2target", 1, &RefereeBase::disBase2TargetDataCallBack, this);
   XmlRpc::XmlRpcValue rpc_value;
   send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
   add_ui_frequency_ = getParam(nh, "add_ui_frequency", 5);
@@ -113,6 +114,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
       if (rpc_value[i]["name"] == "lane_line")
         lane_line_time_change_ui_ =
             new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
+      if (rpc_value[i]["name"] == "distance")
+        distance_base_time_change_ui_ =
+            new DistanceBaseTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "pitch")
         pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_);
       if (rpc_value[i]["name"] == "image_transmission")
@@ -233,6 +237,8 @@ void RefereeBase::addUi()
     lane_line_time_change_ui_->addForQueue();
   if (balance_pitch_time_change_group_ui_)
     balance_pitch_time_change_group_ui_->addForQueue();
+  if (distance_base_time_change_ui_)
+    distance_base_time_change_ui_->addForQueue();
   if (pitch_angle_time_change_ui_)
     pitch_angle_time_change_ui_->addForQueue();
   if (image_transmission_angle_time_change_ui_)
@@ -627,4 +633,10 @@ void RefereeBase::visualizeStateDataCallBack(const rm_msgs::VisualizeStateDataCo
   }
 }
 
+void RefereeBase::disBase2TargetDataCallBack(const std_msgs::Float32ConstPtr& data)
+{
+  if (distance_base_time_change_ui_ && !is_adding_)
+    distance_base_time_change_ui_->updateDistanceBaseData(data, ros::Time::now());
+}
+
 }  // namespace rm_referee

rm_referee/src/ui/time_change_ui.cpp

@@ -278,6 +278,19 @@ void BalancePitchTimeChangeGroupUi::calculatePointPosition(const rm_msgs::Balanc
   updateForQueue();
 }
 
+void DistanceBaseTimeChangeUi::updateDistanceBaseData(const std_msgs::Float32ConstPtr data, const ros::Time& time)
+{
+  distance_base_ = data->data;
+  updateForQueue();
+}
+
+void DistanceBaseTimeChangeUi::updateConfig()
+{
+  std::string distance = std::to_string(distance_base_);
+  graph_->setContent(distance);
+  graph_->setColor(rm_referee::GraphColor::ORANGE);
+}
+
 void PitchAngleTimeChangeUi::updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time)
 {
   for (unsigned int i = 0; i < data->name.size(); i++)