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

ros-controls · Sep 11, 2024 · 1dc3d2a · 1dc3d2a
1 parent 48a7f8b
commit 1dc3d2a
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
@@ -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;
 }