From 2114d4f6c641322bca47c51a5659f8679b50bffa Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Sat, 27 Dec 2025 12:50:55 +0530
Subject: [PATCH 01/12] fix(ackermann): handle NaN/Inf values in odometry
 update

This adds 'try_update' methods to SteeringKinematics and SteeringOdometry to safely handle cases where sensors return NaN or Infinite values.

Previously, invalid sensor data caused the update loop to compute invalid odometry, potentially destabilizing the controller.

Updates:
- Added try_update_from_position/velocity to library
- Updated Ackermann controller to use safe update methods

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/ackermann_steering_controller.cpp     | 22 ++++++++++---
 .../steering_kinematics.hpp                   | 10 ++++++
 .../src/steering_kinematics.cpp               | 33 +++++++++++++++++++
 .../src/steering_odometry.cpp                 | 33 +++++++++++++++++++
 4 files changed, 93 insertions(+), 5 deletions(-)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index eccfeb3977..1bd41bd734 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -59,7 +59,11 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
 
   if (params_.open_loop)
   {
-    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
+    if (!odometry_.try_update_open_loop(
+          last_linear_velocity_, last_angular_velocity_, period.seconds()))
+    {
+      return false;
+    }
   }
   else
   {
@@ -89,23 +93,31 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
     const double steering_left_position = steering_left_position_op.value();
 
     if (
-      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
-      std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
+      !std::isfinite(traction_right_wheel_value) || !std::isfinite(traction_left_wheel_value) ||
+      !std::isfinite(steering_right_position) || !std::isfinite(steering_left_position))
     {
+      RCLCPP_DEBUG(logger, "Odometery failed! : sensors returning infinity or NaN!");
+      return false;
+    }
+    else
+    {
+      bool success = false;
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_position(
+        success = odometry_.try_update_from_position(
           traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
           steering_left_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_velocity(
+        success = odometry_.try_update_from_velocity(
           traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
           steering_left_position, period.seconds());
       }
+
+      return success;
     }
   }
   return true;
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
index 5021f7342f..4bb23e5711 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
@@ -139,6 +139,16 @@ class SteeringKinematics
    */
   void update_open_loop(const double v_bx, const double omega_bz, const double dt);
 
+  bool try_update_open_loop(double linear, double angular, double delTime);
+
+  bool try_update_from_position(
+    double right_traction, double left_traction, double right_steering, double left_steering,
+    double delTime);
+
+  bool try_update_from_velocity(
+    double right_traction, double left_traction, double right_steering, double left_steering,
+    double delTime);
+
   /**
    * \brief Set odometry type
    * \param type odometry type
diff --git a/steering_controllers_library/src/steering_kinematics.cpp b/steering_controllers_library/src/steering_kinematics.cpp
index 2d5e6640b6..0633007c2e 100644
--- a/steering_controllers_library/src/steering_kinematics.cpp
+++ b/steering_controllers_library/src/steering_kinematics.cpp
@@ -394,4 +394,37 @@ void SteeringKinematics::reset_accumulators()
   angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
 }
 
+bool SteeringKinematics::try_update_open_loop(double linear, double angular, double delTime)
+{
+  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
+  update_open_loop(linear, angular, delTime);
+  return true;
+}
+
+bool SteeringKinematics::try_update_from_position(
+  double right_traction, double left_traction, double right_steering, double left_steering,
+  double delTime)
+{
+  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
+  if (
+    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
+    !std::isfinite(right_steering) || !std::isfinite(left_steering))
+    return false;
+  update_from_position(right_traction, left_traction, right_steering, left_steering, delTime);
+  return true;
+}
+
+bool SteeringKinematics::try_update_from_velocity(
+  double right_traction, double left_traction, double right_steering, double left_steering,
+  double delTime)
+{
+  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
+  if (
+    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
+    !std::isfinite(right_steering) || !std::isfinite(left_steering))
+    return false;
+  update_from_velocity(right_traction, left_traction, right_steering, left_steering, delTime);
+  return true;
+}
+
 }  // namespace steering_kinematics
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index f668230d5a..a015a718f3 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -110,4 +110,37 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
 
 void SteeringOdometry::reset_odometry() { sk_impl_.reset_odometry(); }
 
+bool SteeringOdometry::try_update_open_loop(double linear, double angular, double delTime)
+{
+  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
+  update_open_loop(linear, angular, delTime);
+  return true;
+}
+
+bool SteeringOdometry::try_update_from_position(
+  double right_traction, double left_traction, double right_steering, double left_steering,
+  double delTime)
+{
+  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
+  if (
+    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
+    !std::isfinite(right_steering) || !std::isfinite(left_steering))
+    return false;
+  update_from_position(right_traction, left_traction, right_steering, left_steering, delTime);
+  return true;
+}
+
+bool SteeringOdometry::try_update_from_velocity(
+  double right_traction, double left_traction, double right_steering, double left_steering,
+  double delTime)
+{
+  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
+  if (
+    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
+    !std::isfinite(right_steering) || !std::isfinite(left_steering))
+    return false;
+  update_from_velocity(right_traction, left_traction, right_steering, left_steering, delTime);
+  return true;
+}
+
 }  // namespace steering_odometry

From 0c0ebb4204df112febab469b620603859da1a61c Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Sat, 27 Dec 2025 19:28:21 +0530
Subject: [PATCH 02/12] fix: move try_update declarations inside
 SteeringOdometry class

---
 .../steering_controllers_library/steering_odometry.hpp      | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
index 9fa95dd45e..666fb1d7de 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -116,6 +116,12 @@ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] Stee
 
   void reset_odometry();
 
+  bool try_update_open_loop(double linear, double angular, double delTime);
+
+  bool try_update_from_position(double right_traction, double left_traction, double right_steering, double left_steering, double delTime);
+
+  bool try_update_from_velocity(double right_traction, double left_traction, double right_steering, double left_steering, double delTime);
+
 private:
   steering_kinematics::SteeringKinematics sk_impl_;
 };

From 854944724ee15d5b23e0e7264885c36cc57091de Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Sun, 28 Dec 2025 01:37:41 +0530
Subject: [PATCH 03/12] fix(ackermann): add NaN checks to library and safe
 open-loop update

- Refactored update_from_position/velocity to handle NaN checks internally per review.
- Added try_update_open_loop to allow error prpagation without breaking ABI.
- Updated Ackermann controller to use safe library methods.
---
 .../src/ackermann_steering_controller.cpp     | 12 ++-
 .../steering_kinematics.hpp                   |  8 --
 .../steering_odometry.hpp                     |  4 -
 .../src/steering_kinematics.cpp               | 73 ++++++++++---------
 .../src/steering_odometry.cpp                 | 51 +++++--------
 5 files changed, 61 insertions(+), 87 deletions(-)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index 1bd41bd734..2ddfa90ebe 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -59,11 +59,9 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
 
   if (params_.open_loop)
   {
-    if (!odometry_.try_update_open_loop(
-          last_linear_velocity_, last_angular_velocity_, period.seconds()))
-    {
-      return false;
-    }
+    if(!odometry_.try_update_open_loop(
+          last_linear_velocity_, last_angular_velocity_, period.seconds()
+        ))return false;
   }
   else
   {
@@ -105,14 +103,14 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
-        success = odometry_.try_update_from_position(
+        success = odometry_.update_from_position(
           traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
           steering_left_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
-        success = odometry_.try_update_from_velocity(
+        success = odometry_.update_from_velocity(
           traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
           steering_left_position, period.seconds());
       }
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
index 4bb23e5711..c3f2f64aa2 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
@@ -141,14 +141,6 @@ class SteeringKinematics
 
   bool try_update_open_loop(double linear, double angular, double delTime);
 
-  bool try_update_from_position(
-    double right_traction, double left_traction, double right_steering, double left_steering,
-    double delTime);
-
-  bool try_update_from_velocity(
-    double right_traction, double left_traction, double right_steering, double left_steering,
-    double delTime);
-
   /**
    * \brief Set odometry type
    * \param type odometry type
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
index 666fb1d7de..4ac04e662f 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -118,10 +118,6 @@ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] Stee
 
   bool try_update_open_loop(double linear, double angular, double delTime);
 
-  bool try_update_from_position(double right_traction, double left_traction, double right_steering, double left_steering, double delTime);
-
-  bool try_update_from_velocity(double right_traction, double left_traction, double right_steering, double left_steering, double delTime);
-
 private:
   steering_kinematics::SteeringKinematics sk_impl_;
 };
diff --git a/steering_controllers_library/src/steering_kinematics.cpp b/steering_controllers_library/src/steering_kinematics.cpp
index 0633007c2e..6ded2827ef 100644
--- a/steering_controllers_library/src/steering_kinematics.cpp
+++ b/steering_controllers_library/src/steering_kinematics.cpp
@@ -78,6 +78,9 @@ bool SteeringKinematics::update_odometry(
 bool SteeringKinematics::update_from_position(
   const double traction_wheel_pos, const double steer_pos, const double dt)
 {
+
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_pos) || !std::isfinite(steer_pos)) return false;
+
   const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_;
 
   /// Update old position with current:
@@ -90,6 +93,11 @@ bool SteeringKinematics::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double steer_pos, const double dt)
 {
+
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) 
+    || !std::isfinite(traction_left_wheel_pos) || !std::isfinite(steer_pos)) 
+    return false;
+
   const double traction_right_wheel_est_pos_diff =
     traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
@@ -107,6 +115,11 @@ bool SteeringKinematics::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
+
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) 
+    || !std::isfinite(traction_left_wheel_pos) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) 
+    return false;
+
   const double traction_right_wheel_est_pos_diff =
     traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
@@ -124,6 +137,9 @@ bool SteeringKinematics::update_from_position(
 bool SteeringKinematics::update_from_velocity(
   const double traction_wheel_vel, const double steer_pos, const double dt)
 {
+
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
+
   steer_pos_ = steer_pos;
   double linear_velocity = traction_wheel_vel * wheel_radius_;
   const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_;
@@ -156,6 +172,9 @@ bool SteeringKinematics::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double steer_pos, const double dt)
 {
+
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) || !std::isfinite(left_traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
+
   steer_pos_ = steer_pos;
   double linear_velocity = get_linear_velocity_double_traction_axle(
     right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
@@ -169,6 +188,11 @@ bool SteeringKinematics::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
+
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) || 
+    !std::isfinite(left_traction_wheel_vel) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
+
+  
   // overdetermined, we take the average
   const double right_steer_pos_est = std::atan(
     wheel_base_ * std::tan(right_steer_pos) /
@@ -187,14 +211,29 @@ bool SteeringKinematics::update_from_velocity(
 
 void SteeringKinematics::update_open_loop(const double v_bx, const double omega_bz, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return;
   /// Save last linear and angular velocity:
   linear_ = v_bx;
+
   angular_ = omega_bz;
 
   /// Integrate odometry:
   integrate_fk(v_bx, omega_bz, dt);
 }
 
+bool SteeringKinematics::try_update_open_loop(const double v_bx, const double omega_bz, const double dt)
+{
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return false;
+  /// Save last linear and angular velocity:
+  linear_ = v_bx;
+
+  angular_ = omega_bz;
+
+  /// Integrate odometry:
+  integrate_fk(v_bx, omega_bz, dt);
+  return true;
+}
+
 void SteeringKinematics::set_wheel_params(
   double wheel_radius, double wheel_base, double wheel_track)
 {
@@ -393,38 +432,4 @@ void SteeringKinematics::reset_accumulators()
   linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
   angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
 }
-
-bool SteeringKinematics::try_update_open_loop(double linear, double angular, double delTime)
-{
-  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
-  update_open_loop(linear, angular, delTime);
-  return true;
-}
-
-bool SteeringKinematics::try_update_from_position(
-  double right_traction, double left_traction, double right_steering, double left_steering,
-  double delTime)
-{
-  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
-  if (
-    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
-    !std::isfinite(right_steering) || !std::isfinite(left_steering))
-    return false;
-  update_from_position(right_traction, left_traction, right_steering, left_steering, delTime);
-  return true;
-}
-
-bool SteeringKinematics::try_update_from_velocity(
-  double right_traction, double left_traction, double right_steering, double left_steering,
-  double delTime)
-{
-  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
-  if (
-    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
-    !std::isfinite(right_steering) || !std::isfinite(left_steering))
-    return false;
-  update_from_velocity(right_traction, left_traction, right_steering, left_steering, delTime);
-  return true;
-}
-
 }  // namespace steering_kinematics
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index a015a718f3..c139e2df6e 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -33,6 +33,7 @@ void SteeringOdometry::init(const rclcpp::Time & time) { sk_impl_.init(time); }
 bool SteeringOdometry::update_from_position(
   const double traction_wheel_pos, const double steer_pos, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_pos) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_position(traction_wheel_pos, steer_pos, dt);
 }
 
@@ -40,6 +41,8 @@ bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double steer_pos, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) ||
+     !std::isfinite(traction_left_wheel_pos) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_position(
     traction_right_wheel_pos, traction_left_wheel_pos, steer_pos, dt);
 }
@@ -48,6 +51,8 @@ bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) ||
+     !std::isfinite(traction_left_wheel_pos) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
   return sk_impl_.update_from_position(
     traction_right_wheel_pos, traction_left_wheel_pos, right_steer_pos, left_steer_pos, dt);
 }
@@ -55,6 +60,7 @@ bool SteeringOdometry::update_from_position(
 bool SteeringOdometry::update_from_velocity(
   const double traction_wheel_vel, const double steer_pos, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_velocity(traction_wheel_vel, steer_pos, dt);
 }
 
@@ -62,6 +68,8 @@ bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double steer_pos, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
+     !std::isfinite(left_traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_velocity(
     right_traction_wheel_vel, left_traction_wheel_vel, steer_pos, dt);
 }
@@ -70,15 +78,24 @@ bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
+     !std::isfinite(left_traction_wheel_vel) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
   return sk_impl_.update_from_velocity(
     right_traction_wheel_vel, left_traction_wheel_vel, right_steer_pos, left_steer_pos, dt);
 }
 
 void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt)
 {
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return;
   sk_impl_.update_open_loop(v_bx, omega_bz, dt);
 }
 
+bool SteeringOdometry::try_update_open_loop(const double v_bx, const double omega_bz, const double dt){
+  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return false;
+  sk_impl_.try_update_open_loop(v_bx, omega_bz, dt);
+  return true;
+}
+
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track)
 {
   sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track);
@@ -109,38 +126,4 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
 }
 
 void SteeringOdometry::reset_odometry() { sk_impl_.reset_odometry(); }
-
-bool SteeringOdometry::try_update_open_loop(double linear, double angular, double delTime)
-{
-  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
-  update_open_loop(linear, angular, delTime);
-  return true;
-}
-
-bool SteeringOdometry::try_update_from_position(
-  double right_traction, double left_traction, double right_steering, double left_steering,
-  double delTime)
-{
-  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
-  if (
-    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
-    !std::isfinite(right_steering) || !std::isfinite(left_steering))
-    return false;
-  update_from_position(right_traction, left_traction, right_steering, left_steering, delTime);
-  return true;
-}
-
-bool SteeringOdometry::try_update_from_velocity(
-  double right_traction, double left_traction, double right_steering, double left_steering,
-  double delTime)
-{
-  if (std::fabs(delTime) < std::numeric_limits<double>::epsilon()) return false;
-  if (
-    !std::isfinite(right_traction) || !std::isfinite(left_traction) ||
-    !std::isfinite(right_steering) || !std::isfinite(left_steering))
-    return false;
-  update_from_velocity(right_traction, left_traction, right_steering, left_steering, delTime);
-  return true;
-}
-
 }  // namespace steering_odometry

From c1b228133c00a08042343da923fa5d32c6fb8b60 Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Thu, 1 Jan 2026 17:46:56 +0530
Subject: [PATCH 04/12] Fix: Prevent invalid odometry updates on NaN input

- Added isfinite check in update_and_write_commands to protect last_linear_velocity_
- Added regression test confirming robot stops safely on NaN input

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/ackermann_steering_controller.cpp     | 20 ++----
 .../steering_kinematics.hpp                   |  2 -
 .../steering_odometry.hpp                     |  2 -
 .../src/steering_controllers_library.cpp      | 10 ++-
 .../src/steering_kinematics.cpp               | 40 +----------
 .../src/steering_odometry.cpp                 | 18 +----
 .../test_steering_controllers_library.cpp     | 66 +++++++++++++++++++
 7 files changed, 81 insertions(+), 77 deletions(-)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index 2ddfa90ebe..eccfeb3977 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -59,9 +59,7 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
 
   if (params_.open_loop)
   {
-    if(!odometry_.try_update_open_loop(
-          last_linear_velocity_, last_angular_velocity_, period.seconds()
-        ))return false;
+    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
   }
   else
   {
@@ -91,31 +89,23 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
     const double steering_left_position = steering_left_position_op.value();
 
     if (
-      !std::isfinite(traction_right_wheel_value) || !std::isfinite(traction_left_wheel_value) ||
-      !std::isfinite(steering_right_position) || !std::isfinite(steering_left_position))
+      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+      std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
     {
-      RCLCPP_DEBUG(logger, "Odometery failed! : sensors returning infinity or NaN!");
-      return false;
-    }
-    else
-    {
-      bool success = false;
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
-        success = odometry_.update_from_position(
+        odometry_.update_from_position(
           traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
           steering_left_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
-        success = odometry_.update_from_velocity(
+        odometry_.update_from_velocity(
           traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
           steering_left_position, period.seconds());
       }
-
-      return success;
     }
   }
   return true;
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
index c3f2f64aa2..5021f7342f 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp
@@ -139,8 +139,6 @@ class SteeringKinematics
    */
   void update_open_loop(const double v_bx, const double omega_bz, const double dt);
 
-  bool try_update_open_loop(double linear, double angular, double delTime);
-
   /**
    * \brief Set odometry type
    * \param type odometry type
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
index 4ac04e662f..9fa95dd45e 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -116,8 +116,6 @@ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] Stee
 
   void reset_odometry();
 
-  bool try_update_open_loop(double linear, double angular, double delTime);
-
 private:
   steering_kinematics::SteeringKinematics sk_impl_;
 };
diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index beae04762e..4b1bcb3058 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -520,8 +520,14 @@ 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];
+  }
+  if (std::isfinite(reference_interfaces_[1]))
+  {
+    last_angular_velocity_ = reference_interfaces_[1];
+  }
   update_odometry(period);
 
   // MOVE ROBOT
diff --git a/steering_controllers_library/src/steering_kinematics.cpp b/steering_controllers_library/src/steering_kinematics.cpp
index 6ded2827ef..2d5e6640b6 100644
--- a/steering_controllers_library/src/steering_kinematics.cpp
+++ b/steering_controllers_library/src/steering_kinematics.cpp
@@ -78,9 +78,6 @@ bool SteeringKinematics::update_odometry(
 bool SteeringKinematics::update_from_position(
   const double traction_wheel_pos, const double steer_pos, const double dt)
 {
-
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_pos) || !std::isfinite(steer_pos)) return false;
-
   const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_;
 
   /// Update old position with current:
@@ -93,11 +90,6 @@ bool SteeringKinematics::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double steer_pos, const double dt)
 {
-
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) 
-    || !std::isfinite(traction_left_wheel_pos) || !std::isfinite(steer_pos)) 
-    return false;
-
   const double traction_right_wheel_est_pos_diff =
     traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
@@ -115,11 +107,6 @@ bool SteeringKinematics::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) 
-    || !std::isfinite(traction_left_wheel_pos) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) 
-    return false;
-
   const double traction_right_wheel_est_pos_diff =
     traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
@@ -137,9 +124,6 @@ bool SteeringKinematics::update_from_position(
 bool SteeringKinematics::update_from_velocity(
   const double traction_wheel_vel, const double steer_pos, const double dt)
 {
-
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
-
   steer_pos_ = steer_pos;
   double linear_velocity = traction_wheel_vel * wheel_radius_;
   const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_;
@@ -172,9 +156,6 @@ bool SteeringKinematics::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double steer_pos, const double dt)
 {
-
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) || !std::isfinite(left_traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
-
   steer_pos_ = steer_pos;
   double linear_velocity = get_linear_velocity_double_traction_axle(
     right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
@@ -188,11 +169,6 @@ bool SteeringKinematics::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) || 
-    !std::isfinite(left_traction_wheel_vel) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
-
-  
   // overdetermined, we take the average
   const double right_steer_pos_est = std::atan(
     wheel_base_ * std::tan(right_steer_pos) /
@@ -211,29 +187,14 @@ bool SteeringKinematics::update_from_velocity(
 
 void SteeringKinematics::update_open_loop(const double v_bx, const double omega_bz, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return;
   /// Save last linear and angular velocity:
   linear_ = v_bx;
-
   angular_ = omega_bz;
 
   /// Integrate odometry:
   integrate_fk(v_bx, omega_bz, dt);
 }
 
-bool SteeringKinematics::try_update_open_loop(const double v_bx, const double omega_bz, const double dt)
-{
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return false;
-  /// Save last linear and angular velocity:
-  linear_ = v_bx;
-
-  angular_ = omega_bz;
-
-  /// Integrate odometry:
-  integrate_fk(v_bx, omega_bz, dt);
-  return true;
-}
-
 void SteeringKinematics::set_wheel_params(
   double wheel_radius, double wheel_base, double wheel_track)
 {
@@ -432,4 +393,5 @@ void SteeringKinematics::reset_accumulators()
   linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
   angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
 }
+
 }  // namespace steering_kinematics
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index c139e2df6e..f668230d5a 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -33,7 +33,6 @@ void SteeringOdometry::init(const rclcpp::Time & time) { sk_impl_.init(time); }
 bool SteeringOdometry::update_from_position(
   const double traction_wheel_pos, const double steer_pos, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_pos) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_position(traction_wheel_pos, steer_pos, dt);
 }
 
@@ -41,8 +40,6 @@ bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double steer_pos, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) ||
-     !std::isfinite(traction_left_wheel_pos) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_position(
     traction_right_wheel_pos, traction_left_wheel_pos, steer_pos, dt);
 }
@@ -51,8 +48,6 @@ bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_right_wheel_pos) ||
-     !std::isfinite(traction_left_wheel_pos) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
   return sk_impl_.update_from_position(
     traction_right_wheel_pos, traction_left_wheel_pos, right_steer_pos, left_steer_pos, dt);
 }
@@ -60,7 +55,6 @@ bool SteeringOdometry::update_from_position(
 bool SteeringOdometry::update_from_velocity(
   const double traction_wheel_vel, const double steer_pos, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_velocity(traction_wheel_vel, steer_pos, dt);
 }
 
@@ -68,8 +62,6 @@ bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double steer_pos, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
-     !std::isfinite(left_traction_wheel_vel) || !std::isfinite(steer_pos)) return false;
   return sk_impl_.update_from_velocity(
     right_traction_wheel_vel, left_traction_wheel_vel, steer_pos, dt);
 }
@@ -78,24 +70,15 @@ bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(right_traction_wheel_vel) ||
-     !std::isfinite(left_traction_wheel_vel) || !std::isfinite(right_steer_pos) || !std::isfinite(left_steer_pos)) return false;
   return sk_impl_.update_from_velocity(
     right_traction_wheel_vel, left_traction_wheel_vel, right_steer_pos, left_steer_pos, dt);
 }
 
 void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt)
 {
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return;
   sk_impl_.update_open_loop(v_bx, omega_bz, dt);
 }
 
-bool SteeringOdometry::try_update_open_loop(const double v_bx, const double omega_bz, const double dt){
-  if(std::fabs(dt) < std::numeric_limits<double>::epsilon() || !std::isfinite(v_bx) || !std::isfinite(omega_bz)) return false;
-  sk_impl_.try_update_open_loop(v_bx, omega_bz, dt);
-  return true;
-}
-
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track)
 {
   sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track);
@@ -126,4 +109,5 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
 }
 
 void SteeringOdometry::reset_odometry() { sk_impl_.reset_odometry(); }
+
 }  // namespace steering_odometry
diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
index 793bc2ae7d..d29562e2e0 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.cpp
+++ b/steering_controllers_library/test/test_steering_controllers_library.cpp
@@ -386,6 +386,72 @@ 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)
+{
+  // 1. 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);
+
+  // 2. Lifecycle
+  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);
+
+  // 3. The Publicist Hack (Unlock Input AND Output)
+  struct Publicist : public TestableSteeringControllersLibrary
+  {
+    // Unlock input buffer
+    using steering_controllers_library::SteeringControllersLibrary::input_ref_;
+    // Unlock the wheels (Output)
+    using controller_interface::ControllerInterfaceBase::command_interfaces_;
+  };
+  auto * pub_controller = static_cast<Publicist *>(controller_.get());
+
+  // 4. Send Valid Command (1.5 m/s)
+  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);
+
+  // 5. Force Update
+  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));
+
+  // 6. CHECK THE WHEELS
+  // FIX: Use .get_optional().value() (This is how ROS 2 reads values back)
+  double wheel_speed_1 = pub_controller->command_interfaces_[0].get_optional().value();
+
+  // It should be moving
+  ASSERT_GT(wheel_speed_1, 0.1);
+
+  // 7. Send NaN Command
+  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);
+
+  // 8. Force Update Again
+  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));
+
+  // 9. THE PROOF
+  // The wheel speed should stay exactly the same
+    EXPECT_DOUBLE_EQ(pub_controller->command_interfaces_[0].get_optional().value(), 0.0);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

From 9bf29696e6999fdcac5cb49d3862605b568e9291 Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Fri, 2 Jan 2026 11:09:52 +0530
Subject: [PATCH 05/12] Fix open-loop timeout behavior and add test coverage

- Reset last_linear_velocity_ and last_angular_velocity_ to 0.0 when reference interfaces are NaN (indicating a timeout) in update_and_write_commands.
- Previously, the controller persisted the last valid velocity during a timeout in open-loop mode, causing unsafe behavior.
- Added test_open_loop_update_timeout to verify that velocities reset to zero upon timeout.
- Added necessary FRIEND_TEST macro to test_steering_controllers_library.hpp to enable the test case.
---
 .../src/steering_controllers_library.cpp      |  6 +++
 .../test_steering_controllers_library.cpp     | 45 +++++++++++++------
 .../test_steering_controllers_library.hpp     |  1 +
 3 files changed, 39 insertions(+), 13 deletions(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index 4b1bcb3058..7b7075a23b 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -524,10 +524,16 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
   {
     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
diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
index d29562e2e0..33289f548d 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.cpp
+++ b/steering_controllers_library/test/test_steering_controllers_library.cpp
@@ -388,7 +388,7 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout)
 
 TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
 {
-  // 1. Setup Options
+  // Setup Options
   auto node_options = controller_->define_custom_node_options();
   node_options.append_parameter_override("open_loop", true);
   node_options.append_parameter_override(
@@ -397,22 +397,17 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
     "steering_joints_names", std::vector<std::string>{"steer_left", "steer_right"});
   SetUpController("test_steering_controllers_library", node_options);
 
-  // 2. Lifecycle
   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);
 
-  // 3. The Publicist Hack (Unlock Input AND Output)
   struct Publicist : public TestableSteeringControllersLibrary
   {
-    // Unlock input buffer
     using steering_controllers_library::SteeringControllersLibrary::input_ref_;
-    // Unlock the wheels (Output)
     using controller_interface::ControllerInterfaceBase::command_interfaces_;
   };
   auto * pub_controller = static_cast<Publicist *>(controller_.get());
 
-  // 4. Send Valid Command (1.5 m/s)
   auto command_msg = ControllerReferenceMsg();
   command_msg.header.stamp = controller_->get_node()->now();
   command_msg.twist.linear.x = 1.5;
@@ -420,20 +415,15 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
 
   pub_controller->input_ref_.set(command_msg);
 
-  // 5. Force Update
   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));
 
-  // 6. CHECK THE WHEELS
-  // FIX: Use .get_optional().value() (This is how ROS 2 reads values back)
   double wheel_speed_1 = pub_controller->command_interfaces_[0].get_optional().value();
 
-  // It should be moving
   ASSERT_GT(wheel_speed_1, 0.1);
 
-  // 7. Send NaN Command
   auto nan_msg = ControllerReferenceMsg();
   nan_msg.header.stamp = controller_->get_node()->now();
   nan_msg.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
@@ -441,17 +431,46 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
 
   pub_controller->input_ref_.set(nan_msg);
 
-  // 8. Force Update Again
   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));
 
-  // 9. THE PROOF
   // 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);
diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp
index 2ef1092f83..c3589cbf08 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.hpp
+++ b/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(

From 2804dd6dde11d8552379e91ef5952fb1862c85e2 Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Wed, 7 Jan 2026 17:25:48 +0530
Subject: [PATCH 06/12] style: fix linting errors via pre-commit

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/steering_controllers_library.cpp              |  6 ++++--
 .../test/test_steering_controllers_library.cpp        | 11 +++++------
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index 7b7075a23b..2aae21c5a8 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -524,14 +524,16 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
   {
     last_linear_velocity_ = reference_interfaces_[0];
   }
-  else{
+  else
+  {
     last_linear_velocity_ = 0.0;
   }
   if (std::isfinite(reference_interfaces_[1]))
   {
     last_angular_velocity_ = reference_interfaces_[1];
   }
-  else{
+  else
+  {
     last_angular_velocity_ = 0.0;
   }
   update_odometry(period);
diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
index 33289f548d..0dbbc8f937 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.cpp
+++ b/steering_controllers_library/test/test_steering_controllers_library.cpp
@@ -403,8 +403,8 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
 
   struct Publicist : public TestableSteeringControllersLibrary
   {
-    using steering_controllers_library::SteeringControllersLibrary::input_ref_;
     using controller_interface::ControllerInterfaceBase::command_interfaces_;
+    using steering_controllers_library::SteeringControllersLibrary::input_ref_;
   };
   auto * pub_controller = static_cast<Publicist *>(controller_.get());
 
@@ -437,7 +437,7 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
     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);
+  EXPECT_DOUBLE_EQ(pub_controller->command_interfaces_[0].get_optional().value(), 0.0);
 }
 
 TEST_F(SteeringControllersLibraryTest, test_open_loop_update_timeout)
@@ -450,15 +450,14 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_timeout)
   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
+  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.linear.x = 5.0;
   msg.twist.angular.z = 0.0;
-  controller_->input_ref_.set(msg); 
-
+  controller_->input_ref_.set(msg);
 
   controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.1));
 

From ad45105550e3e98a10cd70556804e8ffea52c133 Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Thu, 8 Jan 2026 00:46:58 +0530
Subject: [PATCH 07/12] fix: add missing cmath header for strict build

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/steering_controllers_library.cpp                         | 1 +
 1 file changed, 1 insertion(+)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index 2aae21c5a8..7cd17f537f 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -19,6 +19,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <cmath>
 
 #include "controller_interface/tf_prefix.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"

From aee344f881aa1f9fa0d7651db51c65a397f430fc Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Thu, 8 Jan 2026 11:55:57 +0530
Subject: [PATCH 08/12] style: sort headers via pre-commit

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/steering_controllers_library.cpp                        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index 7cd17f537f..71f5658c04 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -14,12 +14,12 @@
 
 #include "steering_controllers_library/steering_controllers_library.hpp"
 
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <string>
 #include <utility>
 #include <vector>
-#include <cmath>
 
 #include "controller_interface/tf_prefix.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"

From 3f1bc603b830df3048fca18dec175866796fc5ca Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Sun, 1 Feb 2026 18:08:11 +0530
Subject: [PATCH 09/12] fix(review): refactor odometry update logic and harden
 tests~

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/steering_controllers_library.cpp      | 42 +++++++++----------
 .../test_steering_controllers_library.cpp     |  8 ++--
 2 files changed, 24 insertions(+), 26 deletions(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index 71f5658c04..1a0eb9545a 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -520,25 +520,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
-  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
 
   // Limit velocities and accelerations:
@@ -675,8 +656,7 @@ 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", );
       }
       else
       {
@@ -684,10 +664,28 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
         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();
 
diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
index 0dbbc8f937..804d476f3b 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.cpp
+++ b/steering_controllers_library/test/test_steering_controllers_library.cpp
@@ -420,9 +420,7 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
   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);
+  ASSERT_GT(pub_controller->command_interfaces_[0].get_optional().value(), 0.1);
 
   auto nan_msg = ControllerReferenceMsg();
   nan_msg.header.stamp = controller_->get_node()->now();
@@ -436,7 +434,7 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_ignore_nan_vals)
   controller_->update_and_write_commands(
     controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01));
 
-  // The wheel speed should stay exactly the same
+  // The wheel speed should have been reset to 0.0
   EXPECT_DOUBLE_EQ(pub_controller->command_interfaces_[0].get_optional().value(), 0.0);
 }
 
@@ -468,6 +466,8 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_timeout)
   controller_->update(future_time, rclcpp::Duration::from_seconds(0.1));
 
   EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0);
+
+  EXPECT_DOUBLE_EQ(controller_->odom_state_msg_.twist.twist.linear.x, 0.0);
 }
 
 int main(int argc, char ** argv)

From 2c87aa829eb4ae2d6d4284a9d36244fe0251ae9f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 2 Feb 2026 12:30:29 +0100
Subject: [PATCH 10/12] Update
 steering_controllers_library/src/steering_controllers_library.cpp

---
 .../src/steering_controllers_library.cpp                        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index b4345c08e2..87ef3e0a41 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -657,7 +657,7 @@ 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", );
+                                                : "command interface value", i);
       }
       else
       {

From cd51a451e84711460496dbd8d64228d04e3ff299 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 2 Feb 2026 13:56:49 +0100
Subject: [PATCH 11/12] Fix pre-commit

---
 .../src/steering_controllers_library.cpp                       | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index 87ef3e0a41..a1b32baef1 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -657,7 +657,8 @@ 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
       {

From 91c84ff92c3993e5dd06c844b48066c31d1bb608 Mon Sep 17 00:00:00 2001
From: Ishan1923 <ecdev4ishan@gmail.com>
Date: Sun, 8 Feb 2026 14:52:09 +0530
Subject: [PATCH 12/12] test: verify open loop timeout using internal state to
 avoid message lag

Signed-off-by: Ishan1923 <ecdev4ishan@gmail.com>
---
 .../src/steering_controllers_library.cpp                   | 3 +--
 .../test/test_steering_controllers_library.cpp             | 7 ++++++-
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index a1b32baef1..87ef3e0a41 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/steering_controllers_library/src/steering_controllers_library.cpp
@@ -657,8 +657,7 @@ 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
       {
diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
index 804d476f3b..21baa2ad57 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.cpp
+++ b/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);
@@ -467,7 +473,6 @@ TEST_F(SteeringControllersLibraryTest, test_open_loop_update_timeout)
 
   EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0);
 
-  EXPECT_DOUBLE_EQ(controller_->odom_state_msg_.twist.twist.linear.x, 0.0);
 }
 
 int main(int argc, char ** argv)