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
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,...
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....
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: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
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,...
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