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