From 78d60040b8c916a7c4eb59961738dc39b86ae0ba Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 21:06:16 +0000
Subject: [PATCH 01/14] Update documentation and consolidate variable names

---
 .../steering_odometry.hpp                     |  59 ++++++----
 .../src/steering_odometry.cpp                 | 105 +++++++++---------
 2 files changed, 92 insertions(+), 72 deletions(-)

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 e4a22f6d3b..f7128ab585 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -132,7 +132,7 @@ class SteeringOdometry
    * \brief Updates the odometry class with latest velocity command
    * \param linear  Linear velocity [m/s]
    * \param angular Angular velocity [rad/s]
-   * \param time    Current time
+   * \param dt      time difference to last call
    */
   void update_open_loop(const double linear, const double angular, const double dt);
 
@@ -175,22 +175,23 @@ class SteeringOdometry
   /**
    * \brief Sets the wheel parameters: radius, separation and wheelbase
    */
-  void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0);
+  void set_wheel_params(
+    const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0);
 
   /**
    * \brief Velocity rolling window size setter
    * \param velocity_rolling_window_size Velocity rolling window size
    */
-  void set_velocity_rolling_window_size(size_t velocity_rolling_window_size);
+  void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size);
 
   /**
    * \brief Calculates inverse kinematics for the desired linear and angular velocities
-   * \param Vx  Desired linear velocity [m/s]
-   * \param theta_dot Desired angular velocity [rad/s]
+   * \param v_bx     Linear velocity of the robot in x_b-axis direction
+   * \param omega_bz Angular velocity of the robot around x_z-axis
    * \return Tuple of velocity commands and steering commands
    */
   std::tuple<std::vector<double>, std::vector<double>> get_commands(
-    const double Vx, const double theta_dot);
+    const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -199,35 +200,49 @@ class SteeringOdometry
 
 private:
   /**
-   * \brief Uses precomputed linear and angular velocities to compute dometry and update
-   * accumulators \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt)
-   * computed by previous odometry method \param angular Angular velocity [rad] (angular
-   * displacement, i.e. m/s * dt) computed by previous odometry method
+   * \brief Uses precomputed linear and angular velocities to compute odometry
+   * \param v_bx  Linear  velocity   [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt      time difference to last call
+   */
+  bool update_odometry(const double v_bx, const double omega_bz, const double dt);
+
+  /**
+   * \brief Integrates the displacements (linear and angular) using 2nd order Runge-Kutta
+   * \param v_bx  Linear displacement [m], i.e. m/s * dt computed by encoders
+   * \param omega_bz Angular displaycement [rad], i.e. m/s * dt computed by encoders
    */
-  bool update_odometry(const double linear_velocity, const double angular, const double dt);
+  void integrate_runge_kutta_2(const double v_bx, const double omega_bz);
 
   /**
    * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
-   * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by
-   * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed
-   * by encoders
+   * \param v_bx  Linear velocity [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt      time difference to last call
    */
-  void integrate_runge_kutta_2(double linear, double angular);
+  void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt);
+
+  /**
+   * \brief Integrates the discplacements (linear and angular) using exact method
+   * \param v_bx  Linear displacement [m], i.e. m/s * dt computed by encoders
+   * \param omega_bz Angular displaycement [rad], i.e. m/s * dt computed by encoders
+   */
+  void integrate_exact(const double v_bx, const double omega_bz);
 
   /**
    * \brief Integrates the velocities (linear and angular) using exact method
-   * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by
-   * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed
-   * by encoders
+   * \param v_bx  Linear velocity [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt      time difference to last call
    */
-  void integrate_exact(double linear, double angular);
+  void integrate_exact(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \brief Calculates steering angle from the desired translational and rotational velocity
-   * \param Vx   Linear  velocity   [m]
-   * \param theta_dot Angular velocity [rad]
+   * \param v_bx     Linear velocity of the robot in x_b-axis direction
+   * \param omega_bz Angular velocity of the robot around x_z-axis
    */
-  double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot);
+  double convert_trans_rot_vel_to_steering_angle(const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset linear and angular accumulators
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 6fb20478a4..6d53c1636d 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -48,11 +48,10 @@ void SteeringOdometry::init(const rclcpp::Time & time)
   timestamp_ = time;
 }
 
-bool SteeringOdometry::update_odometry(
-  const double linear_velocity, const double angular, const double dt)
+bool SteeringOdometry::update_odometry(const double linear, const double angular, const double dt)
 {
   /// Integrate odometry:
-  SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt);
+  integrate_exact(linear, angular, dt);
 
   /// We cannot estimate the speed with very small time intervals:
   if (dt < 0.0001)
