From 52bee6e5c62c18066c6d30d9dd041c551f18c987 Mon Sep 17 00:00:00 2001
From: 1moule <1961466188@qq.com>
Date: Tue, 28 May 2024 02:06:38 +0800
Subject: [PATCH] Receive follow vel des from cmd_chassis for feedforward.

---
 rm_chassis_controllers/src/chassis_base.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp
index bba37a5f..12cb53c9 100644
--- a/rm_chassis_controllers/src/chassis_base.cpp
+++ b/rm_chassis_controllers/src/chassis_base.cpp
@@ -198,7 +198,7 @@ void ChassisBase<T...>::follow(const ros::Time& time, const ros::Duration& perio
               roll, pitch, yaw);
     double follow_error = angles::shortest_angular_distance(yaw, 0);
     pid_follow_.computeCommand(-follow_error, period);
-    vel_cmd_.z = pid_follow_.getCurrentCmd();
+    vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des;
   }
   catch (tf2::TransformException& ex)
   {