pSBL.cpp
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 #include "ompl/geometric/planners/sbl/pSBL.h"
38 #include "ompl/base/goals/GoalState.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 #include <cassert>
42 
43 ompl::geometric::pSBL::pSBL(const base::SpaceInformationPtr &si) : base::Planner(si, "pSBL"), samplerArray_(si)
44 {
46  specs_.multithreaded = true;
47  setThreadCount(2);
48 
49  Planner::declareParam<double>("range", this, &pSBL::setRange, &pSBL::getRange, "0.:1.:10000.");
50  Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount, "1:64");
51 }
52 
53 ompl::geometric::pSBL::~pSBL()
54 {
55  freeMemory();
56 }
57 
59 {
60  Planner::setup();
64 
65  tStart_.grid.setDimension(projectionEvaluator_->getDimension());
66  tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
67 }
68 
70 {
71  Planner::clear();
72 
73  samplerArray_.clear();
74 
75  freeMemory();
76 
77  tStart_.grid.clear();
78  tStart_.size = 0;
79  tStart_.pdf.clear();
80 
81  tGoal_.grid.clear();
82  tGoal_.size = 0;
83  tGoal_.pdf.clear();
84 
85  removeList_.motions.clear();
86  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
87 }
88 
89 void ompl::geometric::pSBL::freeGridMotions(Grid<MotionInfo> &grid)
90 {
91  for (const auto &it : grid)
92  {
93  for (unsigned int i = 0; i < it.second->data.size(); ++i)
94  {
95  if (it.second->data[i]->state)
96  si_->freeState(it.second->data[i]->state);
97  delete it.second->data[i];
98  }
99  }
100 }
101 
102 void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc,
103  SolutionInfo *sol)
104 {
105  RNG rng;
106 
107  std::vector<Motion *> solution;
108  base::State *xstate = si_->allocState();
109  bool startTree = rng.uniformBool();
110 
111  while (!sol->found && ptc == false)
112  {
113  bool retry = true;
114  while (retry && !sol->found && ptc == false)
115  {
116  removeList_.lock.lock();
117  if (!removeList_.motions.empty())
118  {
119  if (loopLock_.try_lock())
120  {
121  retry = false;
122  std::map<Motion *, bool> seen;
123  for (auto &motion : removeList_.motions)
124  if (seen.find(motion.motion) == seen.end())
125  removeMotion(*motion.tree, motion.motion, seen);
126  removeList_.motions.clear();
127  loopLock_.unlock();
128  }
129  }
130  else
131  retry = false;
132  removeList_.lock.unlock();
133  }
134 
135  if (sol->found || ptc)
136  break;
137 
138  loopLockCounter_.lock();
139  if (loopCounter_ == 0)
140  loopLock_.lock();
141  loopCounter_++;
142  loopLockCounter_.unlock();
143 
144  TreeData &tree = startTree ? tStart_ : tGoal_;
145  startTree = !startTree;
146  TreeData &otherTree = startTree ? tStart_ : tGoal_;
147 
148  Motion *existing = selectMotion(rng, tree);
149  if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
150  continue;
151 
152  /* create a motion */
153  auto *motion = new Motion(si_);
154  si_->copyState(motion->state, xstate);
155  motion->parent = existing;
156  motion->root = existing->root;
157 
158  existing->lock.lock();
159  existing->children.push_back(motion);
160  existing->lock.unlock();
161 
162  addMotion(tree, motion);
163 
164  if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
165  {
166  sol->lock.lock();
167  if (!sol->found)
168  {
169  sol->found = true;
170  auto path(std::make_shared<PathGeometric>(si_));
171  for (auto &i : solution)
172  path->append(i->state);
173  pdef_->addSolutionPath(path, false, 0.0, getName());
174  }
175  sol->lock.unlock();
176  }
177 
178  loopLockCounter_.lock();
179  loopCounter_--;
180  if (loopCounter_ == 0)
181  loopLock_.unlock();
182  loopLockCounter_.unlock();
183  }
184 
185  si_->freeState(xstate);
186 }
187 
189 {
190  checkValidity();
191 
192  auto *goal = dynamic_cast<base::GoalState *>(pdef_->getGoal().get());
193 
194  if (!goal)
195  {
196  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
198  }
199 
200  while (const base::State *st = pis_.nextStart())
201  {
202  auto *motion = new Motion(si_);
203  si_->copyState(motion->state, st);
204  motion->valid = true;
205  motion->root = motion->state;
206  addMotion(tStart_, motion);
207  }
208 
209  if (tGoal_.size == 0)
210  {
211  if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
212  {
213  auto *motion = new Motion(si_);
214  si_->copyState(motion->state, goal->getState());
215  motion->valid = true;
216  motion->root = motion->state;
217  addMotion(tGoal_, motion);
218  }
219  else
220  OMPL_ERROR("%s: Goal state is invalid!", getName().c_str());
221  }
222 
223  if (tStart_.size == 0)
224  {
225  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
227  }
228  if (tGoal_.size == 0)
229  {
230  OMPL_ERROR("%s: Motion planning goal tree could not be initialized!", getName().c_str());
232  }
233 
234  samplerArray_.resize(threadCount_);
235 
236  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
237  (int)(tStart_.size + tGoal_.size));
238 
239  SolutionInfo sol;
240  sol.found = false;
241  loopCounter_ = 0;
242 
243  std::vector<std::thread *> th(threadCount_);
244  for (unsigned int i = 0; i < threadCount_; ++i)
245  th[i] = new std::thread([this, i, &ptc, &sol]
246  {
247  threadSolve(i, ptc, &sol);
248  });
249  for (unsigned int i = 0; i < threadCount_; ++i)
250  {
251  th[i]->join();
252  delete th[i];
253  }
254 
255  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
257  tStart_.grid.size(), tGoal_.grid.size());
258 
260 }
261 
262 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
263  std::vector<Motion *> &solution)
264 {
266  projectionEvaluator_->computeCoordinates(motion->state, coord);
267 
268  otherTree.lock.lock();
269  Grid<MotionInfo>::Cell *cell = otherTree.grid.getCell(coord);
270 
271  if (cell && !cell->data.empty())
272  {
273  Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
274  otherTree.lock.unlock();
275 
276  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root,
277  start ? connectOther->root : motion->root))
278  {
279  auto *connect = new Motion(si_);
280 
281  si_->copyState(connect->state, connectOther->state);
282  connect->parent = motion;
283  connect->root = motion->root;
284 
285  motion->lock.lock();
286  motion->children.push_back(connect);
287  motion->lock.unlock();
288 
289  addMotion(tree, connect);
290 
291  if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
292  {
293  if (start)
294  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
295  else
296  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
297 
298  /* extract the motions and put them in solution vector */
299 
300  std::vector<Motion *> mpath1;
301  while (motion != nullptr)
302  {
303  mpath1.push_back(motion);
304  motion = motion->parent;
305  }
306 
307  std::vector<Motion *> mpath2;
308  while (connectOther != nullptr)
309  {
310  mpath2.push_back(connectOther);
311  connectOther = connectOther->parent;
312  }
313 
314  if (!start)
315  mpath1.swap(mpath2);
316 
317  for (int i = mpath1.size() - 1; i >= 0; --i)
318  solution.push_back(mpath1[i]);
319  solution.insert(solution.end(), mpath2.begin(), mpath2.end());
320 
321  return true;
322  }
323  }
324  }
325  else
326  otherTree.lock.unlock();
327 
328  return false;
329 }
330 
331 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
332 {
333  std::vector<Motion *> mpath;
334 
335  /* construct the solution path */
336  while (motion != nullptr)
337  {
338  mpath.push_back(motion);
339  motion = motion->parent;
340  }
341 
342  bool result = true;
343 
344  /* check the path */
345  for (int i = mpath.size() - 1; result && i >= 0; --i)
346  {
347  mpath[i]->lock.lock();
348  if (!mpath[i]->valid)
349  {
350  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
351  mpath[i]->valid = true;
352  else
353  {
354  // remember we need to remove this motion
356  prm.tree = &tree;
357  prm.motion = mpath[i];
358  removeList_.lock.lock();
359  removeList_.motions.push_back(prm);
360  removeList_.lock.unlock();
361  result = false;
362  }
363  }
364  mpath[i]->lock.unlock();
365  }
366 
367  return result;
368 }
369 
370 ompl::geometric::pSBL::Motion *ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree)
371 {
372  tree.lock.lock();
373  GridCell *cell = tree.pdf.sample(rng.uniform01());
374  Motion *result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : nullptr;
375  tree.lock.unlock();
376  return result;
377 }
378 
379 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen)
380 {
381  /* remove from grid */
382  seen[motion] = true;
383 
385  projectionEvaluator_->computeCoordinates(motion->state, coord);
386  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
387  if (cell)
388  {
389  for (unsigned int i = 0; i < cell->data.size(); ++i)
390  if (cell->data[i] == motion)
391  {
392  cell->data.erase(cell->data.begin() + i);
393  tree.size--;
394  break;
395  }
396  if (cell->data.empty())
397  {
398  tree.pdf.remove(cell->data.elem_);
399  tree.grid.remove(cell);
400  tree.grid.destroyCell(cell);
401  }
402  else
403  {
404  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
405  }
406  }
407 
408  /* remove self from parent list */
409 
410  if (motion->parent)
411  {
412  for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
413  if (motion->parent->children[i] == motion)
414  {
415  motion->parent->children.erase(motion->parent->children.begin() + i);
416  break;
417  }
418  }
419 
420  /* remove children */
421  for (auto &i : motion->children)
422  {
423  i->parent = nullptr;
424  removeMotion(tree, i, seen);
425  }
426 
427  if (motion->state)
428  si_->freeState(motion->state);
429  delete motion;
430 }
431 
432 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
433 {
435  projectionEvaluator_->computeCoordinates(motion->state, coord);
436  tree.lock.lock();
437  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
438  if (cell)
439  {
440  cell->data.push_back(motion);
441  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
442  }
443  else
444  {
445  cell = tree.grid.createCell(coord);
446  cell->data.push_back(motion);
447  tree.grid.add(cell);
448  cell->data.elem_ = tree.pdf.add(cell, 1.0);
449  }
450  tree.size++;
451  tree.lock.unlock();
452 }
453 
455 {
456  Planner::getPlannerData(data);
457 
458  std::vector<MotionInfo> motionInfo;
459  tStart_.grid.getContent(motionInfo);
460 
461  for (auto &m : motionInfo)
462  for (auto &motion : m.motions_)
463  if (motion->parent == nullptr)
464  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
465  else
466  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1),
467  base::PlannerDataVertex(motion->state, 1));
468 
469  motionInfo.clear();
470  tGoal_.grid.getContent(motionInfo);
471  for (auto &m : motionInfo)
472  for (auto &motion : m.motions_)
473  if (motion->parent == nullptr)
474  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
475  else
476  // The edges in the goal tree are reversed so that they are in the same direction as start tree
477  data.addEdge(base::PlannerDataVertex(motion->state, 2),
478  base::PlannerDataVertex(motion->parent->state, 2));
479 
480  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
481 }
482 
483 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads)
484 {
485  assert(nthreads > 0);
486  threadCount_ = nthreads;
487 }
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: pSBL.cpp:69
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
TreeData tGoal_
The goal tree.
Definition: SBL.h:261
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:171
Representation of a simple grid.
Definition: Grid.h:50
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
Definition: pSBL.cpp:483
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:197
Definition of a goal state.
Definition: GoalState.h:48
void clear()
Clears the PDF.
Definition: PDF.h:242
bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector< Motion *> &solution)
Check if a solution can be obtained by connecting two trees using a specified motion.
Definition: SBL.cpp:183
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
Definition: SBL.cpp:330
void freeMemory()
Free the memory allocated by the planner.
Definition: SBL.h:222
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
_T data
The data we store in the cell.
Definition: Grid.h:60
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: SBL.h:255
bool uniformBool()
Generate a random boolean.
Definition: RandomNumbers.h:88
bool isPathValid(TreeData &tree, Motion *motion)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
Definition: SBL.cpp:242
base::State * state
The state this motion leads to.
Definition: SBL.h:162
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:200
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:54
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:454
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:268
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: pSBL.cpp:58
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
void remove(Element *elem)
Removes the data in the given Element from the PDF. After calling this function, the Element object s...
Definition: PDF.h:178
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
Definition: SBL.h:218
unsigned int size
The number of motions (in total) from the tree.
Definition: SBL.h:215
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
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...
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:274
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:49
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
Definition: SBL.h:212
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
unsigned int getThreadCount() const
Get the thread count.
Definition: pSBL.h:133
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
Definition of a cell in this grid.
Definition: Grid.h:57
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:231
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
Definition: PDF.h:97
This bit is set if casting to goal state (ompl::base::GoalState) is possible.
Definition: GoalTypes.h:59
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
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
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: pSBL.h:118
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:270
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
double getRange() const
Get the range the planner is using.
Definition: pSBL.h:124
double maxDistance_
The maximum length of a motion to be added in the tree.
Definition: SBL.h:264
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
TreeData tStart_
The start tree.
Definition: SBL.h:258