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,8 +521,22 @@ 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];
+  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);
 
   // MOVE ROBOT

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -386,6 +386,90 @@ 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));
+
+  double wheel_speed_1 = pub_controller->command_interfaces_[0].get_optional().value();
+
+  ASSERT_GT(wheel_speed_1, 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 stay exactly the same
+  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(