Skip to content

Commit

Permalink
fix(steering-odometry): handle infinite turning radius properly (#1285)…
Browse files Browse the repository at this point in the history
… (#1287)

(cherry picked from commit 1dc3d2a)

Co-authored-by: Rein Appeldoorn <[email protected]>
  • Loading branch information
mergify[bot] and reinzor authored Sep 12, 2024
1 parent c83b3bb commit 1d9c5c3
Showing 1 changed file with 10 additions and 4 deletions.
14 changes: 10 additions & 4 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,11 +134,17 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle(
const double steer_pos)
{
double turning_radius = wheelbase_ / std::tan(steer_pos);
const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_;
const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_;

if (std::isinf(turning_radius))
{
return (vel_wheel_r + vel_wheel_l) * 0.5;
}

// overdetermined, we take the average
double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius /
(turning_radius + wheel_track_ * 0.5);
double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius /
(turning_radius - wheel_track_ * 0.5);
const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5);
const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5);
return (vel_r + vel_l) * 0.5;
}

Expand Down

0 comments on commit 1d9c5c3

Please sign in to comment.