protect against div by zero

mikeferguson · May 10, 2022 · e59619d · e59619d
1 parent 84dd27f
commit e59619d
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/graceful_controller_ros/src/graceful_controller_ros.cpp b/graceful_controller_ros/src/graceful_controller_ros.cpp
@@ -544,7 +544,11 @@ bool GracefulControllerROS::simulate(const geometry_msgs::PoseStamped& target_po
     double footprint_scaling = 1.0;
     if (vel_x > scaling_vel_x_)
     {
-      double ratio = (vel_x - scaling_vel_x_) / (max_vel_x_ - scaling_vel_x_);
+      double ratio = max_vel_x_ - scaling_vel_x_;
+      if (ratio > 0)
+      {
+        ratio = (vel_x - scaling_vel_x_) / ratio;
+      }
       footprint_scaling += ratio * scaling_factor_;
     }