fix(ackermann): handle NaN/Inf values in odometry update

steering_controllers_library/src/steering_controllers_library.cpp

@@ -14,6 +14,7 @@
 
 #include "steering_controllers_library/steering_controllers_library.hpp"
 
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <string>
@@ -520,11 +521,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
 {
   auto logger = get_node()->get_logger();
 
-  // store current ref (for open loop odometry) and update odometry
-  last_linear_velocity_ = reference_interfaces_[0];
-  last_angular_velocity_ = reference_interfaces_[1];
-  update_odometry(period);
-
   // MOVE ROBOT
 
   // Limit velocities and accelerations:
@@ -661,19 +657,36 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
         RCLCPP_DEBUG(
           logger, "Unable to retrieve %s for steering wheel %zu",
           !state_interface_value_op.has_value() ? "state interface value"
-                                                : "command interface value",
-          i);
+                                                : "command interface value", i);
       }
       else
       {
         controller_state_msg_.steer_positions.push_back(state_interface_value_op.value());
         controller_state_msg_.steering_angle_command.push_back(command_interface_value_op.value());
       }
     }
-
     controller_state_publisher_->try_publish(controller_state_msg_);
   }
 
+  // store current ref (for open loop odometry) and update odometry
+  if (std::isfinite(reference_interfaces_[0]))
+  {
+    last_linear_velocity_ = reference_interfaces_[0];
+  }
+  else
+  {
+    last_linear_velocity_ = 0.0;
+  }
+  if (std::isfinite(reference_interfaces_[1]))
+  {
+    last_angular_velocity_ = reference_interfaces_[1];
+  }
+  else
+  {
+    last_angular_velocity_ = 0.0;
+  }
+  update_odometry(period);
+
   reference_interfaces_[0] = std::numeric_limits<double>::quiet_NaN();
   reference_interfaces_[1] = std::numeric_limits<double>::quiet_NaN();
 

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -268,6 +268,9 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout)
   msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z;
   controller_->input_ref_.set(msg);
 
+  EXPECT_GT(controller_->command_interfaces_[0].get_optional().value(), 0.0);
+  EXPECT_GT(controller_->command_interfaces_[1].get_optional().value(), 0.0);
+
   // age_of_last_command > ref_timeout_
   ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
   ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X);
@@ -363,6 +366,9 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout)
 
   age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp;
 
+  EXPECT_GT(controller_->command_interfaces_[0].get_optional().value(), 0.0);
+  EXPECT_GT(controller_->command_interfaces_[1].get_optional().value(), 0.0);
+
   // age_of_last_command > ref_timeout_
   ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
   ASSERT_EQ(controller_->input_ref_.get().twist.linear.x, TEST_LINEAR_VELOCITY_X);
@@ -386,6 +392,89 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout)
   EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6);
 }
 
+TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
+{
+  // Setup Options
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("open_loop", true);
+  node_options.append_parameter_override(
+    "traction_joints_names", std::vector<std::string>{"wheel_left", "wheel_right"});
+  node_options.append_parameter_override(
+    "steering_joints_names", std::vector<std::string>{"steer_left", "steer_right"});
+  SetUpController("test_steering_controllers_library", node_options);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->set_chained_mode(false);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  struct Publicist : public TestableSteeringControllersLibrary
+  {
+    using controller_interface::ControllerInterfaceBase::command_interfaces_;
+    using steering_controllers_library::SteeringControllersLibrary::input_ref_;
+  };
+  auto * pub_controller = static_cast<Publicist *>(controller_.get());
+
+  auto command_msg = ControllerReferenceMsg();
+  command_msg.header.stamp = controller_->get_node()->now();
+  command_msg.twist.linear.x = 1.5;
+  command_msg.twist.angular.z = 0.0;
+
+  pub_controller->input_ref_.set(command_msg);
+
+  controller_->update_reference_from_subscribers(
+    controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
+  controller_->update_and_write_commands(
+    controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
+
+  ASSERT_GT(pub_controller->command_interfaces_[0].get_optional().value(), 0.1);
+
+  auto nan_msg = ControllerReferenceMsg();
+  nan_msg.header.stamp = controller_->get_node()->now();
+  nan_msg.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+  nan_msg.twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+
+  pub_controller->input_ref_.set(nan_msg);
+
+  controller_->update_reference_from_subscribers(
+    controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
+  controller_->update_and_write_commands(
+    controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
+
+  // The wheel speed should have been reset to 0.0
+  EXPECT_DOUBLE_EQ(pub_controller->command_interfaces_[0].get_optional().value(), 0.0);
+}
+
+TEST_F(SteeringControllersLibraryTest, test_open_loop_update_timeout)
+{
+  // 1. SETUP WITH OPTIONS
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("open_loop", true);
+  node_options.append_parameter_override("reference_timeout", 1.0);
+
+  SetUpController("test_steering_controllers_library", node_options);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->set_chained_mode(false);  // We are testing standalone mode
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  ControllerReferenceMsg msg;
+  msg.header.stamp = controller_->get_node()->now();
+  msg.twist.linear.x = 5.0;
+  msg.twist.angular.z = 0.0;
+  controller_->input_ref_.set(msg);
+
+  controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.1));
+
+  EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 5.0);
+
+  rclcpp::Time future_time = controller_->get_node()->now() + rclcpp::Duration::from_seconds(2.0);
+
+  controller_->update(future_time, rclcpp::Duration::from_seconds(0.1));
+
+  EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0);
+
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

steering_controllers_library/test/test_steering_controllers_library.hpp

@@ -76,6 +76,7 @@ class TestableSteeringControllersLibrary
   FRIEND_TEST(SteeringControllersLibraryTest, configure_succeeds_tf_tilde_prefix_set_namespace);
   FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout);
   FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout);
+  FRIEND_TEST(SteeringControllersLibraryTest, test_open_loop_update_timeout);
 
 public:
   controller_interface::CallbackReturn on_configure(