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 }
unsigned int numGoalVertices() const
Returns the number of goal vertices.
Base class for a planner.
Definition: Planner.h:287
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...
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...
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:171
point now()
Get the current time point.
Definition: Time.h:134
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:128
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
std::string getNumPathsShared() const
Get number of paths shared by the algorithm.
Definition: CForest.cpp:218
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:87
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
unsigned int numStartVertices() const
Returns the number of start 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...
A class to store the exit status of Planner::solve()
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
std::string getNumStatesShared() const
Get number of states actually shared by the algorithm.
Definition: CForest.cpp:223
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: CForest.cpp:123
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:176
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
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex,...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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 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,...
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
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...
std::string getBestCost() const
Get best cost among all the planners.
Definition: CForest.cpp:213
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...
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
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:140
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....