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 #1149

Merged
merged 14 commits into from
Jun 5, 2024
Merged

Fix steering controllers library code documentation and naming #1149

Changes from 1 commit
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
78d6004
Update documentation and consolidate variable names
christophfroehlich May 24, 2024
759264b
Simplify private methods and further update docs
christophfroehlich May 24, 2024
f85fc5b
Rename methods
christophfroehlich May 24, 2024
8602f49
Rename method and variables
christophfroehlich May 24, 2024
bf9998b
Rename convert method
christophfroehlich May 24, 2024
2531973
Rename variables and improve doc
christophfroehlich May 24, 2024
77212df
Rename local variables
christophfroehlich May 25, 2024
e115836
Use std::isfinite instead of !isnan
christophfroehlich May 27, 2024
6942c1a
Use a lowercase theta for heading
christophfroehlich May 27, 2024
4e5653d
Make some temporary variables const
christophfroehlich May 27, 2024
f0b2780
Let update_from_position call update_from_velocity
christophfroehlich May 25, 2024
d8ce400
Explicitly set variables with 0 in constructor
christophfroehlich May 28, 2024
479d850
Fix docstring
christophfroehlich May 29, 2024
089a803
Apply consistent variable naming
christophfroehlich Jun 5, 2024
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
55 changes: 16 additions & 39 deletions steering_controllers_library/src/steering_odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,70 +73,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_;
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved

/// 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_velocity = tan(steer_pos) * linear_velocity / wheelbase_;

return update_odometry(linear_velocity, angular_velocity, dt);
return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt);
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
}

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_;
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved

/// 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_velocity = 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_velocity, dt);
return update_from_velocity(
traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt);
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
}

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;
traction_right_wheel_old_pos_ = traction_right_wheel_pos;
traction_left_wheel_old_pos_ = traction_left_wheel_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_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;

return update_odometry(linear_velocity, angular_velocity, 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);
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
}

bool SteeringOdometry::update_from_velocity(
Expand Down