set last*velocity variables for open loop odometry

ros-controls · Oct 9, 2023 · 3f28f8f · 3f28f8f
1 parent f62fc3a
commit 3f28f8f
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -443,10 +443,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
   if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
   {
     // store and set commands
-    const double linear_command = reference_interfaces_[0];
-    const double angular_command = reference_interfaces_[1];
+    last_linear_velocity_ = reference_interfaces_[0];
+    last_angular_velocity_ = reference_interfaces_[1];
+
     auto [traction_commands, steering_commands] =
-      odometry_.get_commands(linear_command, angular_command);
+      odometry_.get_commands(last_linear_velocity_, last_angular_velocity_);
     if (params_.front_steering)
     {
       for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)