@@ -61,7 +60,7 @@ bool SteeringOdometry::update_odometry(
   }
 
   /// Estimate speeds using a rolling mean to filter them out:
-  linear_acc_.accumulate(linear_velocity);
+  linear_acc_.accumulate(linear);
   angular_acc_.accumulate(angular);
 
   linear_ = linear_acc_.getRollingMean();
@@ -83,9 +82,9 @@ bool SteeringOdometry::update_from_position(
   /// Compute linear and angular diff:
   const double linear_velocity = traction_wheel_est_pos_diff / dt;
   steer_pos_ = steer_pos;
-  const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_position(
@@ -108,9 +107,9 @@ bool SteeringOdometry::update_from_position(
   const double linear_velocity =
     (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
   steer_pos_ = steer_pos;
-  const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_position(
@@ -134,9 +133,9 @@ bool SteeringOdometry::update_from_position(
   const double linear_velocity =
     (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
   steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
-  const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(
@@ -144,9 +143,9 @@ bool SteeringOdometry::update_from_velocity(
 {
   steer_pos_ = steer_pos;
   double linear_velocity = traction_wheel_vel * wheel_radius_;
-  const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(
@@ -157,9 +156,9 @@ bool SteeringOdometry::update_from_velocity(
     (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
   steer_pos_ = steer_pos;
 
-  const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(
@@ -169,9 +168,9 @@ bool SteeringOdometry::update_from_velocity(
   steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
   double linear_velocity =
     (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
-  const double angular = steer_pos_ * linear_velocity / wheelbase_;
+  const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt)
@@ -181,7 +180,7 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula
   angular_ = angular;
 
   /// Integrate odometry:
-  SteeringOdometry::integrate_exact(linear * dt, angular * dt);
+  integrate_exact(linear, angular, dt);
 }
 
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)
@@ -200,36 +199,37 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_
 
 void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; }
 
-double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot)
+double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double v_bx, double omega_bz)
 {
-  if (theta_dot == 0 || Vx == 0)
+  if (omega_bz == 0 || v_bx == 0)
   {
     return 0;
   }
-  return std::atan(theta_dot * wheelbase_ / Vx);
+  return std::atan(omega_bz * wheelbase_ / v_bx);
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  const double Vx, const double theta_dot)
+  const double v_bx, const double omega_bz)
 {
-  // desired velocity and steering angle of the middle of traction and steering axis
-  double Ws, alpha;
+  // desired wheel speed and steering angle of the middle of traction and steering axis
+  double Ws, phi;
 
-  if (Vx == 0 && theta_dot != 0)
+  if (v_bx == 0 && omega_bz != 0)
   {
-    alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2;
-    Ws = abs(theta_dot) * wheelbase_ / wheel_radius_;
+    // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller
+    phi = omega_bz > 0 ? M_PI_2 : -M_PI_2;
+    Ws = abs(omega_bz) * wheelbase_ / wheel_radius_;
   }
   else
   {
-    alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot);
-    Ws = Vx / (wheel_radius_ * std::cos(steer_pos_));
+    phi = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(v_bx, omega_bz);
+    Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_));
   }
 
   if (config_type_ == BICYCLE_CONFIG)
   {
     std::vector<double> traction_commands = {Ws};
-    std::vector<double> steering_commands = {alpha};
+    std::vector<double> steering_commands = {phi};
     return std::make_tuple(traction_commands, steering_commands);
   }
   else if (config_type_ == TRICYCLE_CONFIG)
@@ -247,7 +247,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
       double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
     }
-    steering_commands = {alpha};
+    steering_commands = {phi};
     return std::make_tuple(traction_commands, steering_commands);
   }
   else if (config_type_ == ACKERMANN_CONFIG)
@@ -257,7 +257,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     if (fabs(steer_pos_) < 1e-6)
     {
       traction_commands = {Ws, Ws};
-      steering_commands = {alpha, alpha};
+      steering_commands = {phi, phi};
     }
     else
     {
@@ -266,9 +266,9 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
       double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
 
-      double numerator = 2 * wheelbase_ * std::sin(alpha);
-      double denominator_first_member = 2 * wheelbase_ * std::cos(alpha);
-      double denominator_second_member = wheel_track_ * std::sin(alpha);
+      double numerator = 2 * wheelbase_ * std::sin(phi);
+      double denominator_first_member = 2 * wheelbase_ * std::cos(phi);
+      double denominator_second_member = wheel_track_ * std::sin(phi);
 
       double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member);
       double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member);
@@ -290,38 +290,43 @@ void SteeringOdometry::reset_odometry()
   reset_accumulators();
 }
 
-void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular)
+void SteeringOdometry::integrate_runge_kutta_2(const double v_bx, const double omega_bz)
 {
-  const double direction = heading_ + angular * 0.5;
+  const double direction = heading_ + omega_bz * 0.5;
 
   /// Runge-Kutta 2nd order integration:
-  x_ += linear * cos(direction);
-  y_ += linear * sin(direction);
-  heading_ += angular;
+  x_ += v_bx * cos(direction);
+  y_ += v_bx * sin(direction);
+  heading_ += omega_bz;
+}
+void SteeringOdometry::integrate_runge_kutta_2(
+  const double v_bx, const double omega_bz, const double dt)
+{
+  integrate_runge_kutta_2(v_bx * dt, omega_bz * dt);
 }
 
-/**
- * \brief Other possible integration method provided by the class
- * \param linear
- * \param angular
- */
-void SteeringOdometry::integrate_exact(double linear, double angular)
+void SteeringOdometry::integrate_exact(const double v_bx, const double omega_bz)
 {
-  if (fabs(angular) < 1e-6)
+  if (fabs(omega_bz) < 1e-6)
   {
-    integrate_runge_kutta_2(linear, angular);
+    integrate_runge_kutta_2(v_bx, omega_bz);
   }
   else
   {
-    /// Exact integration (should solve problems when angular is zero):
+    /// Exact integration (should solve problems when omega_bz is zero):
     const double heading_old = heading_;
-    const double r = linear / angular;
-    heading_ += angular;
+    const double r = v_bx / omega_bz;
+    heading_ += omega_bz;
     x_ += r * (sin(heading_) - sin(heading_old));
     y_ += -r * (cos(heading_) - cos(heading_old));
   }
 }
 
+void SteeringOdometry::integrate_exact(const double v_bx, const double omega_bz, const double dt)
+{
+  integrate_exact(v_bx * dt, omega_bz * dt);
+}
+
 void SteeringOdometry::reset_accumulators()
 {
   linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);

From 759264bba92fa4e5f694a69a20da9b8c597097b3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 21:26:33 +0000
Subject: [PATCH 02/14] Simplify private methods and further update docs

---
 .../steering_odometry.hpp                     | 22 ++--------
 .../src/steering_odometry.cpp                 | 42 +++++++++----------
 2 files changed, 23 insertions(+), 41 deletions(-)

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 f7128ab585..06158a5995 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -207,33 +207,19 @@ class SteeringOdometry
    */
   bool update_odometry(const double v_bx, const double omega_bz, const double dt);
 
-  /**
-   * \brief Integrates the displacements (linear and angular) using 2nd order Runge-Kutta
-   * \param v_bx  Linear displacement [m], i.e. m/s * dt computed by encoders
-   * \param omega_bz Angular displaycement [rad], i.e. m/s * dt computed by encoders
-   */
-  void integrate_runge_kutta_2(const double v_bx, const double omega_bz);
-
   /**
    * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
-   * \param v_bx  Linear velocity [m/s]
+   * \param v_bx Linear velocity [m/s]
    * \param omega_bz Angular velocity [rad/s]
-   * \param dt      time difference to last call
+   * \param dt time difference to last call
    */
   void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt);
 
-  /**
-   * \brief Integrates the discplacements (linear and angular) using exact method
-   * \param v_bx  Linear displacement [m], i.e. m/s * dt computed by encoders
-   * \param omega_bz Angular displaycement [rad], i.e. m/s * dt computed by encoders
-   */
-  void integrate_exact(const double v_bx, const double omega_bz);
-
   /**
    * \brief Integrates the velocities (linear and angular) using exact method
-   * \param v_bx  Linear velocity [m/s]
+   * \param v_bx Linear velocity [m/s]
    * \param omega_bz Angular velocity [rad/s]
-   * \param dt      time difference to last call
+   * \param dt time difference to last call
    */
   void integrate_exact(const double v_bx, const double omega_bz, const double dt);
 
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 6d53c1636d..4741e4a90b 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -290,43 +290,39 @@ void SteeringOdometry::reset_odometry()
   reset_accumulators();
 }
 
