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

… (#1286)

(cherry picked from commit 1dc3d2a)

Co-authored-by: Rein Appeldoorn <reinzor@gmail.com>

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;
 }