Fix open-loop odometry in case of ref timeout (#1454)

Co-authored-by: Dr. Denis <[email protected]>

Author: christophfroehlich

steering_controllers_library/src/steering_controllers_library.cpp

@@ -378,23 +378,23 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
 
   if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
   {
-    // store (for open loop odometry) and set commands
-    last_linear_velocity_ = reference_interfaces_[0];
-    last_angular_velocity_ = reference_interfaces_[1];
-
     const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp;
     const auto timeout =
       age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0);
 
+    // store (for open loop odometry) and set commands
+    last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0];
+    last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1];
+
     auto [traction_commands, steering_commands] = odometry_.get_commands(
-      last_linear_velocity_, last_angular_velocity_, params_.open_loop,
+      reference_interfaces_[0], reference_interfaces_[1], params_.open_loop,
       params_.reduce_wheel_speed_until_steering_reached);
 
     if (params_.front_steering)
     {
       for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
       {
-        command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]);
+        command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]);
       }
       for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
       {
@@ -406,7 +406,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
       {
         for (size_t i = 0; i < params_.front_wheels_names.size(); i++)
         {
-          command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]);
+          command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]);
         }
         for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)
         {