SimpleScoredSamplingPlanner extended with parameter that allows to …

…try fallback generators even if valid trajectories were found

- default value is set to false which does not change behavior compared to the current version
- extra ctor arg is equipped with default value so API of the present applications does not change

base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h

@@ -65,13 +65,19 @@ class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch
   /**
    * Takes a list of generators and critics. Critics return costs > 0, or negative costs for invalid trajectories.
    * Generators other than the first are fallback generators,  meaning they only get to generate if the previous
-   * generator did not find a valid trajectory.
+   * generator did not find a valid trajectory. The user may force fallback generators to be utilized
+   * if allow_fallback_generators argument is set to true (false by default).
    * Will use every generator until it stops returning trajectories or count reaches max_samples.
    * Then resets count and tries for the next in the list.
    * passing max_samples = -1 (default): Each Sampling planner will continue to call
    * generator until generator runs out of samples (or forever if that never happens)
    */
-  SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples = -1);
+  SimpleScoredSamplingPlanner(
+    std::vector<TrajectorySampleGenerator*> gen_list,
+    std::vector<TrajectoryCostFunction*>& critics,
+    int max_samples = -1,
+    bool allow_fallback_generators = false
+  );
 
   /**
    * runs all scoring functions over the trajectory creating a weigthed sum
@@ -99,6 +105,7 @@ class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch
   std::vector<TrajectoryCostFunction*> critics_;
 
   int max_samples_;
+  bool allow_fallback_generators_;
 };
 
 

base_local_planner/src/simple_scored_sampling_planner.cpp

@@ -41,8 +41,13 @@
 
 namespace base_local_planner {
 
-  SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples) {
+  SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(
+    std::vector<TrajectorySampleGenerator*> gen_list,
+    std::vector<TrajectoryCostFunction*>& critics,
+    int max_samples,
+    bool allow_fallback_generators) {
     max_samples_ = max_samples;
+    allow_fallback_generators_ = allow_fallback_generators;
     gen_list_ = gen_list;
     critics_ = critics;
   }
@@ -133,7 +138,7 @@ namespace base_local_planner {
         }
       }
       ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
-      if (best_traj_cost >= 0) {
+      if (best_traj_cost >= 0 && !allow_fallback_generators_) {
         // do not try fallback generators
         break;
       }