37 #include "ompl/tools/multiplan/OptimizePlan.h"
38 #include "ompl/geometric/PathSimplifier.h"
39 #include "ompl/base/OptimizationObjective.h"
43 if (planner && planner->getSpaceInformation().get() !=
getProblemDefinition()->getSpaceInformation().get())
44 throw Exception(
"Planner instance does not match space information");
50 planners_.push_back(pa(getProblemDefinition()->getSpaceInformation()));
61 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 base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective());
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);
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
void simplify(PathGeometric &path, double maxTime)
Run simplification algorithms on the path for at most maxTime seconds.
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
A boost shared pointer wrapper for ompl::base::Planner.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
The planner found an exact solution.
boost::posix_time::ptime point
Representation of a point in time.
A class to store the exit status of Planner::solve()
This class contains routines that attempt to simplify geometric paths.
The exception type for ompl.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition of a geometric path.
point now(void)
Get the current time point.