00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/tools/multiplan/ParallelPlan.h"
00038 #include "ompl/geometric/PathHybridization.h"
00039
00040 ompl::tools::ParallelPlan::ParallelPlan(const base::ProblemDefinitionPtr &pdef) :
00041 pdef_(pdef), phybrid_(new geometric::PathHybridization(pdef->getSpaceInformation()))
00042 {
00043 }
00044
00045 ompl::tools::ParallelPlan::~ParallelPlan(void)
00046 {
00047 }
00048
00049 void ompl::tools::ParallelPlan::addPlanner(const base::PlannerPtr &planner)
00050 {
00051 if (planner && planner->getSpaceInformation().get() != pdef_->getSpaceInformation().get())
00052 throw Exception("Planner instance does not match space information");
00053 if (planner->getProblemDefinition().get() != pdef_.get())
00054 planner->setProblemDefinition(pdef_);
00055 planners_.push_back(planner);
00056 }
00057
00058 void ompl::tools::ParallelPlan::addPlannerAllocator(const base::PlannerAllocator &pa)
00059 {
00060 base::PlannerPtr planner = pa(pdef_->getSpaceInformation());
00061 planner->setProblemDefinition(pdef_);
00062 planners_.push_back(planner);
00063 }
00064
00065 void ompl::tools::ParallelPlan::clearPlanners(void)
00066 {
00067 planners_.clear();
00068 }
00069
00070 void ompl::tools::ParallelPlan::clearHybridizationPaths(void)
00071 {
00072 phybrid_->clear();
00073 }
00074
00075 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(double solveTime, bool hybridize)
00076 {
00077 return solve(solveTime, 1, planners_.size(), hybridize);
00078 }
00079
00080 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(double solveTime, std::size_t minSolCount, std::size_t maxSolCount, bool hybridize)
00081 {
00082 return solve(base::timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)), minSolCount, maxSolCount, hybridize);
00083 }
00084
00085
00086 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(const base::PlannerTerminationCondition &ptc, bool hybridize)
00087 {
00088 return solve(ptc, 1, planners_.size(), hybridize);
00089 }
00090
00091 ompl::base::PlannerStatus ompl::tools::ParallelPlan::solve(const base::PlannerTerminationCondition &ptc, std::size_t minSolCount, std::size_t maxSolCount, bool hybridize)
00092 {
00093 if (!pdef_->getSpaceInformation()->isSetup())
00094 pdef_->getSpaceInformation()->setup();
00095 foundSolCount_ = 0;
00096
00097 time::point start = time::now();
00098 std::vector<boost::thread*> threads(planners_.size());
00099 if (hybridize)
00100 for (std::size_t i = 0 ; i < threads.size() ; ++i)
00101 threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveMore, this, planners_[i].get(), minSolCount, maxSolCount, &ptc));
00102 else
00103 for (std::size_t i = 0 ; i < threads.size() ; ++i)
00104 threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveOne, this, planners_[i].get(), minSolCount, &ptc));
00105
00106 for (std::size_t i = 0 ; i < threads.size() ; ++i)
00107 {
00108 threads[i]->join();
00109 delete threads[i];
00110 }
00111
00112 if (hybridize)
00113 {
00114 if (phybrid_->pathCount() > 1)
00115 if (const base::PathPtr &hsol = phybrid_->getHybridPath())
00116 {
00117 geometric::PathGeometric *pg = static_cast<geometric::PathGeometric*>(hsol.get());
00118 double difference = 0.0;
00119 bool approximate = !pdef_->getGoal()->isSatisfied(pg->getStates().back(), &difference);
00120 pdef_->addSolutionPath(hsol, approximate, difference);
00121 }
00122 }
00123
00124 logInform("Solution found in %f seconds", time::seconds(time::now() - start));
00125
00126 return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
00127 }
00128
00129 void ompl::tools::ParallelPlan::solveOne(base::Planner *planner, std::size_t minSolCount, const base::PlannerTerminationCondition *ptc)
00130 {
00131 logDebug("Starting %s", planner->getName().c_str());
00132 time::point start = time::now();
00133 if (planner->solve(*ptc))
00134 {
00135 double duration = time::seconds(time::now() - start);
00136 foundSolCountLock_.lock();
00137 unsigned int nrSol = ++foundSolCount_;
00138 foundSolCountLock_.unlock();
00139 if (nrSol >= minSolCount)
00140 ptc->terminate();
00141 logDebug("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
00142 }
00143 }
00144
00145 void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount, const base::PlannerTerminationCondition *ptc)
00146 {
00147 time::point start = time::now();
00148 if (planner->solve(*ptc))
00149 {
00150 double duration = time::seconds(time::now() - start);
00151 foundSolCountLock_.lock();
00152 unsigned int nrSol = ++foundSolCount_;
00153 foundSolCountLock_.unlock();
00154
00155 if (nrSol >= maxSolCount)
00156 ptc->terminate();
00157
00158 logDebug("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
00159
00160 const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
00161
00162 boost::mutex::scoped_lock slock(phlock_);
00163 start = time::now();
00164 unsigned int attempts = 0;
00165 for (std::size_t i = 0 ; i < paths.size() ; ++i)
00166 attempts += phybrid_->recordPath(paths[i].path_, false);
00167
00168 if (phybrid_->pathCount() >= minSolCount)
00169 phybrid_->computeHybridPath();
00170
00171 duration = time::seconds(time::now() - start);
00172 logDebug("Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration, (unsigned int)phybrid_->pathCount(), attempts);
00173 }
00174 }