iteratively reduce speed when using footprint scaling

graceful_controller_ros/cfg/GracefulController.cfg

@@ -7,8 +7,6 @@ gen = ParameterGenerator()
 
 gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.5)
 gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.1)
-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("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s",  1.0, 0)
 gen.add("min_in_place_vel_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.4, 0, 20.0)
@@ -40,4 +38,9 @@ gen.add("yaw_gap_tolerance", double_t, 0, "Maximum distance between poses in the
 # Goal tolerance latch
 gen.add("latch_xy_goal_tolerance", bool_t, 0, "When goal has been reached, just fix heading", False)
 
+# Footprint scaling parameters
+gen.add("scaling_vel_x", double_t, 0, "Above this velocity, the footprint will be scaled up", 0.5, 0.0);
+gen.add("scaling_factor", double_t, 0, "Amount to scale footprint when at max velocity", 0.0, 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/src/graceful_controller_ros.cpp

@@ -260,8 +260,9 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
                                                        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;
   }
 
   /**
@@ -412,12 +413,19 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
         break;
       }
 
-      // Configure controller max velocity
-      controller_->setVelocityLimits(min_vel_x_, max_vel_x, max_vel_theta_);
-      if (simulate(target_pose, cmd_vel))
+      // Iteratively try to find a path, incrementally reducing the velocity
+      double sim_velocity = max_vel_x;
+      do
       {
-        return true;
+        // Configure controller max velocity
+        controller_->setVelocityLimits(min_vel_x_, sim_velocity, max_vel_theta_);
+        if (simulate(target_pose, cmd_vel))
+        {
+          return true;
+        }
+        sim_velocity -= scaling_step_;
       }
+      while (sim_velocity >= scaling_vel_x_);
     }
 
     ROS_ERROR("No pose in path was reachable");
@@ -714,6 +722,7 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
   double min_vel_x_;
   double scaling_vel_x_;
   double scaling_factor_;
+  double scaling_step_;
   double max_vel_theta_;
   double min_in_place_vel_theta_;
   double acc_lim_x_;