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 #include <thread>
41 
42 ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
43 {
44  specs_.optimizingPaths = true;
45  specs_.multithreaded = true;
46 
47  numPathsShared_ = 0;
48  numStatesShared_ = 0;
49  focusSearch_ = true;
50 
51  numThreads_ = std::max(std::thread::hardware_concurrency(), 2u);
52  Planner::declareParam<bool>("focus_search", this, &CForest::setFocusSearch, &CForest::getFocusSearch, "0,1");
53  Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
54 
55  addPlannerProgressProperty("best cost REAL", [this]
56  {
57  return getBestCost();
58  });
59  addPlannerProgressProperty("shared paths INTEGER", [this]
60  {
61  return getNumPathsShared();
62  });
63  addPlannerProgressProperty("shared states INTEGER", [this]
64  {
65  return getNumStatesShared();
66  });
67 }
68 
69 ompl::geometric::CForest::~CForest() = default;
70 
71 void ompl::geometric::CForest::setNumThreads(unsigned int numThreads)
72 {
73  numThreads_ = numThreads ? numThreads : std::max(std::thread::hardware_concurrency(), 2u);
74 }
75 
76 void ompl::geometric::CForest::addPlannerInstanceInternal(const base::PlannerPtr &planner)
77 {
78  if (!planner->getSpecs().canReportIntermediateSolutions)
79  OMPL_WARN("%s cannot report intermediate solutions, not added as CForest planner.", planner->getName().c_str());
80  else
81  {
82  planner->setProblemDefinition(pdef_);
83  if (planner->params().hasParam("focus_search"))
84  planner->params()["focus_search"] = focusSearch_;
85  else
86  OMPL_WARN("%s does not appear to support search focusing.", planner->getName().c_str());
87 
88  planners_.push_back(planner);
89  }
90 }
91 
93 {
95 
96  for (std::size_t i = 0; i < planners_.size(); ++i)
97  {
98  base::PlannerData pd(si_);
99  planners_[i]->getPlannerData(pd);
100 
101  for (unsigned int j = 0; j < pd.numVertices(); ++j)
102  {
104 
105  v.setTag(i);
106  std::vector<unsigned int> edgeList;
107  unsigned int numEdges = pd.getIncomingEdges(j, edgeList);
108  for (unsigned int k = 0; k < numEdges; ++k)
109  {
110  base::Cost edgeWeight;
111  base::PlannerDataVertex &w = pd.getVertex(edgeList[k]);
112 
113  w.setTag(i);
114  pd.getEdgeWeight(j, k, &edgeWeight);
115  data.addEdge(v, w, pd.getEdge(j, k), edgeWeight);
116  }
117  }
118 
119  for (unsigned int j = 0; j < pd.numGoalVertices(); ++j)
120  data.markGoalState(pd.getGoalVertex(j).getState());
121 
122  for (unsigned int j = 0; j < pd.numStartVertices(); ++j)
123  data.markStartState(pd.getStartVertex(j).getState());
124  }
125 }
126 
128 {
129  Planner::clear();
130  for (auto &planner : planners_)
131  planner->clear();
132 
133  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
134  numPathsShared_ = 0;
135  numStatesShared_ = 0;
136 
137  std::vector<base::StateSamplerPtr> samplers;
138  samplers.reserve(samplers_.size());
139  for (auto &sampler : samplers_)
140  if (sampler.use_count() > 1)
141  samplers.push_back(sampler);
142  samplers_.swap(samplers);
143 }
144 
146 {
147  Planner::setup();
148  if (pdef_->hasOptimizationObjective())
149  opt_ = pdef_->getOptimizationObjective();
150  else
151  {
152  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
153  "planning time.",
154  getName().c_str());
155  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
156  }
157 
158  bestCost_ = opt_->infiniteCost();
159 
160  if (planners_.empty())
161  {
162  OMPL_INFORM("%s: Number and type of instances not specified. Defaulting to %d instances of RRTstar.",
163  getName().c_str(), numThreads_);
164  addPlannerInstances<RRTstar>(numThreads_);
165  }
166 
167  for (auto &planner : planners_)
168  if (!planner->isSetup())
169  planner->setup();
170 
171  // This call is needed to make sure the ParamSet is up to date after changes induced by the planner setup calls
172  // above, via the state space wrappers for CForest.
173  si_->setup();
174 }
175 
177 {
178  checkValidity();
179 
180  time::point start = time::now();
181  std::vector<std::thread *> threads(planners_.size());
182  const base::ReportIntermediateSolutionFn prevSolutionCallback =
183  getProblemDefinition()->getIntermediateSolutionCallback();
184 
185  if (prevSolutionCallback)
186  OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
187 
188  pdef_->setIntermediateSolutionCallback(
189  [this](const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost)
190  {
191  return newSolutionFound(planner, states, cost);
192  });
193  bestCost_ = opt_->infiniteCost();
194 
195  // run each planner in its own thread, with the same ptc.
196  for (std::size_t i = 0; i < threads.size(); ++i)
197  {
198  base::Planner *planner = planners_[i].get();
199  threads[i] = new std::thread([this, planner, &ptc]
200  {
201  return solve(planner, ptc);
202  });
203  }
204 
205  for (auto &thread : threads)
206  {
207  thread->join();
208  delete thread;
209  }
210 
211  // restore callback
212  getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
213  OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
214  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
215 }
216 
218 {
219  return std::to_string(bestCost_.value());
220 }
221 
223 {
224  return std::to_string(numPathsShared_);
225 }
226 
228 {
229  return std::to_string(numStatesShared_);
230 }
231 
232 void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner,
233  const std::vector<const base::State *> &states, const base::Cost cost)
234 {
235  bool change = false;
236  std::vector<const base::State *> statesToShare;
237  newSolutionFoundMutex_.lock();
238  if (opt_->isCostBetterThan(cost, bestCost_))
239  {
240  ++numPathsShared_;
241  bestCost_ = cost;
242  change = true;
243 
244  // Filtering the states to add only those not already added.
245  statesToShare.reserve(states.size());
246  for (auto state : states)
247  {
248  if (statesShared_.find(state) == statesShared_.end())
249  {
250  statesShared_.insert(state);
251  statesToShare.push_back(state);
252  ++numStatesShared_;
253  }
254  }
255  }
256  newSolutionFoundMutex_.unlock();
257 
258  if (!change || statesToShare.empty())
259  return;
260 
261  for (auto &i : samplers_)
262  {
263  base::CForestStateSampler *sampler = static_cast<base::CForestStateSampler *>(i.get());
264  const base::CForestStateSpaceWrapper *space =
265  static_cast<const base::CForestStateSpaceWrapper *>(sampler->getStateSpace());
266  const base::Planner *cfplanner = space->getPlanner();
267  if (cfplanner != planner)
268  sampler->setStatesToSample(statesToShare);
269  }
270 }
271 
273 {
274  OMPL_DEBUG("Starting %s", planner->getName().c_str());
275  time::point start = time::now();
276  if (planner->solve(ptc))
277  {
278  double duration = time::seconds(time::now() - start);
279  OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
280  }
281 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:227
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...
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: CForest.cpp:176
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:127
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 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...
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 numVertices() const
Retrieve the number of vertices in this structure.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:113
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: CForest.cpp:145
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.
unsigned int numStartVertices() const
Returns the number of start vertices.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:209
std::string getBestCost() const
Get best cost among all the planners.
Definition: CForest.cpp:217
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:71
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:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
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:119
A shared pointer wrapper for ompl::base::Planner.
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
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:87
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...
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...
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().
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
std::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...
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
point now()
Get the current time point.
Definition: Time.h:70
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:222
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.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: CForest.cpp:92
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:75
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68