All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/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(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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines