CForest.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Authors: Javier V. Gómez, Ioan Sucan, Mark Moll */
36 
37 #include "ompl/geometric/planners/cforest/CForest.h"
38 #include "ompl/geometric/planners/rrt/RRTstar.h"
39 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
40 
41 ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
42 {
43  specs_.optimizingPaths = true;
44  specs_.multithreaded = true;
45 
46  numPathsShared_ = 0;
47  numStatesShared_ = 0;
48  prune_ = true;
49 
50  numThreads_ = std::max(boost::thread::hardware_concurrency(), 2u);
51  Planner::declareParam<bool>("prune", this, &CForest::setPrune, &CForest::getPrune, "0,1");
52  Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
53 
54  addPlannerProgressProperty("best cost REAL",
55  boost::bind(&CForest::getBestCost, this));
56  addPlannerProgressProperty("shared paths INTEGER",
57  boost::bind(&CForest::getNumPathsShared, this));
58  addPlannerProgressProperty("shared states INTEGER",
59  boost::bind(&CForest::getNumStatesShared, this));
60 }
61 
62 ompl::geometric::CForest::~CForest()
63 {
64 }
65 
66 void ompl::geometric::CForest::setNumThreads(unsigned int numThreads)
67 {
68  numThreads_ = numThreads ? numThreads : std::max(boost::thread::hardware_concurrency(), 2u);
69 }
70 
71 void ompl::geometric::CForest::addPlannerInstanceInternal(const base::PlannerPtr &planner)
72 {
73  if (!planner->getSpecs().canReportIntermediateSolutions)
74  OMPL_WARN("%s cannot report intermediate solutions, not added as CForest planner.", planner->getName().c_str());
75  else
76  {
77  planner->setProblemDefinition(pdef_);
78  if (planner->params().hasParam("prune"))
79  planner->params()["prune"] = prune_;
80  planners_.push_back(planner);
81  }
82 }
83 
85 {
87 
88  for (std::size_t i = 0 ; i < planners_.size() ; ++i)
89  {
90  base::PlannerData pd(si_);
91  planners_[i]->getPlannerData(pd);
92 
93  for (unsigned int j = 0; j < pd.numVertices(); ++j)
94  {
96 
97  v.setTag(i);
98  std::vector<unsigned int> edgeList;
99  unsigned int numEdges = pd.getIncomingEdges(j, edgeList);
100  for (unsigned int k = 0; k <numEdges; ++k)
101  {
102  base::Cost edgeWeight;
103  base::PlannerDataVertex &w = pd.getVertex(edgeList[k]);
104 
105  w.setTag(i);
106  pd.getEdgeWeight(j, k, &edgeWeight);
107  data.addEdge(v, w, pd.getEdge(j, k), edgeWeight);
108  }
109  }
110 
111  for (unsigned int j = 0; j < pd.numGoalVertices(); ++j)
112  data.markGoalState(pd.getGoalVertex(j).getState());
113 
114  for (unsigned int j = 0; j < pd.numStartVertices(); ++j)
115  data.markStartState(pd.getStartVertex(j).getState());
116  }
117 }
118 
120 {
121  Planner::clear();
122  for (std::size_t i = 0; i < planners_.size(); ++i)
123  planners_[i]->clear();
124 
125  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
126  numPathsShared_ = 0;
127  numStatesShared_ = 0;
128 
129  std::vector<base::StateSamplerPtr> samplers;
130  samplers.reserve(samplers_.size());
131  for (std::size_t i = 0; i < samplers_.size(); ++i)
132  if (samplers_[i].use_count() > 1)
133  samplers.push_back(samplers_[i]);
134  samplers_.swap(samplers);
135 }
136 
138 {
139  Planner::setup();
140  if (pdef_->hasOptimizationObjective())
141  opt_ = pdef_->getOptimizationObjective();
142  else
143  {
144  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
145  opt_.reset(new base::PathLengthOptimizationObjective(si_));
146  }
147 
148  bestCost_ = opt_->infiniteCost();
149 
150  if (planners_.empty())
151  {
152  OMPL_INFORM("%s: Number and type of instances not specified. Defaulting to %d instances of RRTstar.", getName().c_str(), numThreads_);
153  addPlannerInstances<RRTstar>(numThreads_);
154  }
155 
156  for (std::size_t i = 0; i < planners_.size() ; ++i)
157  if (!planners_[i]->isSetup())
158  planners_[i]->setup();
159 
160  // 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.
161  si_->setup();
162 }
163 
165 {
166  checkValidity();
167 
168  time::point start = time::now();
169  std::vector<boost::thread*> threads(planners_.size());
170  const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback();
171 
172  if (prevSolutionCallback)
173  OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
174 
175  pdef_->setIntermediateSolutionCallback(boost::bind(&CForest::newSolutionFound, this, _1, _2, _3));
176  bestCost_ = opt_->infiniteCost();
177 
178  // run each planner in its own thread, with the same ptc.
179  for (std::size_t i = 0 ; i < threads.size() ; ++i)
180  threads[i] = new boost::thread(boost::bind(&CForest::solve, this, planners_[i].get(), ptc));
181 
182  for (std::size_t i = 0 ; i < threads.size() ; ++i)
183  {
184  threads[i]->join();
185  delete threads[i];
186  }
187 
188  // restore callback
189  getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
190  OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
191  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
192 }
193 
195 {
196  return boost::lexical_cast<std::string>(bestCost_);
197 }
198 
200 {
201  return boost::lexical_cast<std::string>(numPathsShared_);
202 }
203 
205 {
206  return boost::lexical_cast<std::string>(numStatesShared_);
207 }
208 
209 void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost)
210 {
211  bool change = false;
212  std::vector<const base::State *> statesToShare;
213  newSolutionFoundMutex_.lock();
214  if (opt_->isCostBetterThan(cost, bestCost_))
215  {
216  ++numPathsShared_;
217  bestCost_ = cost;
218  change = true;
219 
220  // Filtering the states to add only those not already added.
221  statesToShare.reserve(states.size());
222  for (std::vector<const base::State *>::const_iterator st = states.begin(); st != states.end(); ++st)
223  {
224  if (statesShared_.find(*st) == statesShared_.end())
225  {
226  statesShared_.insert(*st);
227  statesToShare.push_back(*st);
228  ++numStatesShared_;
229  }
230  }
231  }
232  newSolutionFoundMutex_.unlock();
233 
234  if (!change || statesToShare.empty()) return;
235 
236  for (std::size_t i = 0; i < samplers_.size(); ++i)
237  {
238  base::CForestStateSampler *sampler = static_cast<base::CForestStateSampler*>(samplers_[i].get());
239  const base::CForestStateSpaceWrapper *space = static_cast<const base::CForestStateSpaceWrapper*>(sampler->getStateSpace());
240  const base::Planner *cfplanner = space->getPlanner();
241  if (cfplanner != planner)
242  sampler->setStatesToSample(statesToShare);
243  }
244 }
245 
247 {
248  OMPL_DEBUG("Starting %s", planner->getName().c_str());
249  time::point start = time::now();
250  if (planner->solve(ptc))
251  {
252  double duration = time::seconds(time::now() - start);
253  OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
254  }
255 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
std::string getBestCost() const
Get best cost among all the planners.
Definition: CForest.cpp:194
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: CForest.cpp:137
State space wrapper to use together with CForest. It adds some functionalities to the regular state s...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex, false is returned.
unsigned int numGoalVertices() const
Returns the number of goal vertices.
const PlannerDataVertex & getStartVertex(unsigned int i) const
Retrieve a reference to the ith start vertex object. If i is greater than the number of start vertice...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:204
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
unsigned int getIncomingEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of vertices with outgoing edges to the vertex with index v. The number of edges connec...
void setNumThreads(unsigned int numThreads=0)
Set default number of threads to use when no planner instances are specified by the user...
Definition: CForest.cpp:66
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:119
Extended state sampler to use with the CForest planning algorithm. It wraps the user-specified state ...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
void setStatesToSample(const std::vector< const State * > &states)
Fills the vector StatesToSample_ of states to be sampled in the next calls to sampleUniform(), sampleUniformNear() or sampleGaussian().
A boost shared pointer wrapper for ompl::base::Planner.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: CForest.cpp:84
const PlannerDataVertex & getGoalVertex(unsigned int i) const
Retrieve a reference to the ith goal vertex object. If i is greater than the number of goal vertices...
Base class for a planner.
Definition: Planner.h:232
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:199
boost::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: CForest.cpp:164
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
An optimization objective which corresponds to optimizing path length.
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2. If this edge does not exist, NO_EDGE is returned.
point now()
Get the current time point.
Definition: Time.h:56
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:118
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:198
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:72
unsigned int numStartVertices() const
Returns the number of start vertices.
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:74
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68