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