-void SteeringOdometry::integrate_runge_kutta_2(const double v_bx, const double omega_bz)
-{
-  const double direction = heading_ + omega_bz * 0.5;
-
-  /// Runge-Kutta 2nd order integration:
-  x_ += v_bx * cos(direction);
-  y_ += v_bx * sin(direction);
-  heading_ += omega_bz;
-}
 void SteeringOdometry::integrate_runge_kutta_2(
   const double v_bx, const double omega_bz, const double dt)
 {
-  integrate_runge_kutta_2(v_bx * dt, omega_bz * dt);
+  // Compute intermediate value of the heading
+  const double Theta_mid = heading_ + omega_bz * 0.5 * dt;
+
+  // Use the intermediate values to update the state
+  x_ += v_bx * cos(Theta_mid) * dt;
+  y_ += v_bx * sin(Theta_mid) * dt;
+  heading_ += omega_bz * dt;
 }
 
-void SteeringOdometry::integrate_exact(const double v_bx, const double omega_bz)
+void SteeringOdometry::integrate_exact(const double v_bx, const double omega_bz, const double dt)
 {
-  if (fabs(omega_bz) < 1e-6)
+  const double delta_x_b = v_bx * dt;
+  const double delta_Theta = omega_bz * dt;
+
+  if (fabs(delta_Theta) < 1e-6)
   {
-    integrate_runge_kutta_2(v_bx, omega_bz);
+    /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero):
+    integrate_runge_kutta_2(v_bx, omega_bz, dt);
   }
   else
   {
-    /// Exact integration (should solve problems when omega_bz is zero):
+    /// Exact integration
     const double heading_old = heading_;
-    const double r = v_bx / omega_bz;
-    heading_ += omega_bz;
-    x_ += r * (sin(heading_) - sin(heading_old));
-    y_ += -r * (cos(heading_) - cos(heading_old));
+    const double R = delta_x_b / delta_Theta;
+    heading_ += delta_Theta;
+    x_ += R * (sin(heading_) - sin(heading_old));
+    y_ += -R * (cos(heading_) - cos(heading_old));
   }
 }
 
