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 logDebug(
"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 logDebug(
"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 logDebug(
"Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration, (
unsigned int)phybrid_->pathCount(), attempts);