37 #include "ompl/tools/multiplan/ParallelPlan.h"
38 #include "ompl/geometric/PathHybridization.h"
41 pdef_(pdef), phybrid_(new geometric::PathHybridization(pdef->getSpaceInformation()))
45 ompl::tools::ParallelPlan::~ParallelPlan(
void)
51 if (planner && planner->getSpaceInformation().get() != pdef_->getSpaceInformation().get())
52 throw Exception(
"Planner instance does not match space information");
53 if (planner->getProblemDefinition().get() != pdef_.get())
54 planner->setProblemDefinition(pdef_);
55 planners_.push_back(planner);
61 planner->setProblemDefinition(pdef_);
62 planners_.push_back(planner);
77 return solve(solveTime, 1, planners_.size(), hybridize);
88 return solve(ptc, 1, planners_.size(), hybridize);
93 if (!pdef_->getSpaceInformation()->isSetup())
94 pdef_->getSpaceInformation()->setup();
98 std::vector<boost::thread*> threads(planners_.size());
100 for (std::size_t i = 0 ; i < threads.size() ; ++i)
101 threads[i] =
new boost::thread(boost::bind(&
ParallelPlan::solveMore,
this, planners_[i].
get(), minSolCount, maxSolCount, &ptc));
103 for (std::size_t i = 0 ; i < threads.size() ; ++i)
104 threads[i] =
new boost::thread(boost::bind(&
ParallelPlan::solveOne,
this, planners_[i].
get(), minSolCount, &ptc));
106 for (std::size_t i = 0 ; i < threads.size() ; ++i)
114 if (phybrid_->pathCount() > 1)
118 double difference = 0.0;
119 bool approximate = !pdef_->getGoal()->isSatisfied(pg->
getStates().back(), &difference);
120 pdef_->addSolutionPath(hsol, approximate, difference);
133 if (planner->
solve(*ptc))
136 foundSolCountLock_.lock();
137 unsigned int nrSol = ++foundSolCount_;
138 foundSolCountLock_.unlock();
139 if (nrSol >= minSolCount)
141 OMPL_DEBUG(
"Solution found by %s in %lf seconds", planner->
getName().c_str(), duration);
148 if (planner->
solve(*ptc))
151 foundSolCountLock_.lock();
152 unsigned int nrSol = ++foundSolCount_;
153 foundSolCountLock_.unlock();
155 if (nrSol >= maxSolCount)
158 OMPL_DEBUG(
"Solution found by %s in %lf seconds", planner->
getName().c_str(), duration);
160 const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
162 boost::mutex::scoped_lock slock(phlock_);
164 unsigned int attempts = 0;
165 for (std::size_t i = 0 ; i < paths.size() ; ++i)
166 attempts += phybrid_->recordPath(paths[i].path_,
false);
168 if (phybrid_->pathCount() >= minSolCount)
169 phybrid_->computeHybridPath();
172 OMPL_DEBUG(
"Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration, (
unsigned int)phybrid_->pathCount(), attempts);
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
const std::string & getName(void) const
Get the name of the planner.
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void terminate(void) const
Notify that the condition for termination should become true, regardless of what eval() returns...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
A boost shared pointer wrapper for ompl::base::Planner.
Base class for a planner.
boost::posix_time::ptime point
Representation of a point in time.
A class to store the exit status of Planner::solve()
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
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.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.