-void SteeringOdometry::integrate_exact(const double v_bx, const double omega_bz, const double dt)
-{
-  integrate_exact(v_bx * dt, omega_bz * dt);
-}
-
 void SteeringOdometry::reset_accumulators()
 {
   linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);

From f85fc5b64ea714bcf27e3b98841b3f4d50e0edfe Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 21:28:02 +0000
Subject: [PATCH 03/14] Rename methods

---
 .../steering_controllers_library/steering_odometry.hpp      | 4 ++--
 steering_controllers_library/src/steering_odometry.cpp      | 6 +++---
 2 files changed, 5 insertions(+), 5 deletions(-)

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 06158a5995..9bad40202d 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -216,12 +216,12 @@ class SteeringOdometry
   void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt);
 
   /**
-   * \brief Integrates the velocities (linear and angular) using exact method
+   * \brief Integrates the velocities (linear and angular)
    * \param v_bx Linear velocity [m/s]
    * \param omega_bz Angular velocity [rad/s]
    * \param dt time difference to last call
    */
-  void integrate_exact(const double v_bx, const double omega_bz, const double dt);
+  void integrate(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \brief Calculates steering angle from the desired translational and rotational velocity
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 4741e4a90b..4d98b49250 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -51,7 +51,7 @@ void SteeringOdometry::init(const rclcpp::Time & time)
 bool SteeringOdometry::update_odometry(const double linear, const double angular, const double dt)
 {
   /// Integrate odometry:
-  integrate_exact(linear, angular, dt);
+  integrate(linear, angular, dt);
 
   /// We cannot estimate the speed with very small time intervals:
   if (dt < 0.0001)
@@ -180,7 +180,7 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula
   angular_ = angular;
 
   /// Integrate odometry:
-  integrate_exact(linear, angular, dt);
+  integrate(linear, angular, dt);
 }
 
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)
@@ -302,7 +302,7 @@ void SteeringOdometry::integrate_runge_kutta_2(
   heading_ += omega_bz * dt;
 }
 
