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  maxDistance_ = 0.0;
48  setThreadCount(2);
49  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
50 
51  Planner::declareParam<double>("range", this, &pSBL::setRange, &pSBL::getRange, "0.:1.:10000.");
52  Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount, "1:64");
53 }
54 
55 ompl::geometric::pSBL::~pSBL()
56 {
57  freeMemory();
58 }
59 
61 {
62  Planner::setup();
66 
67  tStart_.grid.setDimension(projectionEvaluator_->getDimension());
68  tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
69 }
70 
72 {
73  Planner::clear();
74 
75  samplerArray_.clear();
76 
77  freeMemory();
78 
79  tStart_.grid.clear();
80  tStart_.size = 0;
81  tStart_.pdf.clear();
82 
83  tGoal_.grid.clear();
84  tGoal_.size = 0;
85  tGoal_.pdf.clear();
86 
87  removeList_.motions.clear();
88  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
89 }
90 
91 void ompl::geometric::pSBL::freeGridMotions(Grid<MotionInfo> &grid)
92 {
93  for (const auto &it : grid)
94  {
95  for (unsigned int i = 0; i < it.second->data.size(); ++i)
96  {
97  if (it.second->data[i]->state)
98  si_->freeState(it.second->data[i]->state);
99  delete it.second->data[i];
100  }
101  }
102 }
103 
104 void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc,
105  SolutionInfo *sol)
106 {
107  RNG rng;
108 
109  std::vector<Motion *> solution;
110  base::State *xstate = si_->allocState();
111  bool startTree = rng.uniformBool();
112 
113  while (!sol->found && ptc == false)
114  {
115  bool retry = true;
116  while (retry && !sol->found && ptc == false)
117  {
118  removeList_.lock.lock();
119  if (!removeList_.motions.empty())
120  {
121  if (loopLock_.try_lock())
122  {
123  retry = false;
124  std::map<Motion *, bool> seen;
125  for (auto &motion : removeList_.motions)
126  if (seen.find(motion.motion) == seen.end())
127  removeMotion(*motion.tree, motion.motion, seen);
128  removeList_.motions.clear();
129  loopLock_.unlock();
130  }
131  }
132  else
133  retry = false;
134  removeList_.lock.unlock();
135  }
136 
137  if (sol->found || ptc)
138  break;
139 
140  loopLockCounter_.lock();
141  if (loopCounter_ == 0)
142  loopLock_.lock();
143  loopCounter_++;
144  loopLockCounter_.unlock();
145 
146  TreeData &tree = startTree ? tStart_ : tGoal_;
147  startTree = !startTree;
148  TreeData &otherTree = startTree ? tStart_ : tGoal_;
149 
150  Motion *existing = selectMotion(rng, tree);
151  if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
152  continue;
153 
154  /* create a motion */
155  auto *motion = new Motion(si_);
156  si_->copyState(motion->state, xstate);
157  motion->parent = existing;
158  motion->root = existing->root;
159 
160  existing->lock.lock();
161  existing->children.push_back(motion);
162  existing->lock.unlock();
163 
164  addMotion(tree, motion);
165 
166  if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
167  {
168  sol->lock.lock();
169  if (!sol->found)
170  {
171  sol->found = true;
172  auto path(std::make_shared<PathGeometric>(si_));
173  for (auto &i : solution)
174  path->append(i->state);
175  pdef_->addSolutionPath(path, false, 0.0, getName());
176  }
177  sol->lock.unlock();
178  }
179 
180  loopLockCounter_.lock();
181  loopCounter_--;
182  if (loopCounter_ == 0)
183  loopLock_.unlock();
184  loopLockCounter_.unlock();
185  }
186 
187  si_->freeState(xstate);
188 }
189 
191 {
192  checkValidity();
193 
194  base::GoalState *goal = dynamic_cast<base::GoalState *>(pdef_->getGoal().get());
195 
196  if (!goal)
197  {
198  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
200  }
201 
202  while (const base::State *st = pis_.nextStart())
203  {
204  auto *motion = new Motion(si_);
205  si_->copyState(motion->state, st);
206  motion->valid = true;
207  motion->root = motion->state;
208  addMotion(tStart_, motion);
209  }
210 
211  if (tGoal_.size == 0)
212  {
213  if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
214  {
215  auto *motion = new Motion(si_);
216  si_->copyState(motion->state, goal->getState());
217  motion->valid = true;
218  motion->root = motion->state;
219  addMotion(tGoal_, motion);
220  }
221  else
222  OMPL_ERROR("%s: Goal state is invalid!", getName().c_str());
223  }
224 
225  if (tStart_.size == 0)
226  {
227  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
229  }
230  if (tGoal_.size == 0)
231  {
232  OMPL_ERROR("%s: Motion planning goal tree could not be initialized!", getName().c_str());
234  }
235 
236  samplerArray_.resize(threadCount_);
237 
238  OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
239  (int)(tStart_.size + tGoal_.size));
240 
241  SolutionInfo sol;
242  sol.found = false;
243  loopCounter_ = 0;
244 
245  std::vector<std::thread *> th(threadCount_);
246  for (unsigned int i = 0; i < threadCount_; ++i)
247  th[i] = new std::thread([this, i, &ptc, &sol]
248  {
249  threadSolve(i, ptc, &sol);
250  });
251  for (unsigned int i = 0; i < threadCount_; ++i)
252  {
253  th[i]->join();
254  delete th[i];
255  }
256 
257  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
259  tStart_.grid.size(), tGoal_.grid.size());
260 
262 }
263 
264 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
265  std::vector<Motion *> &solution)
266 {
268  projectionEvaluator_->computeCoordinates(motion->state, coord);
269 
270  otherTree.lock.lock();
271  Grid<MotionInfo>::Cell *cell = otherTree.grid.getCell(coord);
272 
273  if (cell && !cell->data.empty())
274  {
275  Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
276  otherTree.lock.unlock();
277 
278  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root,
279  start ? connectOther->root : motion->root))
280  {
281  auto *connect = new Motion(si_);
282 
283  si_->copyState(connect->state, connectOther->state);
284  connect->parent = motion;
285  connect->root = motion->root;
286 
287  motion->lock.lock();
288  motion->children.push_back(connect);
289  motion->lock.unlock();
290 
291  addMotion(tree, connect);
292 
293  if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
294  {
295  if (start)
296  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
297  else
298  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
299 
300  /* extract the motions and put them in solution vector */
301 
302  std::vector<Motion *> mpath1;
303  while (motion != nullptr)
304  {
305  mpath1.push_back(motion);
306  motion = motion->parent;
307  }
308 
309  std::vector<Motion *> mpath2;
310  while (connectOther != nullptr)
311  {
312  mpath2.push_back(connectOther);
313  connectOther = connectOther->parent;
314  }
315 
316  if (!start)
317  mpath1.swap(mpath2);
318 
319  for (int i = mpath1.size() - 1; i >= 0; --i)
320  solution.push_back(mpath1[i]);
321  solution.insert(solution.end(), mpath2.begin(), mpath2.end());
322 
323  return true;
324  }
325  }
326  }
327  else
328  otherTree.lock.unlock();
329 
330  return false;
331 }
332 
333 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
334 {
335  std::vector<Motion *> mpath;
336 
337  /* construct the solution path */
338  while (motion != nullptr)
339  {
340  mpath.push_back(motion);
341  motion = motion->parent;
342  }
343 
344  bool result = true;
345 
346  /* check the path */
347  for (int i = mpath.size() - 1; result && i >= 0; --i)
348  {
349  mpath[i]->lock.lock();
350  if (!mpath[i]->valid)
351  {
352  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
353  mpath[i]->valid = true;
354  else
355  {
356  // remember we need to remove this motion
358  prm.tree = &tree;
359  prm.motion = mpath[i];
360  removeList_.lock.lock();
361  removeList_.motions.push_back(prm);
362  removeList_.lock.unlock();
363  result = false;
364  }
365  }
366  mpath[i]->lock.unlock();
367  }
368 
369  return result;
370 }
371 
372 ompl::geometric::pSBL::Motion *ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree)
373 {
374  tree.lock.lock();
375  GridCell *cell = tree.pdf.sample(rng.uniform01());
376  Motion *result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : nullptr;
377  tree.lock.unlock();
378  return result;
379 }
380 
381 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen)
382 {
383  /* remove from grid */
384  seen[motion] = true;
385 
387  projectionEvaluator_->computeCoordinates(motion->state, coord);
388  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
389  if (cell)
390  {
391  for (unsigned int i = 0; i < cell->data.size(); ++i)
392  if (cell->data[i] == motion)
393  {
394  cell->data.erase(cell->data.begin() + i);
395  tree.size--;
396  break;
397  }
398  if (cell->data.empty())
399  {
400  tree.pdf.remove(cell->data.elem_);
401  tree.grid.remove(cell);
402  tree.grid.destroyCell(cell);
403  }
404  else
405  {
406  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
407  }
408  }
409 
410  /* remove self from parent list */
411 
412  if (motion->parent)
413  {
414  for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
415  if (motion->parent->children[i] == motion)
416  {
417  motion->parent->children.erase(motion->parent->children.begin() + i);
418  break;
419  }
420  }
421 
422  /* remove children */
423  for (auto &i : motion->children)
424  {
425  i->parent = nullptr;
426  removeMotion(tree, i, seen);
427  }
428 
429  if (motion->state)
430  si_->freeState(motion->state);
431  delete motion;
432 }
433 
434 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
435 {
437  projectionEvaluator_->computeCoordinates(motion->state, coord);
438  tree.lock.lock();
439  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
440  if (cell)
441  {
442  cell->data.push_back(motion);
443  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
444  }
445  else
446  {
447  cell = tree.grid.createCell(coord);
448  cell->data.push_back(motion);
449  tree.grid.add(cell);
450  cell->data.elem_ = tree.pdf.add(cell, 1.0);
451  }
452  tree.size++;
453  tree.lock.unlock();
454 }
455 
457 {
458  Planner::getPlannerData(data);
459 
460  std::vector<MotionInfo> motionInfo;
461  tStart_.grid.getContent(motionInfo);
462 
463  for (auto &m : motionInfo)
464  for (auto &motion : m.motions_)
465  if (motion->parent == nullptr)
466  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
467  else
468  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1),
469  base::PlannerDataVertex(motion->state, 1));
470 
471  motionInfo.clear();
472  tGoal_.grid.getContent(motionInfo);
473  for (auto &m : motionInfo)
474  for (auto &motion : m.motions_)
475  if (motion->parent == nullptr)
476  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
477  else
478  // The edges in the goal tree are reversed so that they are in the same direction as start tree
479  data.addEdge(base::PlannerDataVertex(motion->state, 2),
480  base::PlannerDataVertex(motion->parent->state, 2));
481 
482  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
483 }
484 
485 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads)
486 {
487  assert(nthreads > 0);
488  threadCount_ = nthreads;
489 }
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: pSBL.cpp:71
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:265
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:173
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:485
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:206
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:185
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:332
void freeMemory()
Free the memory allocated by the planner.
Definition: SBL.h:226
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:259
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:244
base::State * state
The state this motion leads to.
Definition: SBL.h:164
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:209
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:456
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:270
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: pSBL.cpp:60
const State * getState() const
Get the goal state.
Definition: GoalState.cpp:79
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:222
unsigned int size
The number of motions (in total) from the tree.
Definition: SBL.h:219
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:276
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:216
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:421
unsigned int getThreadCount() const
Get the thread count.
Definition: pSBL.h:133
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
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:190
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:415
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:274
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:268
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
TreeData tStart_
The start tree.
Definition: SBL.h:262