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 }