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] { threadSolve(i, ptc, &sol); });
246  for (unsigned int i = 0; i < threadCount_; ++i)
247  {
248  th[i]->join();
249  delete th[i];
250  }
251 
252  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
254  tStart_.grid.size(), tGoal_.grid.size());
255 
257 }
258 
259 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
260  std::vector<Motion *> &solution)
261 {
262  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
263  projectionEvaluator_->computeCoordinates(motion->state, coord);
264 
265  otherTree.lock.lock();
266  Grid<MotionInfo>::Cell *cell = otherTree.grid.getCell(coord);
267 
268  if (cell && !cell->data.empty())
269  {
270  Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
271  otherTree.lock.unlock();
272 
273  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root,
274  start ? connectOther->root : motion->root))
275  {
276  auto *connect = new Motion(si_);
277 
278  si_->copyState(connect->state, connectOther->state);
279  connect->parent = motion;
280  connect->root = motion->root;
281 
282  motion->lock.lock();
283  motion->children.push_back(connect);
284  motion->lock.unlock();
285 
286  addMotion(tree, connect);
287 
288  if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
289  {
290  if (start)
291  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
292  else
293  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
294 
295  /* extract the motions and put them in solution vector */
296 
297  std::vector<Motion *> mpath1;
298  while (motion != nullptr)
299  {
300  mpath1.push_back(motion);
301  motion = motion->parent;
302  }
303 
304  std::vector<Motion *> mpath2;
305  while (connectOther != nullptr)
306  {
307  mpath2.push_back(connectOther);
308  connectOther = connectOther->parent;
309  }
310 
311  if (!start)
312  mpath1.swap(mpath2);
313 
314  for (int i = mpath1.size() - 1; i >= 0; --i)
315  solution.push_back(mpath1[i]);
316  solution.insert(solution.end(), mpath2.begin(), mpath2.end());
317 
318  return true;
319  }
320  }
321  }
322  else
323  otherTree.lock.unlock();
324 
325  return false;
326 }
327 
328 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
329 {
330  std::vector<Motion *> mpath;
331 
332  /* construct the solution path */
333  while (motion != nullptr)
334  {
335  mpath.push_back(motion);
336  motion = motion->parent;
337  }
338 
339  bool result = true;
340 
341  /* check the path */
342  for (int i = mpath.size() - 1; result && i >= 0; --i)
343  {
344  mpath[i]->lock.lock();
345  if (!mpath[i]->valid)
346  {
347  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
348  mpath[i]->valid = true;
349  else
350  {
351  // remember we need to remove this motion
352  PendingRemoveMotion prm;
353  prm.tree = &tree;
354  prm.motion = mpath[i];
355  removeList_.lock.lock();
356  removeList_.motions.push_back(prm);
357  removeList_.lock.unlock();
358  result = false;
359  }
360  }
361  mpath[i]->lock.unlock();
362  }
363 
364  return result;
365 }
366 
367 ompl::geometric::pSBL::Motion *ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree)
368 {
369  tree.lock.lock();
370  GridCell *cell = tree.pdf.sample(rng.uniform01());
371  Motion *result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : nullptr;
372  tree.lock.unlock();
373  return result;
374 }
375 
376 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen)
377 {
378  /* remove from grid */
379  seen[motion] = true;
380 
381  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
382  projectionEvaluator_->computeCoordinates(motion->state, coord);
383  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
384  if (cell)
385  {
386  for (unsigned int i = 0; i < cell->data.size(); ++i)
387  if (cell->data[i] == motion)
388  {
389  cell->data.erase(cell->data.begin() + i);
390  tree.size--;
391  break;
392  }
393  if (cell->data.empty())
394  {
395  tree.pdf.remove(cell->data.elem_);
396  tree.grid.remove(cell);
397  tree.grid.destroyCell(cell);
398  }
399  else
400  {
401  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
402  }
403  }
404 
405  /* remove self from parent list */
406 
407  if (motion->parent)
408  {
409  for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
410  if (motion->parent->children[i] == motion)
411  {
412  motion->parent->children.erase(motion->parent->children.begin() + i);
413  break;
414  }
415  }
416 
417  /* remove children */
418  for (auto &i : motion->children)
419  {
420  i->parent = nullptr;
421  removeMotion(tree, i, seen);
422  }
423 
424  if (motion->state)
425  si_->freeState(motion->state);
426  delete motion;
427 }
428 
429 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
430 {
431  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
432  projectionEvaluator_->computeCoordinates(motion->state, coord);
433  tree.lock.lock();
434  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
435  if (cell)
436  {
437  cell->data.push_back(motion);
438  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
439  }
440  else
441  {
442  cell = tree.grid.createCell(coord);
443  cell->data.push_back(motion);
444  tree.grid.add(cell);
445  cell->data.elem_ = tree.pdf.add(cell, 1.0);
446  }
447  tree.size++;
448  tree.lock.unlock();
449 }
450 
452 {
453  Planner::getPlannerData(data);
454 
455  std::vector<MotionInfo> motionInfo;
456  tStart_.grid.getContent(motionInfo);
457 
458  for (auto &m : motionInfo)
459  for (auto &motion : m.motions_)
460  if (motion->parent == nullptr)
461  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
462  else
463  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1),
464  base::PlannerDataVertex(motion->state, 1));
465 
466  motionInfo.clear();
467  tGoal_.grid.getContent(motionInfo);
468  for (auto &m : motionInfo)
469  for (auto &motion : m.motions_)
470  if (motion->parent == nullptr)
471  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
472  else
473  // The edges in the goal tree are reversed so that they are in the same direction as start tree
474  data.addEdge(base::PlannerDataVertex(motion->state, 2),
475  base::PlannerDataVertex(motion->parent->state, 2));
476 
477  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
478 }
479 
480 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads)
481 {
482  assert(nthreads > 0);
483  threadCount_ = nthreads;
484 }
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
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Grid< MotionInfo >::Cell GridCell
A grid cell.
Definition: SBL.h:236
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:256
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
Definition: SBL.h:308
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
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:366
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
Definition of an abstract state.
Definition: State.h:113
TreeData tGoal_
The goal tree.
Definition: SBL.h:357
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
@ GOAL_STATE
This bit is set if casting to goal state (ompl::base::GoalState) is possible.
Definition: GoalTypes.h:155
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:477
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
Definition: SBL.cpp:330
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
Definition: SBL.h:314
double getRange() const
Get the range the planner is using.
Definition: pSBL.h:220
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: SBL.h:351
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
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 freeMemory()
Free the memory allocated by the planner.
Definition: SBL.h:318
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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:106
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: pSBL.h:214
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:228
void clear()
Clears the PDF.
Definition: PDF.h:306
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
@ INVALID_GOAL
Invalid goal state.
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...
TreeData tStart_
The start tree.
Definition: SBL.h:354
unsigned int size
The number of motions (in total) from the tree.
Definition: SBL.h:311
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
Definition: Grid.h:119
A class to store the exit status of Planner::solve()
double maxDistance_
The maximum length of a motion to be added in the tree.
Definition: SBL.h:360
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: pSBL.cpp:58
Definition of a cell in this grid.
Definition: Grid.h:122
unsigned int getThreadCount() const
Get the thread count.
Definition: pSBL.h:229
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
Definition: pSBL.cpp:480
@ EXACT_SOLUTION
The planner found an exact solution.
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:480
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
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 a goal state.
Definition: GoalState.h:112
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
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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
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 addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
_T data
The data we store in the cell.
Definition: Grid.h:125
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:274
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
Representation of a simple grid.
Definition: Grid.h:83
@ INVALID_START
Invalid start state or no start state specified.
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:268
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:253