From 1dc3d2aa47bb7746f5a414d016e9566c9eef4060 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 11 Sep 2024 14:18:40 +0200 Subject: [PATCH] fix(steering-odometry): handle infinite turning radius properly (#1285) --- .../src/steering_odometry.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index ba431faf33..824ec86f59 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -133,11 +133,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; }