bugfix: max_vel_y >= vy, unless proportional saturation

kenomo · corot · commit 0e839074c340 · 2022-06-01T17:47:26.000+09:00
diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp
@@ -878,7 +878,7 @@ void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega,
   
   // limit strafing velocity
   if (vy > max_vel_y || vy < -max_vel_y)
-    ratio_y = std::abs(vy / max_vel_y);
+    ratio_y = std::abs(max_vel_y / vy);
   
   // Limit angular velocity
   if (omega > max_vel_theta || omega < -max_vel_theta)
