fix(steering-odometry): handle infinite turning radius properly (#1285)…

… (#1287) (cherry picked from commit 1dc3d2a) Co-authored-by: Rein Appeldoorn <[email protected]>
ros-controls · Sep 12, 2024 · 1d9c5c3 · 1d9c5c3
1 parent c83b3bb
commit 1d9c5c3
Showing 1 changed file with 10 additions and 4 deletions.
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
@@ -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;
 }