ompl/geometric/planners/cforest/src/CForest.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, Rice University
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 Rice University 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 /* Authors: Javier V. Gómez, Ioan Sucan, Mark Moll */
00036 
00037 #include "ompl/geometric/planners/cforest/CForest.h"
00038 #include "ompl/geometric/planners/rrt/RRTstar.h"
00039 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00040 
00041 ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
00042 {
00043     specs_.optimizingPaths = true;
00044     specs_.multithreaded = true;
00045 
00046     numPathsShared_ = 0;
00047     numStatesShared_ = 0;
00048     prune_ = true;
00049 
00050     numThreads_ = std::max(boost::thread::hardware_concurrency(), 2u);
00051     Planner::declareParam<bool>("prune", this, &CForest::setPrune, &CForest::getPrune, "0,1");
00052     Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
00053 
00054     addPlannerProgressProperty("best cost REAL",
00055                                boost::bind(&CForest::getBestCost, this));
00056     addPlannerProgressProperty("shared paths INTEGER",
00057                                boost::bind(&CForest::getNumPathsShared, this));
00058     addPlannerProgressProperty("shared states INTEGER",
00059                                boost::bind(&CForest::getNumStatesShared, this));
00060 }
00061 
00062 ompl::geometric::CForest::~CForest()
00063 {
00064 }
00065 
00066 void ompl::geometric::CForest::setNumThreads(unsigned int numThreads)
00067 {
00068     numThreads_ = numThreads ? numThreads : std::max(boost::thread::hardware_concurrency(), 2u);
00069 }
00070 
00071 void ompl::geometric::CForest::addPlannerInstanceInternal(const base::PlannerPtr &planner)
00072 {
00073     if (!planner->getSpecs().canReportIntermediateSolutions)
00074         OMPL_WARN("%s cannot report intermediate solutions, not added as CForest planner.", planner->getName().c_str());
00075     else
00076     {
00077         planner->setProblemDefinition(pdef_);
00078         if (planner->params().hasParam("prune"))
00079             planner->params()["prune"] = prune_;
00080         planners_.push_back(planner);
00081     }
00082 }
00083 
00084 void ompl::geometric::CForest::getPlannerData(base::PlannerData &data) const
00085 {
00086     Planner::getPlannerData(data);
00087 
00088     for (std::size_t i = 0 ; i < planners_.size() ; ++i)
00089     {
00090         base::PlannerData pd(si_);
00091         planners_[i]->getPlannerData(pd);
00092 
00093         for (unsigned int j = 0; j < pd.numVertices(); ++j)
00094         {
00095             base::PlannerDataVertex &v = pd.getVertex(j);
00096 
00097             v.setTag(i);
00098             std::vector<unsigned int> edgeList;
00099             unsigned int numEdges = pd.getIncomingEdges(j, edgeList);
00100             for (unsigned int k = 0; k <numEdges; ++k)
00101             {
00102                 base::Cost edgeWeight;
00103                 base::PlannerDataVertex &w = pd.getVertex(edgeList[k]);
00104 
00105                 w.setTag(i);
00106                 pd.getEdgeWeight(j, k, &edgeWeight);
00107                 data.addEdge(v, w, pd.getEdge(j, k), edgeWeight);
00108             }
00109         }
00110 
00111         for (unsigned int j = 0; j < pd.numGoalVertices(); ++j)
00112             data.markGoalState(pd.getGoalVertex(j).getState());
00113 
00114         for (unsigned int j = 0; j < pd.numStartVertices(); ++j)
00115             data.markStartState(pd.getStartVertex(j).getState());
00116     }
00117 }
00118 
00119 void ompl::geometric::CForest::clear()
00120 {
00121     Planner::clear();
00122     for (std::size_t i = 0; i < planners_.size(); ++i)
00123         planners_[i]->clear();
00124 
00125     bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
00126     numPathsShared_ = 0;
00127     numStatesShared_ = 0;
00128 
00129     std::vector<base::StateSamplerPtr> samplers;
00130     samplers.reserve(samplers_.size());
00131     for (std::size_t i = 0; i < samplers_.size(); ++i)
00132         if (samplers_[i].use_count() > 1)
00133             samplers.push_back(samplers_[i]);
00134     samplers_.swap(samplers);
00135 }
00136 
00137 void ompl::geometric::CForest::setup()
00138 {
00139     Planner::setup();
00140     if (pdef_->hasOptimizationObjective())
00141         opt_ = pdef_->getOptimizationObjective();
00142     else
00143     {
00144         OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
00145         opt_.reset(new base::PathLengthOptimizationObjective(si_));
00146     }
00147 
00148     bestCost_ = opt_->infiniteCost();
00149 
00150     if (planners_.empty())
00151     {
00152         OMPL_INFORM("%s: Number and type of instances not specified. Defaulting to %d instances of RRTstar.", getName().c_str(), numThreads_);
00153         addPlannerInstances<RRTstar>(numThreads_);
00154     }
00155 
00156     for (std::size_t i = 0; i < planners_.size() ; ++i)
00157         if (!planners_[i]->isSetup())
00158             planners_[i]->setup();
00159 
00160     // This call is needed to make sure the ParamSet is up to date after changes induced by the planner setup calls above, via the state space wrappers for CForest.
00161     si_->setup();
00162 }
00163 
00164 ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc)
00165 {
00166     checkValidity();
00167 
00168     time::point start = time::now();
00169     std::vector<boost::thread*> threads(planners_.size());
00170     const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback();
00171 
00172     if (prevSolutionCallback)
00173         OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
00174 
00175     pdef_->setIntermediateSolutionCallback(boost::bind(&CForest::newSolutionFound, this, _1, _2, _3));
00176     bestCost_ = opt_->infiniteCost();
00177 
00178     // run each planner in its own thread, with the same ptc.
00179     for (std::size_t i = 0 ; i < threads.size() ; ++i)
00180         threads[i] = new boost::thread(boost::bind(&CForest::solve, this, planners_[i].get(), ptc));
00181 
00182     for (std::size_t i = 0 ; i < threads.size() ; ++i)
00183     {
00184         threads[i]->join();
00185         delete threads[i];
00186     }
00187 
00188     // restore callback
00189     getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
00190     OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
00191     return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
00192 }
00193 
00194 std::string ompl::geometric::CForest::getBestCost() const
00195 {
00196     return boost::lexical_cast<std::string>(bestCost_);
00197 }
00198 
00199 std::string ompl::geometric::CForest::getNumPathsShared() const
00200 {
00201     return boost::lexical_cast<std::string>(numPathsShared_);
00202 }
00203 
00204 std::string ompl::geometric::CForest::getNumStatesShared() const
00205 {
00206     return boost::lexical_cast<std::string>(numStatesShared_);
00207 }
00208 
00209 void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost)
00210 {
00211     bool change = false;
00212     std::vector<const base::State *> statesToShare;
00213     newSolutionFoundMutex_.lock();
00214     if (opt_->isCostBetterThan(cost, bestCost_))
00215     {
00216         ++numPathsShared_;
00217         bestCost_ = cost;
00218         change = true;
00219 
00220         // Filtering the states to add only those not already added.
00221         statesToShare.reserve(states.size());
00222         for (std::vector<const base::State *>::const_iterator st = states.begin(); st != states.end(); ++st)
00223         {
00224             if (statesShared_.find(*st) == statesShared_.end())
00225             {
00226                 statesShared_.insert(*st);
00227                 statesToShare.push_back(*st);
00228                 ++numStatesShared_;
00229             }
00230         }
00231      }
00232      newSolutionFoundMutex_.unlock();
00233 
00234     if (!change || statesToShare.empty()) return;
00235 
00236     for (std::size_t i = 0; i < samplers_.size(); ++i)
00237     {
00238         base::CForestStateSampler *sampler = static_cast<base::CForestStateSampler*>(samplers_[i].get());
00239         const base::CForestStateSpaceWrapper *space = static_cast<const base::CForestStateSpaceWrapper*>(sampler->getStateSpace());
00240         const base::Planner *cfplanner = space->getPlanner();
00241         if (cfplanner != planner)
00242             sampler->setStatesToSample(statesToShare);
00243     }
00244 }
00245 
00246 void ompl::geometric::CForest::solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
00247 {
00248     OMPL_DEBUG("Starting %s", planner->getName().c_str());
00249     time::point start = time::now();
00250     if (planner->solve(ptc))
00251     {
00252         double duration = time::seconds(time::now() - start);
00253         OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
00254     }
00255 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines