Skip to content

Commit

Permalink
Fix steering controllers library code documentation and naming (#1149)
Browse files Browse the repository at this point in the history
* 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 b245155)
  • Loading branch information
christophfroehlich authored and mergify[bot] committed Jun 5, 2024
1 parent 9262d74 commit 576fafb
Show file tree
Hide file tree
Showing 5 changed files with 139 additions and 150 deletions.
23 changes: 12 additions & 11 deletions ackermann_steering_controller/src/ackermann_steering_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}
}
Expand Down
10 changes: 5 additions & 5 deletions bicycle_steering_controller/src/bicycle_steering_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand All @@ -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
*/
Expand All @@ -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
*/
Expand All @@ -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
*/
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
Loading

0 comments on commit 576fafb

Please sign in to comment.