iteratively reduce speed when using footprint scaling

graceful_controller_ros/cfg/GracefulController.cfg

@@ -41,5 +41,6 @@ gen.add("latch_xy_goal_tolerance", bool_t, 0, "When goal has been reached, just
 # Footprint scaling at higher speeds
 gen.add("scaling_vel_x", double_t, 0, "Above this velocity, the footprint will be scaled up", 0.5);
 gen.add("scaling_factor", double_t, 0, "Amount to scale footprint when at max velocity", 0.0);
+gen.add("scaling_step", double_t, 0, "Amount to reduce x velocity when iteratively reducing velocity", 0.1, 0.0, 1.0);
 
 exit(gen.generate("graceful_controller", "graceful_controller", "GracefulController"))

graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp

@@ -142,6 +142,7 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
   double acc_lim_theta_;
   double scaling_vel_x_;
   double scaling_factor_;
+  double scaling_step_;
   double xy_goal_tolerance_;
   double yaw_goal_tolerance_;
   double min_lookahead_;

graceful_controller_ros/src/graceful_controller_ros.cpp

@@ -262,8 +262,9 @@ void GracefulControllerROS::reconfigureCallback(GracefulControllerConfig& config
       std::make_shared<GracefulController>(config.k1, config.k2, config.min_vel_x, config.max_vel_x, config.acc_lim_x,
                                            config.max_vel_theta, config.beta, config.lambda);
 
-  scaling_vel_x_ = config.scaling_vel_x;
+  scaling_vel_x_ = std::max(config.scaling_vel_x, config.min_vel_x);
   scaling_factor_ = config.scaling_factor;
+  scaling_step_ = config.scaling_step;
 }
 
 bool GracefulControllerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
@@ -411,15 +412,22 @@ bool GracefulControllerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_ve
       break;
     }
 
-    // Configure controller max velocity
-    controller_->setVelocityLimits(min_vel_x_, max_vel_x, max_vel_theta_);
-
-    // Actually find our path
-    if (simulate(target_pose, cmd_vel))
+    // Iteratively try to find a path, incrementally reducing the velocity
+    double sim_velocity = max_vel_x;
+    do
     {
-      // Have valid command
-      return true;
+      // Configure controller max velocity
+      controller_->setVelocityLimits(min_vel_x_, sim_velocity, max_vel_theta_);
+      // Actually simulate our path
+      if (simulate(target_pose, cmd_vel))
+      {
+        // Have valid command
+        return true;
+      }
+      // Reduce velocity and try again for same target_pose
+      sim_velocity -= scaling_step_;
     }
+    while (sim_velocity >= scaling_vel_x_);
   }
 
   ROS_ERROR("No pose in path was reachable");