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  return newSolutionFound(planner, states, cost);
187  });
188  bestCost_ = opt_->infiniteCost();
189 
190  // run each planner in its own thread, with the same ptc.
191  for (std::size_t i = 0; i < threads.size(); ++i)
192  {
193  base::Planner *planner = planners_[i].get();
194  threads[i] = new std::thread([this, planner, &ptc]
195  {
196  return solve(planner, ptc);
197  });
198  }
199 
200  for (auto &thread : threads)
201  {
202  thread->join();
203  delete thread;
204  }
205 
206  // restore callback
207  getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
208  OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
209  return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
210 }
211 
213 {
214  return std::to_string(bestCost_.value());
215 }
216 
218 {
219  return std::to_string(numPathsShared_);
220 }
221 
223 {
224  return std::to_string(numStatesShared_);
225 }
226 
227 void ompl::geometric::CForest::newSolutionFound(const base::Planner *planner,
228  const std::vector<const base::State *> &states, const base::Cost cost)
229 {
230  bool change = false;
231  std::vector<const base::State *> statesToShare;
232  newSolutionFoundMutex_.lock();
233  if (opt_->isCostBetterThan(cost, bestCost_))
234  {
235  ++numPathsShared_;
236  bestCost_ = cost;
237  change = true;
238 
239  // Filtering the states to add only those not already added.
240  statesToShare.reserve(states.size());
241  for (auto state : states)
242  {
243  if (statesShared_.find(state) == statesShared_.end())
244  {
245  statesShared_.insert(state);
246  statesToShare.push_back(state);
247  ++numStatesShared_;
248  }
249  }
250  }
251  newSolutionFoundMutex_.unlock();
252 
253  if (!change || statesToShare.empty())
254  return;
255 
256  for (auto &i : samplers_)
257  {
258  auto *sampler = static_cast<base::CForestStateSampler *>(i.get());
259  const auto *space =
260  static_cast<const base::CForestStateSpaceWrapper *>(sampler->getStateSpace());
261  const base::Planner *cfplanner = space->getPlanner();
262  if (cfplanner != planner)
263  sampler->setStatesToSample(statesToShare);
264  }
265 }
266 
268 {
269  OMPL_DEBUG("Starting %s", planner->getName().c_str());
270  time::point start = time::now();
271  if (planner->solve(ptc))
272  {
273  double duration = time::seconds(time::now() - start);
274  OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
275  }
276 }
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:222
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
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...
std::string getBestCost() const
Get best cost among all the planners.
Definition: CForest.cpp:212
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
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
#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...
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:217
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