-void SteeringOdometry::integrate_exact(const double v_bx, const double omega_bz, const double dt)
+void SteeringOdometry::integrate(const double v_bx, const double omega_bz, const double dt)
 {
   const double delta_x_b = v_bx * dt;
   const double delta_Theta = omega_bz * dt;

From 8602f49676ad7c13c3c6fccb2cafefabdee2f0f5 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 21:33:13 +0000
Subject: [PATCH 04/14] Rename method and variables

---
 .../steering_odometry.hpp                           |  2 +-
 .../src/steering_odometry.cpp                       | 13 +++++++------
 2 files changed, 8 insertions(+), 7 deletions(-)

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 9bad40202d..fcad530ca1 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -221,7 +221,7 @@ class SteeringOdometry
    * \param omega_bz Angular velocity [rad/s]
    * \param dt time difference to last call
    */
-  void integrate(const double v_bx, const double omega_bz, const double dt);
+  void integrate_fk(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \brief Calculates steering angle from the desired translational and rotational velocity
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 4d98b49250..3afa391371 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -48,10 +48,11 @@ void SteeringOdometry::init(const rclcpp::Time & time)
   timestamp_ = time;
 }
 
-bool SteeringOdometry::update_odometry(const double linear, const double angular, const double dt)
+bool SteeringOdometry::update_odometry(
+  const double linear_velocity, const double angular_velocity, const double dt)
 {
   /// Integrate odometry:
-  integrate(linear, angular, dt);
+  integrate_fk(linear_velocity, angular_velocity, dt);
 
   /// We cannot estimate the speed with very small time intervals:
   if (dt < 0.0001)
@@ -60,8 +61,8 @@ bool SteeringOdometry::update_odometry(const double linear, const double angular
   }
 
   /// Estimate speeds using a rolling mean to filter them out:
-  linear_acc_.accumulate(linear);
-  angular_acc_.accumulate(angular);
+  linear_acc_.accumulate(linear_velocity);
+  angular_acc_.accumulate(angular_velocity);
 
   linear_ = linear_acc_.getRollingMean();
   angular_ = angular_acc_.getRollingMean();
@@ -180,7 +181,7 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula
   angular_ = angular;
 
   /// Integrate odometry:
-  integrate(linear, angular, dt);
+  integrate_fk(linear, angular, dt);
 }
 
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)
@@ -302,7 +303,7 @@ void SteeringOdometry::integrate_runge_kutta_2(
   heading_ += omega_bz * dt;
 }
 
-void SteeringOdometry::integrate(const double v_bx, const double omega_bz, const double dt)
+void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt)
 {
   const double delta_x_b = v_bx * dt;
   const double delta_Theta = omega_bz * dt;

From bf9998bbd1d971bed5d2a8722da4aa97f316a114 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 21:39:00 +0000
Subject: [PATCH 05/14] Rename convert method

---
 .../steering_controllers_library/steering_odometry.hpp        | 4 ++--
 steering_controllers_library/src/steering_odometry.cpp        | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

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 fcad530ca1..750aad2407 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -224,11 +224,11 @@ class SteeringOdometry
   void integrate_fk(const double v_bx, const double omega_bz, const double dt);
 
   /**
-   * \brief Calculates steering angle from the desired translational and rotational velocity
+   * \brief Calculates steering angle from the desired twist
    * \param v_bx     Linear velocity of the robot in x_b-axis direction
    * \param omega_bz Angular velocity of the robot around x_z-axis
    */
-  double convert_trans_rot_vel_to_steering_angle(const double v_bx, const double omega_bz);
+  double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset linear and angular accumulators
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 3afa391371..de1182132f 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -200,7 +200,7 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_
 
 void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; }
 
-double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double v_bx, double omega_bz)
+double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz)
 {
   if (omega_bz == 0 || v_bx == 0)
   {
@@ -223,7 +223,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   }
   else
   {
-    phi = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(v_bx, omega_bz);
+    phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
     Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_));
   }
 

From 2531973385c00305665d29e8a4f68d31b87fc4f4 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 24 May 2024 21:42:31 +0000
Subject: [PATCH 06/14] Rename variables and improve doc

---
 .../steering_controllers_library/steering_odometry.hpp | 10 +++++-----
 steering_controllers_library/src/steering_odometry.cpp |  8 ++++----
 2 files changed, 9 insertions(+), 9 deletions(-)

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 750aad2407..f56e05dbb1 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -130,11 +130,11 @@ class SteeringOdometry
 
   /**
    * \brief Updates the odometry class with latest velocity command
-   * \param linear  Linear velocity [m/s]
-   * \param angular Angular velocity [rad/s]
+   * \param v_bx  Linear velocity   [m/s]
+   * \param omega_bz Angular velocity [rad/s]
    * \param dt      time difference to last call
    */
-  void update_open_loop(const double linear, const double angular, const double dt);
+  void update_open_loop(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \brief Set odometry type
@@ -186,8 +186,8 @@ class SteeringOdometry
 
   /**
    * \brief Calculates inverse kinematics for the desired linear and angular velocities
-   * \param v_bx     Linear velocity of the robot in x_b-axis direction
-   * \param omega_bz Angular velocity of the robot around x_z-axis
+   * \param v_bx     Desired linear velocity of the robot in x_b-axis direction
+   * \param omega_bz Desired angular velocity of the robot around x_z-axis
    * \return Tuple of velocity commands and steering commands
    */
   std::tuple<std::vector<double>, std::vector<double>> get_commands(
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index de1182132f..45323c3f82 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -174,14 +174,14 @@ bool SteeringOdometry::update_from_velocity(
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
-void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt)
+void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt)
 {
   /// Save last linear and angular velocity:
-  linear_ = linear;
-  angular_ = angular;
+  linear_ = v_bx;
+  angular_ = omega_bz;
 
   /// Integrate odometry:
-  integrate_fk(linear, angular, dt);
+  integrate_fk(v_bx, omega_bz, dt);
 }
 
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)

