From 576fafb018d359e30f204e44d116e470b2dccbee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
<christophfroehlich@users.noreply.github.com>
Date: Wed, 5 Jun 2024 21:50:09 +0200
Subject: [PATCH] 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>
(cherry picked from commit b24515538cd31b6c2b64268465dd479f464b151d)
---
.../src/ackermann_steering_controller.cpp | 23 +--
.../src/bicycle_steering_controller.cpp | 10 +-
.../steering_odometry.hpp | 63 +++----
.../src/steering_odometry.cpp | 175 ++++++++----------
.../src/tricycle_steering_controller.cpp | 18 +-
5 files changed, 139 insertions(+), 150 deletions(-)
diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index c3a7539c5a..d9d95bf8b5 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 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());
}
}
}
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index 5f013d7d7a..95eaf1965c 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 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());
}
}
}
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 95bcef7e63..882d97f5dc 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -56,7 +56,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
*/
@@ -67,7 +67,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
*/
@@ -91,7 +91,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
*/
@@ -102,7 +102,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
*/
@@ -125,11 +125,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
@@ -170,22 +170,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
@@ -194,35 +195,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
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index bffa588bb8..feefbc89bc 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)
@@ -49,10 +51,10 @@ void SteeringOdometry::init(const rclcpp::Time & time)
}
bool SteeringOdometry::update_odometry(
- const double linear_velocity, const double angular, const double dt)
+ const double linear_velocity, const double angular_velocity, const double dt)
{
/// Integrate odometry:
- SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt);
+ integrate_fk(linear_velocity, angular_velocity, dt);
/// We cannot estimate the speed with very small time intervals:
if (dt < 0.0001)
@@ -62,7 +64,7 @@ bool SteeringOdometry::update_odometry(
/// Estimate speeds using a rolling mean to filter them out:
linear_acc_.accumulate(linear_velocity);
- angular_acc_.accumulate(angular);
+ angular_acc_.accumulate(angular_velocity);
linear_ = linear_acc_.getRollingMean();
angular_ = angular_acc_.getRollingMean();
@@ -73,70 +75,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 = tan(steer_pos) * linear_velocity / wheelbase_;
-
- return update_odometry(linear_velocity, angular, 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 = 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, 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;
-
- /// 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 = 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, 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(
@@ -144,9 +123,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 +136,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,19 +148,19 @@ 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)
+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:
- SteeringOdometry::integrate_exact(linear * dt, angular * dt);
+ integrate_fk(v_bx, omega_bz, dt);
}
void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)
@@ -200,36 +179,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_twist_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_twist_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)
@@ -242,12 +222,12 @@ 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 = {alpha};
+ steering_commands = {phi};
return std::make_tuple(traction_commands, steering_commands);
}
else if (config_type_ == ACKERMANN_CONFIG)
@@ -257,21 +237,23 @@ 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
{
- 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(alpha);
- double denominator_first_member = 2 * wheelbase_ * std::cos(alpha);
- double denominator_second_member = wheel_track_ * std::sin(alpha);
+ 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);
@@ -290,35 +272,36 @@ 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 dt)
{
- const double direction = heading_ + angular * 0.5;
+ // Compute intermediate value of the heading
+ const double theta_mid = heading_ + omega_bz * 0.5 * dt;
- /// Runge-Kutta 2nd order integration:
- x_ += linear * cos(direction);
- y_ += linear * sin(direction);
- heading_ += angular;
+ // 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;
}
-/**
- * \brief Other possible integration method provided by the class
- * \param linear
- * \param angular
- */
-void SteeringOdometry::integrate_exact(double linear, double angular)
+void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt)
{
- if (fabs(angular) < 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(linear, angular);
+ /// 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 angular is zero):
+ /// Exact integration
const double heading_old = heading_;
- const double r = linear / angular;
- heading_ += angular;
- 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));
}
}
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index f89d78a52c..03be6b88f0 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -60,24 +60,28 @@ 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 steer_position = state_interfaces_[STATE_STEER_AXIS].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_position = state_interfaces_[STATE_STEER_AXIS].get_value();
if (
- !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
- !std::isnan(steer_position))
+ std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+ std::isfinite(steering_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, steering_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, steering_position,
+ period.seconds());
}
}
}