ompl/tools/multiplan/src/ParallelPlan.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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() 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() 00066 { 00067 planners_.clear(); 00068 } 00069 00070 void ompl::tools::ParallelPlan::clearHybridizationPaths() 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, 00092 std::size_t maxSolCount, bool hybridize) 00093 { 00094 if (!pdef_->getSpaceInformation()->isSetup()) 00095 pdef_->getSpaceInformation()->setup(); 00096 foundSolCount_ = 0; 00097 00098 time::point start = time::now(); 00099 std::vector<boost::thread*> threads(planners_.size()); 00100 00101 // Decide if we are combining solutions or just taking the first one 00102 if (hybridize) 00103 for (std::size_t i = 0 ; i < threads.size() ; ++i) 00104 threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveMore, this, planners_[i].get(), minSolCount, maxSolCount, &ptc)); 00105 else 00106 for (std::size_t i = 0 ; i < threads.size() ; ++i) 00107 threads[i] = new boost::thread(boost::bind(&ParallelPlan::solveOne, this, planners_[i].get(), minSolCount, &ptc)); 00108 00109 for (std::size_t i = 0 ; i < threads.size() ; ++i) 00110 { 00111 threads[i]->join(); 00112 delete threads[i]; 00113 } 00114 00115 if (hybridize) 00116 { 00117 if (phybrid_->pathCount() > 1) 00118 if (const base::PathPtr &hsol = phybrid_->getHybridPath()) 00119 { 00120 geometric::PathGeometric *pg = static_cast<geometric::PathGeometric*>(hsol.get()); 00121 double difference = 0.0; 00122 bool approximate = !pdef_->getGoal()->isSatisfied(pg->getStates().back(), &difference); 00123 pdef_->addSolutionPath(hsol, approximate, difference, phybrid_->getName()); // name this solution after the hybridization algorithm 00124 } 00125 } 00126 00127 if (pdef_->hasSolution()) 00128 OMPL_INFORM("ParallelPlan::solve(): Solution found by one or more threads in %f seconds", time::seconds(time::now() - start)); 00129 else 00130 OMPL_WARN("ParallelPlan::solve(): Unable to find solution by any of the threads in %f seconds", time::seconds(time::now() - start)); 00131 00132 return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution()); 00133 } 00134 00135 void ompl::tools::ParallelPlan::solveOne(base::Planner *planner, std::size_t minSolCount, const base::PlannerTerminationCondition *ptc) 00136 { 00137 OMPL_DEBUG("ParallelPlan.solveOne starting planner %s", planner->getName().c_str()); 00138 00139 time::point start = time::now(); 00140 if (planner->solve(*ptc)) 00141 { 00142 double duration = time::seconds(time::now() - start); 00143 foundSolCountLock_.lock(); 00144 unsigned int nrSol = ++foundSolCount_; 00145 foundSolCountLock_.unlock(); 00146 if (nrSol >= minSolCount) 00147 ptc->terminate(); 00148 OMPL_DEBUG("ParallelPlan.solveOne: Solution found by %s in %lf seconds", planner->getName().c_str(), duration); 00149 } 00150 } 00151 00152 void ompl::tools::ParallelPlan::solveMore(base::Planner *planner, std::size_t minSolCount, std::size_t maxSolCount, 00153 const base::PlannerTerminationCondition *ptc) 00154 { 00155 OMPL_DEBUG("ParallelPlan.solveMore: starting planner %s", planner->getName().c_str()); 00156 00157 time::point start = time::now(); 00158 if (planner->solve(*ptc)) 00159 { 00160 double duration = time::seconds(time::now() - start); 00161 foundSolCountLock_.lock(); 00162 unsigned int nrSol = ++foundSolCount_; 00163 foundSolCountLock_.unlock(); 00164 00165 if (nrSol >= maxSolCount) 00166 ptc->terminate(); 00167 00168 OMPL_DEBUG("ParallelPlan.solveMore: Solution found by %s in %lf seconds", planner->getName().c_str(), duration); 00169 00170 const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions(); 00171 00172 boost::mutex::scoped_lock slock(phlock_); 00173 start = time::now(); 00174 unsigned int attempts = 0; 00175 for (std::size_t i = 0 ; i < paths.size() ; ++i) 00176 attempts += phybrid_->recordPath(paths[i].path_, false); 00177 00178 if (phybrid_->pathCount() >= minSolCount) 00179 phybrid_->computeHybridPath(); 00180 00181 duration = time::seconds(time::now() - start); 00182 OMPL_DEBUG("ParallelPlan.solveMore: Spent %f seconds hybridizing %u solution paths (attempted %u connections between paths)", duration, 00183 (unsigned int)phybrid_->pathCount(), attempts); 00184 } 00185 }