diff --git a/base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h b/base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h index 8e4aff7f31..1887b3a77f 100644 --- a/base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h +++ b/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 gen_list, std::vector& critics, int max_samples = -1); + SimpleScoredSamplingPlanner( + std::vector gen_list, + std::vector& 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 critics_; int max_samples_; + bool allow_fallback_generators_; }; diff --git a/base_local_planner/src/simple_scored_sampling_planner.cpp b/base_local_planner/src/simple_scored_sampling_planner.cpp index 61e191c238..f368d0d978 100644 --- a/base_local_planner/src/simple_scored_sampling_planner.cpp +++ b/base_local_planner/src/simple_scored_sampling_planner.cpp @@ -41,8 +41,13 @@ namespace base_local_planner { - SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(std::vector gen_list, std::vector& critics, int max_samples) { + SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner( + std::vector gen_list, + std::vector& 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; }