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
(cherry picked from commit 1dc3d2a)
  • Loading branch information
reinzor authored and mergify[bot] committed Sep 11, 2024
1 parent 8df49cb commit 57f1504
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 57f1504

Please sign in to comment.