All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 <boost/thread.hpp>
41 #include <limits>
42 #include <cassert>
43 
44 ompl::geometric::pSBL::pSBL(const base::SpaceInformationPtr &si) : base::Planner(si, "pSBL"),
45  samplerArray_(si)
46 {
48  specs_.multithreaded = true;
49  maxDistance_ = 0.0;
50  setThreadCount(2);
51  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
52 
53  Planner::declareParam<double>("range", this, &pSBL::setRange, &pSBL::getRange);
54  Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount);
55 }
56 
57 ompl::geometric::pSBL::~pSBL(void)
58 {
59  freeMemory();
60 }
61 
63 {
64  Planner::setup();
65  tools::SelfConfig sc(si_, getName());
66  sc.configureProjectionEvaluator(projectionEvaluator_);
67  sc.configurePlannerRange(maxDistance_);
68 
69  tStart_.grid.setDimension(projectionEvaluator_->getDimension());
70  tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
71 }
72 
74 {
75  Planner::clear();
76 
77  samplerArray_.clear();
78 
79  freeMemory();
80 
81  tStart_.grid.clear();
82  tStart_.size = 0;
83  tStart_.pdf.clear();
84 
85  tGoal_.grid.clear();
86  tGoal_.size = 0;
87  tGoal_.pdf.clear();
88 
89  removeList_.motions.clear();
90  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
91 }
92 
93 void ompl::geometric::pSBL::freeGridMotions(Grid<MotionInfo> &grid)
94 {
95  for (Grid<MotionInfo>::iterator it = grid.begin(); it != grid.end() ; ++it)
96  {
97  for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
98  {
99  if (it->second->data[i]->state)
100  si_->freeState(it->second->data[i]->state);
101  delete it->second->data[i];
102  }
103  }
104 }
105 
106 void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
107 {
108  checkValidity();
109  RNG rng;
110 
111  std::vector<Motion*> solution;
112  base::State *xstate = si_->allocState();
113  bool startTree = rng.uniformBool();
114 
115  while (!sol->found && ptc() == false)
116  {
117  bool retry = true;
118  while (retry && !sol->found && ptc() == false)
119  {
120  removeList_.lock.lock();
121  if (!removeList_.motions.empty())
122  {
123  if (loopLock_.try_lock())
124  {
125  retry = false;
126  std::map<Motion*, bool> seen;
127  for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
128  if (seen.find(removeList_.motions[i].motion) == seen.end())
129  removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
130  removeList_.motions.clear();
131  loopLock_.unlock();
132  }
133  }
134  else
135  retry = false;
136  removeList_.lock.unlock();
137  }
138 
139  if (sol->found || ptc())
140  break;
141 
142  loopLockCounter_.lock();
143  if (loopCounter_ == 0)
144  loopLock_.lock();
145  loopCounter_++;
146  loopLockCounter_.unlock();
147 
148 
149  TreeData &tree = startTree ? tStart_ : tGoal_;
150  startTree = !startTree;
151  TreeData &otherTree = startTree ? tStart_ : tGoal_;
152 
153  Motion *existing = selectMotion(rng, tree);
154  if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
155  continue;
156 
157  /* create a motion */
158  Motion *motion = new Motion(si_);
159  si_->copyState(motion->state, xstate);
160  motion->parent = existing;
161  motion->root = existing->root;
162 
163  existing->lock.lock();
164  existing->children.push_back(motion);
165  existing->lock.unlock();
166 
167  addMotion(tree, motion);
168 
169  if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
170  {
171  sol->lock.lock();
172  if (!sol->found)
173  {
174  sol->found = true;
175  PathGeometric *path = new PathGeometric(si_);
176  for (unsigned int i = 0 ; i < solution.size() ; ++i)
177  path->append(solution[i]->state);
178  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
179  }
180  sol->lock.unlock();
181  }
182 
183 
184  loopLockCounter_.lock();
185  loopCounter_--;
186  if (loopCounter_ == 0)
187  loopLock_.unlock();
188  loopLockCounter_.unlock();
189  }
190 
191  si_->freeState(xstate);
192 }
193 
195 {
196  base::GoalState *goal = dynamic_cast<base::GoalState*>(pdef_->getGoal().get());
197 
198  if (!goal)
199  {
200  logError("Unknown type of goal (or goal undefined)");
202  }
203 
204  while (const base::State *st = pis_.nextStart())
205  {
206  Motion *motion = new Motion(si_);
207  si_->copyState(motion->state, st);
208  motion->valid = true;
209  motion->root = motion->state;
210  addMotion(tStart_, motion);
211  }
212 
213  if (tGoal_.size == 0)
214  {
215  if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
216  {
217  Motion *motion = new Motion(si_);
218  si_->copyState(motion->state, goal->getState());
219  motion->valid = true;
220  motion->root = motion->state;
221  addMotion(tGoal_, motion);
222  }
223  else
224  logError("Goal state is invalid!");
225  }
226 
227  if (tStart_.size == 0)
228  {
229  logError("Motion planning start tree could not be initialized!");
231  }
232  if (tGoal_.size == 0)
233  {
234  logError("Motion planning goal tree could not be initialized!");
236  }
237 
238  samplerArray_.resize(threadCount_);
239 
240  logInform("Starting with %d states", (int)(tStart_.size + tGoal_.size));
241 
242  SolutionInfo sol;
243  sol.found = false;
244  loopCounter_ = 0;
245 
246  std::vector<boost::thread*> th(threadCount_);
247  for (unsigned int i = 0 ; i < threadCount_ ; ++i)
248  th[i] = new boost::thread(boost::bind(&pSBL::threadSolve, this, i, ptc, &sol));
249  for (unsigned int i = 0 ; i < threadCount_ ; ++i)
250  {
251  th[i]->join();
252  delete th[i];
253  }
254 
255  logInform("Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
256  tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
257 
259 }
260 
261 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
262 {
264  projectionEvaluator_->computeCoordinates(motion->state, coord);
265 
266  otherTree.lock.lock();
267  Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
268 
269  if (cell && !cell->data.empty())
270  {
271  Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
272  otherTree.lock.unlock();
273 
274  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
275  {
276  Motion *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<base::State*, base::State*>(motion->state, connectOther->state);
292  else
293  connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
294 
295  /* extract the motions and put them in solution vector */
296 
297  std::vector<Motion*> mpath1;
298  while (motion != NULL)
299  {
300  mpath1.push_back(motion);
301  motion = motion->parent;
302  }
303 
304  std::vector<Motion*> mpath2;
305  while (connectOther != NULL)
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 != NULL)
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)] : NULL;
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 
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 (unsigned int i = 0 ; i < motion->children.size() ; ++i)
419  {
420  motion->children[i]->parent = NULL;
421  removeMotion(tree, motion->children[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 {
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> motions;
456  tStart_.grid.getContent(motions);
457 
458  for (unsigned int i = 0 ; i < motions.size() ; ++i)
459  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
460  if (motions[i][j]->parent == NULL)
461  data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
462  else
463  data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
464  base::PlannerDataVertex(motions[i][j]->state, 1));
465 
466  motions.clear();
467  tGoal_.grid.getContent(motions);
468  for (unsigned int i = 0 ; i < motions.size() ; ++i)
469  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
470  if (motions[i][j]->parent == NULL)
471  data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->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(motions[i][j]->state, 2),
475  base::PlannerDataVertex(motions[i][j]->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 }