Skip to content

Commit 576fafb

Browse files
christophfroehlichsaikishorqinqon
authored and
mergify[bot]
committed
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 b245155)
1 parent 9262d74 commit 576fafb

File tree

5 files changed

+139
-150
lines changed

5 files changed

+139
-150
lines changed

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp‎

Lines changed: 12 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
6262
}
6363
else
6464
{
65-
const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
66-
const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
67-
const double front_right_steer_position =
68-
state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
69-
const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
65+
const double traction_right_wheel_value =
66+
state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
67+
const double traction_left_wheel_value =
68+
state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
69+
const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
70+
const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
7071
if (
71-
!std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
72-
!std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
72+
std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
73+
std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
7374
{
7475
if (params_.position_feedback)
7576
{
7677
// Estimate linear and angular velocity using joint information
7778
odometry_.update_from_position(
78-
rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
79-
front_left_steer_position, period.seconds());
79+
traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
80+
steering_left_position, period.seconds());
8081
}
8182
else
8283
{
8384
// Estimate linear and angular velocity using joint information
8485
odometry_.update_from_velocity(
85-
rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
86-
front_left_steer_position, period.seconds());
86+
traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
87+
steering_left_position, period.seconds());
8788
}
8889
}
8990
}

‎bicycle_steering_controller/src/bicycle_steering_controller.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
6060
}
6161
else
6262
{
63-
const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
64-
const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
65-
if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position))
63+
const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
64+
const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
65+
if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
6666
{
6767
if (params_.position_feedback)
6868
{
6969
// Estimate linear and angular velocity using joint information
70-
odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds());
70+
odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds());
7171
}
7272
else
7373
{
7474
// Estimate linear and angular velocity using joint information
75-
odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds());
75+
odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds());
7676
}
7777
}
7878
}

‎steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp‎

Lines changed: 32 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ class SteeringOdometry
5656
/**
5757
* \brief Updates the odometry class with latest wheels position
5858
* \param traction_wheel_pos traction wheel position [rad]
59-
* \param steer_pos Front Steer position [rad]
59+
* \param steer_pos Steer wheel position [rad]
6060
* \param dt time difference to last call
6161
* \return true if the odometry is actually updated
6262
*/
@@ -67,7 +67,7 @@ class SteeringOdometry
6767
* \brief Updates the odometry class with latest wheels position
6868
* \param right_traction_wheel_pos Right traction wheel velocity [rad]
6969
* \param left_traction_wheel_pos Left traction wheel velocity [rad]
70-
* \param front_steer_pos Steer wheel position [rad]
70+
* \param steer_pos Steer wheel position [rad]
7171
* \param dt time difference to last call
7272
* \return true if the odometry is actually updated
7373
*/
@@ -91,7 +91,7 @@ class SteeringOdometry
9191
/**
9292
* \brief Updates the odometry class with latest wheels position
9393
* \param traction_wheel_vel Traction wheel velocity [rad/s]
94-
* \param front_steer_pos Steer wheel position [rad]
94+
* \param steer_pos Steer wheel position [rad]
9595
* \param dt time difference to last call
9696
* \return true if the odometry is actually updated
9797
*/
@@ -102,7 +102,7 @@ class SteeringOdometry
102102
* \brief Updates the odometry class with latest wheels position
103103
* \param right_traction_wheel_vel Right traction wheel velocity [rad/s]
104104
* \param left_traction_wheel_vel Left traction wheel velocity [rad/s]
105-
* \param front_steer_pos Steer wheel position [rad]
105+
* \param steer_pos Steer wheel position [rad]
106106
* \param dt time difference to last call
107107
* \return true if the odometry is actually updated
108108
*/
@@ -125,11 +125,11 @@ class SteeringOdometry
125125

126126
/**
127127
* \brief Updates the odometry class with latest velocity command
128-
* \param linear Linear velocity [m/s]
129-
* \param angular Angular velocity [rad/s]
130-
* \param time Current time
128+
* \param v_bx Linear velocity [m/s]
129+
* \param omega_bz Angular velocity [rad/s]
130+
* \param dt time difference to last call
131131
*/
132-
void update_open_loop(const double linear, const double angular, const double dt);
132+
void update_open_loop(const double v_bx, const double omega_bz, const double dt);
133133

134134
/**
135135
* \brief Set odometry type
@@ -170,22 +170,23 @@ class SteeringOdometry
170170
/**
171171
* \brief Sets the wheel parameters: radius, separation and wheelbase
172172
*/
173-
void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0);
173+
void set_wheel_params(
174+
const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0);
174175

175176
/**
176177
* \brief Velocity rolling window size setter
177178
* \param velocity_rolling_window_size Velocity rolling window size
178179
*/
179-
void set_velocity_rolling_window_size(size_t velocity_rolling_window_size);
180+
void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size);
180181

181182
/**
182183
* \brief Calculates inverse kinematics for the desired linear and angular velocities
183-
* \param Vx Desired linear velocity [m/s]
184-
* \param theta_dot Desired angular velocity [rad/s]
184+
* \param v_bx Desired linear velocity of the robot in x_b-axis direction
185+
* \param omega_bz Desired angular velocity of the robot around x_z-axis
185186
* \return Tuple of velocity commands and steering commands
186187
*/
187188
std::tuple<std::vector<double>, std::vector<double>> get_commands(
188-
const double Vx, const double theta_dot);
189+
const double v_bx, const double omega_bz);
189190

190191
/**
191192
* \brief Reset poses, heading, and accumulators
@@ -194,35 +195,35 @@ class SteeringOdometry
194195

195196
private:
196197
/**
197-
* \brief Uses precomputed linear and angular velocities to compute dometry and update
198-
* accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt)
199-
* computed by previous odometry method \param angular Angular velocity [rad] (angular
200-
* displacement, i.e. m/s * dt) computed by previous odometry method
198+
* \brief Uses precomputed linear and angular velocities to compute odometry
199+
* \param v_bx Linear velocity [m/s]
200+
* \param omega_bz Angular velocity [rad/s]
201+
* \param dt time difference to last call
201202
*/
202-
bool update_odometry(const double linear_velocity, const double angular, const double dt);
203+
bool update_odometry(const double v_bx, const double omega_bz, const double dt);
203204

204205
/**
205206
* \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
206-
* \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by
207-
* encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed
208-
* by encoders
207+
* \param v_bx Linear velocity [m/s]
208+
* \param omega_bz Angular velocity [rad/s]
209+
* \param dt time difference to last call
209210
*/
210-
void integrate_runge_kutta_2(double linear, double angular);
211+
void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt);
211212

212213
/**
213-
* \brief Integrates the velocities (linear and angular) using exact method
214-
* \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by
215-
* encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed
216-
* by encoders
214+
* \brief Integrates the velocities (linear and angular)
215+
* \param v_bx Linear velocity [m/s]
216+
* \param omega_bz Angular velocity [rad/s]
217+
* \param dt time difference to last call
217218
*/
218-
void integrate_exact(double linear, double angular);
219+
void integrate_fk(const double v_bx, const double omega_bz, const double dt);
219220

220221
/**
221-
* \brief Calculates steering angle from the desired translational and rotational velocity
222-
* \param Vx Linear velocity [m]
223-
* \param theta_dot Angular velocity [rad]
222+
* \brief Calculates steering angle from the desired twist
223+
* \param v_bx Linear velocity of the robot in x_b-axis direction
224+
* \param omega_bz Angular velocity of the robot around x_z-axis
224225
*/
225-
double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot);
226+
double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
226227

227228
/**
228229
* \brief Reset linear and angular accumulators

0 commit comments

Comments
 (0)