Merge pull request #166 from liyixin135/dbus-follow-feedforward_

Follow feedforward
rm-controls · Jul 18, 2024 · 167cde0 · 167cde0
2 parents cb20569 + 99c6bd8
commit 167cde0
Showing 1 changed file with 1 addition and 1 deletion.
diff --git 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)
   {