From 77212df995303e692c55d4824d7347823be88e10 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sat, 25 May 2024 20:38:20 +0000
Subject: [PATCH 07/14] Rename local variables

---
 .../src/ackermann_steering_controller.cpp     | 23 ++++++++++---------
 .../src/bicycle_steering_controller.cpp       |  8 +++----
 .../src/tricycle_steering_controller.cpp      | 12 ++++++----
 3 files changed, 23 insertions(+), 20 deletions(-)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index c3a7539c5a..571c56c44e 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
   }
   else
   {
-    const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
-    const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double front_right_steer_position =
-      state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
-    const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
+    const double traction_right_wheel_value =
+      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+    const double traction_left_wheel_value =
+      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
+    const double right_steer_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
+    const double left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
     if (
-      !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
-      !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
+      !std::isnan(traction_right_wheel_value) && !std::isnan(traction_left_wheel_value) &&
+      !std::isnan(right_steer_position) && !std::isnan(left_steer_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_position(
-          rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
-          front_left_steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, right_steer_position,
+          left_steer_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_velocity(
-          rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
-          front_left_steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, right_steer_position,
+          left_steer_position, period.seconds());
       }
     }
   }
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index 5f013d7d7a..017ff21ae9 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp
+++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
@@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
   }
   else
   {
-    const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
+    const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
     const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
-    if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position))
+    if (!std::isnan(traction_wheel_value) && !std::isnan(steer_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_position(traction_wheel_value, steer_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_velocity(traction_wheel_value, steer_position, period.seconds());
       }
     }
   }
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index f89d78a52c..af3fc38293 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -60,24 +60,26 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
   }
   else
   {
-    const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
-    const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
+    const double traction_right_wheel_value =
+      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+    const double traction_left_wheel_value =
+      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
     const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
     if (
-      !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
+      !std::isnan(traction_right_wheel_value) && !std::isnan(traction_left_wheel_value) &&
       !std::isnan(steer_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_position(
-          rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_velocity(
-          rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds());
       }
     }
   }

From e115836ef8a218a4fa609f1418165adf9db11f48 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 27 May 2024 22:00:53 +0200
Subject: [PATCH 08/14] Use std::isfinite instead of !isnan

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
---
 .../src/ackermann_steering_controller.cpp                     | 4 ++--
 .../src/bicycle_steering_controller.cpp                       | 2 +-
 .../src/tricycle_steering_controller.cpp                      | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index 571c56c44e..00a8c45442 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -69,8 +69,8 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
     const double right_steer_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
     const double left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
     if (
-      !std::isnan(traction_right_wheel_value) && !std::isnan(traction_left_wheel_value) &&
-      !std::isnan(right_steer_position) && !std::isnan(left_steer_position))
+      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+      std::isfinite(right_steer_position) && std::isfinite(left_steer_position))
     {
       if (params_.position_feedback)
       {
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index 017ff21ae9..14d503fcda 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp
+++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
@@ -62,7 +62,7 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
   {
     const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
     const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
-    if (!std::isnan(traction_wheel_value) && !std::isnan(steer_position))
+    if (std::isfinite(traction_wheel_value) && std::isfinite(steer_position))
     {
       if (params_.position_feedback)
       {
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index af3fc38293..fa7aad9410 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -66,8 +66,8 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
       state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
     const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
     if (
-      !std::isnan(traction_right_wheel_value) && !std::isnan(traction_left_wheel_value) &&
-      !std::isnan(steer_position))
+      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+      std::isfinite(steer_position))
     {
       if (params_.position_feedback)
       {

From 6942c1a0f6b1cf694167278bf8af976e1135f6f0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 27 May 2024 23:37:32 +0200
Subject: [PATCH 09/14] Use a lowercase theta for heading

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
---
 .../src/steering_odometry.cpp                      | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 45323c3f82..e07b988dd4 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -295,20 +295,20 @@ void SteeringOdometry::integrate_runge_kutta_2(
   const double v_bx, const double omega_bz, const double dt)
 {
   // Compute intermediate value of the heading
-  const double Theta_mid = heading_ + omega_bz * 0.5 * dt;
+  const double theta_mid = heading_ + omega_bz * 0.5 * dt;
 
   // Use the intermediate values to update the state
-  x_ += v_bx * cos(Theta_mid) * dt;
-  y_ += v_bx * sin(Theta_mid) * dt;
+  x_ += v_bx * cos(theta_mid) * dt;
+  y_ += v_bx * sin(theta_mid) * dt;
   heading_ += omega_bz * dt;
 }
 
 void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt)
 {
   const double delta_x_b = v_bx * dt;
-  const double delta_Theta = omega_bz * dt;
+  const double delta_theta = omega_bz * dt;
 
-  if (fabs(delta_Theta) < 1e-6)
+  if (fabs(delta_theta) < 1e-6)
   {
     /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero):
     integrate_runge_kutta_2(v_bx, omega_bz, dt);
@@ -317,8 +317,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co
   {
     /// Exact integration
     const double heading_old = heading_;
-    const double R = delta_x_b / delta_Theta;
-    heading_ += delta_Theta;
+    const double R = delta_x_b / delta_theta;
+    heading_ += delta_theta;
     x_ += R * (sin(heading_) - sin(heading_old));
     y_ += -R * (cos(heading_) - cos(heading_old));
   }

From 4e5653d39cab08071c84140df27f9ebe7d22f9d1 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Mon, 27 May 2024 21:42:59 +0000
Subject: [PATCH 10/14] Make some temporary variables const

---
 .../src/steering_odometry.cpp                 | 24 ++++++++++---------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index e07b988dd4..f388f50687 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -243,9 +243,9 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     }
     else
     {
-      double turning_radius = wheelbase_ / std::tan(steer_pos_);
-      double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
-      double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
+      const double turning_radius = wheelbase_ / std::tan(steer_pos_);
+      const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
+      const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
     }
     steering_commands = {phi};
@@ -262,17 +262,19 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     }
     else
     {
-      double turning_radius = wheelbase_ / std::tan(steer_pos_);
-      double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
-      double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
+      const double turning_radius = wheelbase_ / std::tan(steer_pos_);
+      const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
+      const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
 
-      double numerator = 2 * wheelbase_ * std::sin(phi);
-      double denominator_first_member = 2 * wheelbase_ * std::cos(phi);
-      double denominator_second_member = wheel_track_ * std::sin(phi);
+      const double numerator = 2 * wheelbase_ * std::sin(phi);
+      const double denominator_first_member = 2 * wheelbase_ * std::cos(phi);
+      const double denominator_second_member = wheel_track_ * std::sin(phi);
 
-      double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member);
-      double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member);
+      const double alpha_r =
+        std::atan2(numerator, denominator_first_member + denominator_second_member);
+      const double alpha_l =
+        std::atan2(numerator, denominator_first_member - denominator_second_member);
       steering_commands = {alpha_r, alpha_l};
     }
     return std::make_tuple(traction_commands, steering_commands);

From f0b27807b190d3cc97d64cc9bb3bb55a59131420 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sat, 25 May 2024 22:33:25 +0000
Subject: [PATCH 11/14] Let update_from_position call update_from_velocity

---
 .../src/steering_odometry.cpp                 | 55 ++++++-------------
 1 file changed, 16 insertions(+), 39 deletions(-)

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index f388f50687..f917621e7d 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -73,70 +73,47 @@ bool SteeringOdometry::update_odometry(
 bool SteeringOdometry::update_from_position(
   const double traction_wheel_pos, const double steer_pos, const double dt)
 {
-  /// Get current wheel joint positions:
-  const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_;
-  const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_;
+  const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_;
 
   /// Update old position with current:
-  traction_wheel_old_pos_ = traction_wheel_cur_pos;
+  traction_wheel_old_pos_ = traction_wheel_pos;
 
-  /// Compute linear and angular diff:
-  const double linear_velocity = traction_wheel_est_pos_diff / dt;
-  steer_pos_ = steer_pos;
-  const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_;
-
-  return update_odometry(linear_velocity, angular_velocity, dt);
+  return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt);
 }
 
 bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double steer_pos, const double dt)
 {
-  /// Get current wheel joint positions:
-  const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_;
-  const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_;
-
   const double traction_right_wheel_est_pos_diff =
-    traction_right_wheel_cur_pos - traction_right_wheel_old_pos_;
+    traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
-    traction_left_wheel_cur_pos - traction_left_wheel_old_pos_;
+    traction_left_wheel_pos - traction_left_wheel_old_pos_;
 
   /// Update old position with current:
-  traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos;
-  traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos;
-
-  const double linear_velocity =
-    (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
-  steer_pos_ = steer_pos;
-  const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
+  traction_right_wheel_old_pos_ = traction_right_wheel_pos;
+  traction_left_wheel_old_pos_ = traction_left_wheel_pos;
 
-  return update_odometry(linear_velocity, angular_velocity, dt);
+  return update_from_velocity(
+    traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt);
 }
 
 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)
 {
-  /// Get current wheel joint positions:
-  const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_;
-  const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_;
-
   const double traction_right_wheel_est_pos_diff =
-    traction_right_wheel_cur_pos - traction_right_wheel_old_pos_;
+    traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
-    traction_left_wheel_cur_pos - traction_left_wheel_old_pos_;
+    traction_left_wheel_pos - traction_left_wheel_old_pos_;
 
   /// Update old position with current:
-  traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos;
-  traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos;
+  traction_right_wheel_old_pos_ = traction_right_wheel_pos;
+  traction_left_wheel_old_pos_ = traction_left_wheel_pos;
 
-  /// Compute linear and angular diff:
-  const double linear_velocity =
-    (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
-  steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
-  const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
-
-  return update_odometry(linear_velocity, angular_velocity, dt);
+  return update_from_velocity(
+    traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos,
+    left_steer_pos, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(

From d8ce400c2b5d9716c63f2563df212a3432046a69 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 28 May 2024 19:51:03 +0000
Subject: [PATCH 12/14] Explicitly set variables with 0 in constructor

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

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index f917621e7d..32cdb69454 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -35,6 +35,8 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size)
   wheelbase_(0.0),
   wheel_radius_(0.0),
   traction_wheel_old_pos_(0.0),
+  traction_right_wheel_old_pos_(0.0),
+  traction_left_wheel_old_pos_(0.0),
   velocity_rolling_window_size_(velocity_rolling_window_size),
   linear_acc_(velocity_rolling_window_size),
   angular_acc_(velocity_rolling_window_size)

From 479d850a8ef07dad3b7183b9a19ef1297e55d1cf Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 29 May 2024 18:36:40 +0000
Subject: [PATCH 13/14] Fix docstring

---
 .../steering_controllers_library/steering_odometry.hpp    | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

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 f56e05dbb1..0104b011c7 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -61,7 +61,7 @@ class SteeringOdometry
   /**
    * \brief Updates the odometry class with latest wheels position
    * \param traction_wheel_pos  traction wheel position [rad]
-   * \param steer_pos Front Steer position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -72,7 +72,7 @@ class SteeringOdometry
    * \brief Updates the odometry class with latest wheels position
    * \param right_traction_wheel_pos  Right traction wheel velocity [rad]
    * \param left_traction_wheel_pos  Left traction wheel velocity [rad]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -96,7 +96,7 @@ class SteeringOdometry
   /**
    * \brief Updates the odometry class with latest wheels position
    * \param traction_wheel_vel  Traction wheel velocity [rad/s]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -107,7 +107,7 @@ class SteeringOdometry
    * \brief Updates the odometry class with latest wheels position
    * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]
    * \param left_traction_wheel_vel  Left traction wheel velocity [rad/s]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */

From 089a803fe0fedb96049cc2d22f4f265b28e20819 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 5 Jun 2024 19:28:24 +0000
Subject: [PATCH 14/14] Apply consistent variable naming

Co-authored-by:  Quique Llorente <ellorent@redhat.com>
---
 .../src/ackermann_steering_controller.cpp          | 14 +++++++-------
 .../src/bicycle_steering_controller.cpp            |  8 ++++----
 .../src/tricycle_steering_controller.cpp           | 10 ++++++----
 3 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index 00a8c45442..d9d95bf8b5 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -66,25 +66,25 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
       state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
     const double traction_left_wheel_value =
       state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double right_steer_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
-    const double left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
+    const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
+    const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
     if (
       std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
-      std::isfinite(right_steer_position) && std::isfinite(left_steer_position))
+      std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_position(
-          traction_right_wheel_value, traction_left_wheel_value, right_steer_position,
-          left_steer_position, period.seconds());
+          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(
-          traction_right_wheel_value, traction_left_wheel_value, right_steer_position,
-          left_steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
+          steering_left_position, period.seconds());
       }
     }
   }
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index 14d503fcda..95eaf1965c 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp
+++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
@@ -61,18 +61,18 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
   else
   {
     const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
-    const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
-    if (std::isfinite(traction_wheel_value) && std::isfinite(steer_position))
+    const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+    if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_position(traction_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_velocity(traction_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds());
       }
     }
   }
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index fa7aad9410..03be6b88f0 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -64,22 +64,24 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
       state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
     const double traction_left_wheel_value =
       state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+    const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
     if (
       std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
-      std::isfinite(steer_position))
+      std::isfinite(steering_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_position(
-          traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_position,
+          period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_velocity(
-          traction_right_wheel_value, traction_left_wheel_value, steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_position,
+          period.seconds());
       }
     }
   }