Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

SimpleScoredSamplingPlanner- try fallback generators even if valid trajectories were found (option) #1201

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open

SimpleScoredSamplingPlanner- try fallback generators even if valid trajectories were found (option) #1201

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 9 additions & 2 deletions base_local_planner/include/base_local_planner/simple_scored_sampling_planner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 usage of fallback generators by setting
* force_usage_all_generators argument 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 force_usage_all_generators = false
);

/**
* runs all scoring functions over the trajectory creating a weigthed sum
Expand Down Expand Up @@ -99,6 +105,7 @@ class SimpleScoredSamplingPlanner : public base_local_planner::TrajectorySearch
std::vector<TrajectoryCostFunction*> critics_;

int max_samples_;
bool force_usage_all_generators_;
};


Expand Down
9 changes: 7 additions & 2 deletions base_local_planner/src/simple_scored_sampling_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 force_usage_all_generators) {
max_samples_ = max_samples;
force_usage_all_generators_ = force_usage_all_generators;
gen_list_ = gen_list;
critics_ = critics;
}
Expand Down Expand Up @@ -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 && !force_usage_all_generators_) {
// do not try fallback generators
break;
}
Expand Down