diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 2a5a710a..7c8ca4a8 100644 --- 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)