Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 1 commit into from
Jun 5, 2024
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 12 additions & 11 deletions ackermann_steering_controller/src/ackermann_steering_controller.cpp
View file
Open in desktop
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
View file
Open in desktop
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
63 changes: 32 additions & 31 deletions steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
View file
Open in desktop
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
Loading