Fix steering controllers library code documentation and naming (#1149)

* Update documentation and consolidate variable names

* Simplify private methods and further update docs

* Rename methods

* Rename method and variables

* Rename convert method

* Rename variables and improve doc

* Rename local variables

* Use std::isfinite instead of !isnan

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>

* Use a lowercase theta for heading

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>

* Make some temporary variables const

* Let update_from_position call update_from_velocity

* Explicitly set variables with 0 in constructor

* Fix docstring

* Apply consistent variable naming

Co-authored-by:  Quique Llorente <ellorent@redhat.com>

---------

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Co-authored-by: Quique Llorente <ellorent@redhat.com>

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 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::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
-      !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
+      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+      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(
-          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, steering_right_position,
+          steering_left_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, steering_right_position,
+          steering_left_position, period.seconds());
       }
     }
   }

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 steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
-    if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position))
+    const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
+    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(rear_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(rear_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds());
       }
     }
   }

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
    */
@@ -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 time    Current time
+   * \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
@@ -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     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(
-    const double Vx, const double theta_dot);
+    const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -199,35 +200,35 @@ 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 linear_velocity, const double angular, const double dt);
+  bool update_odometry(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \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 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
+   * \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(double linear, double angular);
+  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
-   * \param Vx   Linear  velocity   [m]
-   * \param theta_dot Angular velocity [rad]
+   * \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(double Vx, double theta_dot);
+  double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset linear and angular accumulators