pSBL.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
38 #define OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/ProjectionEvaluator.h"
42 #include "ompl/base/StateSamplerArray.h"
43 #include "ompl/datastructures/Grid.h"
44 #include "ompl/datastructures/PDF.h"
45 #include <thread>
46 #include <mutex>
47 #include <vector>
48 
49 namespace ompl
50 {
51  namespace geometric
52  {
86  class pSBL : public base::Planner
87  {
88  public:
89  pSBL(const base::SpaceInformationPtr &si);
90 
91  ~pSBL() override;
92 
95  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
96  {
97  projectionEvaluator_ = projectionEvaluator;
98  }
99 
102  void setProjectionEvaluator(const std::string &name)
103  {
104  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
105  }
106 
108  const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
109  {
110  return projectionEvaluator_;
111  }
112 
118  void setRange(double distance)
119  {
120  maxDistance_ = distance;
121  }
122 
124  double getRange() const
125  {
126  return maxDistance_;
127  }
128 
130  void setThreadCount(unsigned int nthreads);
131 
133  unsigned int getThreadCount() const
134  {
135  return threadCount_;
136  }
137 
138  void setup() override;
139 
140  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
141 
142  void clear() override;
143 
144  void getPlannerData(base::PlannerData &data) const override;
145 
146  protected:
147  class Motion;
148  struct MotionInfo;
149 
151  using GridCell = Grid<MotionInfo>::Cell;
152 
154  using CellPDF = PDF<GridCell *>;
155 
156  class Motion
157  {
158  public:
159  Motion() = default;
160 
161  Motion(const base::SpaceInformationPtr &si)
162  : state(si->allocState())
163  {
164  }
165 
166  ~Motion() = default;
167 
168  const base::State *root{nullptr};
169  base::State *state{nullptr};
170  Motion *parent{nullptr};
171  bool valid{false};
172  std::vector<Motion *> children;
173  std::mutex lock;
174  };
175 
177  struct MotionInfo
178  {
179  Motion *operator[](unsigned int i)
180  {
181  return motions_[i];
182  }
183  std::vector<Motion *>::iterator begin()
184  {
185  return motions_.begin();
186  }
187  void erase(std::vector<Motion *>::iterator iter)
188  {
189  motions_.erase(iter);
190  }
191  void push_back(Motion *m)
192  {
193  motions_.push_back(m);
194  }
195  unsigned int size() const
196  {
197  return motions_.size();
198  }
199  bool empty() const
200  {
201  return motions_.empty();
202  }
203  std::vector<Motion *> motions_;
204  CellPDF::Element *elem_;
205  };
206 
207  struct TreeData
208  {
209  TreeData() = default;
210 
211  Grid<MotionInfo> grid{0};
212  unsigned int size{0};
213  CellPDF pdf;
214  std::mutex lock;
215  };
216 
217  struct SolutionInfo
218  {
219  std::vector<Motion *> solution;
220  bool found;
221  std::mutex lock;
222  };
223 
224  struct PendingRemoveMotion
225  {
226  TreeData *tree;
227  Motion *motion;
228  };
229 
230  struct MotionsToBeRemoved
231  {
232  std::vector<PendingRemoveMotion> motions;
233  std::mutex lock;
234  };
235 
236  void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
237 
238  void freeMemory()
239  {
240  freeGridMotions(tStart_.grid);
241  freeGridMotions(tGoal_.grid);
242  }
243 
244  void freeGridMotions(Grid<MotionInfo> &grid);
245 
246  void addMotion(TreeData &tree, Motion *motion);
247  Motion *selectMotion(RNG &rng, TreeData &tree);
248  void removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen);
249  bool isPathValid(TreeData &tree, Motion *motion);
250  bool checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
251  std::vector<Motion *> &solution);
252 
254  base::ProjectionEvaluatorPtr projectionEvaluator_;
255 
256  TreeData tStart_;
257  TreeData tGoal_;
258 
259  MotionsToBeRemoved removeList_;
260  std::mutex loopLock_;
261  std::mutex loopLockCounter_;
262  unsigned int loopCounter_;
263 
264  double maxDistance_{0.};
265 
266  unsigned int threadCount_;
267 
269  std::pair<base::State *, base::State *> connectionPoint_{nullptr, nullptr};
270  };
271  }
272 }
273 
274 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: pSBL.cpp:69
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: pSBL.cpp:188
double getRange() const
Get the range the planner is using.
Definition: pSBL.h:220
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: pSBL.h:204
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: pSBL.cpp:451
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition: pSBL.h:191
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: pSBL.h:214
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PDF< GridCell * > CellPDF
A PDF of grid cells.
Definition: pSBL.h:250
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: pSBL.cpp:58
unsigned int getThreadCount() const
Get the thread count.
Definition: pSBL.h:229
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: pSBL.h:365
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
Definition: pSBL.cpp:480
Class to ease the creation of a set of samplers. This is especially useful for multi-threaded planner...
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: pSBL.h:247
Representation of a simple grid.
Definition: Grid.h:83
Main namespace. Contains everything in this library.