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>
44 ompl::geometric::pSBL::pSBL(
const base::SpaceInformationPtr &si) : base::Planner(si,
"pSBL"),
57 ompl::geometric::pSBL::~pSBL(
void)
69 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
70 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
77 samplerArray_.clear();
89 removeList_.motions.clear();
90 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
97 for (
unsigned int i = 0 ; i < it->second->data.size() ; ++i)
99 if (it->second->data[i]->state)
100 si_->freeState(it->second->data[i]->state);
101 delete it->second->data[i];
106 void ompl::geometric::pSBL::threadSolve(
unsigned int tid,
const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
111 std::vector<Motion*> solution;
112 base::State *xstate = si_->allocState();
113 bool startTree = rng.uniformBool();
115 while (!sol->found && ptc() ==
false)
118 while (retry && !sol->found && ptc() ==
false)
120 removeList_.lock.lock();
121 if (!removeList_.motions.empty())
123 if (loopLock_.try_lock())
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();
136 removeList_.lock.unlock();
139 if (sol->found || ptc())
142 loopLockCounter_.lock();
143 if (loopCounter_ == 0)
146 loopLockCounter_.unlock();
149 TreeData &tree = startTree ? tStart_ : tGoal_;
150 startTree = !startTree;
151 TreeData &otherTree = startTree ? tStart_ : tGoal_;
153 Motion *existing = selectMotion(rng, tree);
154 if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
158 Motion *motion =
new Motion(si_);
159 si_->copyState(motion->state, xstate);
160 motion->parent = existing;
161 motion->root = existing->root;
163 existing->lock.lock();
164 existing->children.push_back(motion);
165 existing->lock.unlock();
167 addMotion(tree, motion);
169 if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
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);
184 loopLockCounter_.lock();
186 if (loopCounter_ == 0)
188 loopLockCounter_.unlock();
191 si_->freeState(xstate);
200 logError(
"Unknown type of goal (or goal undefined)");
207 si_->copyState(motion->state, st);
208 motion->valid =
true;
209 motion->root = motion->state;
210 addMotion(tStart_, motion);
213 if (tGoal_.size == 0)
215 if (si_->satisfiesBounds(goal->
getState()) && si_->isValid(goal->
getState()))
218 si_->copyState(motion->state, goal->
getState());
219 motion->valid =
true;
220 motion->root = motion->state;
221 addMotion(tGoal_, motion);
227 if (tStart_.size == 0)
229 logError(
"Motion planning start tree could not be initialized!");
232 if (tGoal_.size == 0)
234 logError(
"Motion planning goal tree could not be initialized!");
238 samplerArray_.resize(threadCount_);
240 logInform(
"Starting with %d states", (
int)(tStart_.size + tGoal_.size));
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)
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());
261 bool ompl::geometric::pSBL::checkSolution(
RNG &rng,
bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
264 projectionEvaluator_->computeCoordinates(motion->state, coord);
266 otherTree.lock.lock();
269 if (cell && !cell->data.
empty())
271 Motion *connectOther = cell->data[rng.
uniformInt(0, cell->data.
size() - 1)];
272 otherTree.lock.unlock();
274 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
276 Motion *connect =
new Motion(si_);
278 si_->copyState(connect->state, connectOther->state);
279 connect->parent = motion;
280 connect->root = motion->root;
283 motion->children.push_back(connect);
284 motion->lock.unlock();
286 addMotion(tree, connect);
288 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
291 connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->state, connectOther->state);
293 connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
297 std::vector<Motion*> mpath1;
298 while (motion != NULL)
300 mpath1.push_back(motion);
301 motion = motion->parent;
304 std::vector<Motion*> mpath2;
305 while (connectOther != NULL)
307 mpath2.push_back(connectOther);
308 connectOther = connectOther->parent;
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());
323 otherTree.lock.unlock();
328 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
330 std::vector<Motion*> mpath;
333 while (motion != NULL)
335 mpath.push_back(motion);
336 motion = motion->parent;
342 for (
int i = mpath.size() - 1 ; result && i >= 0 ; --i)
344 mpath[i]->lock.lock();
345 if (!mpath[i]->valid)
347 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
348 mpath[i]->valid =
true;
352 PendingRemoveMotion prm;
354 prm.motion = mpath[i];
355 removeList_.lock.lock();
356 removeList_.motions.push_back(prm);
357 removeList_.lock.unlock();
361 mpath[i]->lock.unlock();
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;
376 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen)
382 projectionEvaluator_->computeCoordinates(motion->state, coord);
383 Grid<MotionInfo>::Cell* cell = tree.grid.
getCell(coord);
386 for (
unsigned int i = 0 ; i < cell->data.size(); ++i)
387 if (cell->data[i] == motion)
389 cell->data.erase(cell->data.begin() + i);
393 if (cell->data.empty())
395 tree.pdf.remove(cell->data.elem_);
396 tree.grid.remove(cell);
397 tree.grid.destroyCell(cell);
401 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
409 for (
unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
410 if (motion->parent->children[i] == motion)
412 motion->parent->children.erase(motion->parent->children.begin() + i);
418 for (
unsigned int i = 0 ; i < motion->children.size() ; ++i)
420 motion->children[i]->parent = NULL;
421 removeMotion(tree, motion->children[i], seen);
425 si_->freeState(motion->state);
429 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
432 projectionEvaluator_->computeCoordinates(motion->state, coord);
434 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
437 cell->data.push_back(motion);
438 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
442 cell = tree.grid.createCell(coord);
443 cell->data.push_back(motion);
445 cell->data.elem_ = tree.pdf.add(cell, 1.0);
453 Planner::getPlannerData(data);
455 std::vector<MotionInfo> motions;
456 tStart_.grid.getContent(motions);
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)
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)
482 assert(nthreads > 0);
483 threadCount_ = nthreads;