Steering controllers library: fix open loop mode (#793)

* set last*velocity variables for open loop odometry

* Make function arguments const

* Update function in header file too

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -184,7 +184,8 @@ class SteeringOdometry
    * \param theta_dot Desired angular velocity [rad/s]
    * \return Tuple of velocity commands and steering commands
    */
-  std::tuple<std::vector<double>, std::vector<double>> get_commands(double Vx, double theta_dot);
+  std::tuple<std::vector<double>, std::vector<double>> get_commands(
+    const double Vx, const double theta_dot);
 
   /**
    *  \brief Reset poses, heading, and accumulators

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++)

steering_controllers_library/src/steering_odometry.cpp

@@ -210,7 +210,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  double Vx, double theta_dot)
+  const double Vx, const double theta_dot)
 {
   // desired velocity and steering angle of the middle of traction and steering axis
   double Ws, alpha;