37 #include "ompl/tools/multiplan/OptimizePlan.h"
38 #include "ompl/geometric/PathSimplifier.h"
42 if (planner && planner->getSpaceInformation().get() !=
getProblemDefinition()->getSpaceInformation().get())
43 throw Exception(
"Planner instance does not match space information");
49 planners_.push_back(pa(getProblemDefinition()->getSpaceInformation()));
60 unsigned int nt = std::min(nthreads, (
unsigned int)planners_.size());
67 pp_.clearHybridizationPaths();
72 for (
unsigned int i = 0 ; i < nt ; ++i)
74 planners_[np]->clear();
75 pp_.addPlanner(planners_[np]);
76 np = (np + 1) % planners_.size();
84 if (!pdef->hasOptimizationObjective())
86 OMPL_DEBUG(
"Terminating early since there is no optimization objective specified");
90 double obj_cost = pdef->getOptimizationObjective()->getCost(pdef->getSolutionPath());
92 if (pdef->getOptimizationObjective()->isSatisfied(obj_cost))
94 OMPL_DEBUG(
"Terminating early since solution path satisfies the optimization objective");
97 if (pdef->getSolutionCount() >= maxSol)
99 OMPL_DEBUG(
"Terminating early since %u solutions were generated", maxSol);