Merge pull request #225 from liyixin135/pr

Add GimbalPosState.msg and change chassis' direction
rm-controls · May 8, 2024 · 5411775 · 5411775
2 parents 1d9e8d0 + 8d0f8e2
commit 5411775
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 2 deletions.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -161,10 +161,15 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
       max_angular_z_.init(xml_rpc_value);
     std::string topic;
     nh.getParam("power_limit_topic", topic);
+    target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.);
     chassis_power_limit_subscriber_ =
         nh.subscribe<rm_msgs::ChassisCmd>(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this);
   }
 
+  void updateTrackData(const rm_msgs::TrackData& data)
+  {
+    track_data_ = data;
+  }
   void setLinearXVel(double scale)
   {
     msg_.linear.x = scale * max_linear_x_.output(power_limit_);
@@ -175,7 +180,11 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
   };
   void setAngularZVel(double scale)
   {
-    msg_.angular.z = scale * max_angular_z_.output(power_limit_);
+    if (track_data_.v_yaw > target_vel_yaw_threshold_)
+      vel_direction_ = -1.;
+    if (track_data_.v_yaw < -target_vel_yaw_threshold_)
+      vel_direction_ = 1.;
+    msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_;
   };
   void set2DVel(double scale_x, double scale_y, double scale_z)
   {
@@ -197,8 +206,11 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
   }
 
   LinearInterp max_linear_x_, max_linear_y_, max_angular_z_;
-  double power_limit_ = 0;
+  double power_limit_ = 0.;
+  double target_vel_yaw_threshold_{};
+  double vel_direction_ = 1.;
   ros::Subscriber chassis_power_limit_subscriber_;
+  rm_msgs::TrackData track_data_;
 };
 
 class ChassisCommandSender : public TimeStampCommandSenderBase<rm_msgs::ChassisCmd>

diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt
@@ -20,6 +20,7 @@ add_message_files(
         ShootBeforehandCmd.msg
         GimbalCmd.msg
         GimbalDesError.msg
+        GimbalPosState.msg
         LpData.msg
         KalmanData.msg
         MovingAverageData.msg

diff --git a/rm_msgs/msg/GimbalPosState.msg b/rm_msgs/msg/GimbalPosState.msg
@@ -0,0 +1,6 @@
+std_msgs/Header header
+float64 set_point
+float64 set_point_dot
+float64 process_value
+float64 error
+float64 command