00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/planners/sbl/pSBL.h"
00038 #include "ompl/base/goals/GoalState.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <boost/thread.hpp>
00041 #include <limits>
00042 #include <cassert>
00043
00044 ompl::geometric::pSBL::pSBL(const base::SpaceInformationPtr &si) : base::Planner(si, "pSBL"),
00045 samplerArray_(si)
00046 {
00047 specs_.recognizedGoal = base::GOAL_STATE;
00048 specs_.multithreaded = true;
00049 maxDistance_ = 0.0;
00050 setThreadCount(2);
00051 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
00052
00053 Planner::declareParam<double>("range", this, &pSBL::setRange, &pSBL::getRange);
00054 Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount);
00055 }
00056
00057 ompl::geometric::pSBL::~pSBL(void)
00058 {
00059 freeMemory();
00060 }
00061
00062 void ompl::geometric::pSBL::setup(void)
00063 {
00064 Planner::setup();
00065 tools::SelfConfig sc(si_, getName());
00066 sc.configureProjectionEvaluator(projectionEvaluator_);
00067 sc.configurePlannerRange(maxDistance_);
00068
00069 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
00070 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
00071 }
00072
00073 void ompl::geometric::pSBL::clear(void)
00074 {
00075 Planner::clear();
00076
00077 samplerArray_.clear();
00078
00079 freeMemory();
00080
00081 tStart_.grid.clear();
00082 tStart_.size = 0;
00083 tStart_.pdf.clear();
00084
00085 tGoal_.grid.clear();
00086 tGoal_.size = 0;
00087 tGoal_.pdf.clear();
00088
00089 removeList_.motions.clear();
00090 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
00091 }
00092
00093 void ompl::geometric::pSBL::freeGridMotions(Grid<MotionInfo> &grid)
00094 {
00095 for (Grid<MotionInfo>::iterator it = grid.begin(); it != grid.end() ; ++it)
00096 {
00097 for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00098 {
00099 if (it->second->data[i]->state)
00100 si_->freeState(it->second->data[i]->state);
00101 delete it->second->data[i];
00102 }
00103 }
00104 }
00105
00106 void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
00107 {
00108 checkValidity();
00109 RNG rng;
00110
00111 std::vector<Motion*> solution;
00112 base::State *xstate = si_->allocState();
00113 bool startTree = rng.uniformBool();
00114
00115 while (!sol->found && ptc() == false)
00116 {
00117 bool retry = true;
00118 while (retry && !sol->found && ptc() == false)
00119 {
00120 removeList_.lock.lock();
00121 if (!removeList_.motions.empty())
00122 {
00123 if (loopLock_.try_lock())
00124 {
00125 retry = false;
00126 std::map<Motion*, bool> seen;
00127 for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
00128 if (seen.find(removeList_.motions[i].motion) == seen.end())
00129 removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
00130 removeList_.motions.clear();
00131 loopLock_.unlock();
00132 }
00133 }
00134 else
00135 retry = false;
00136 removeList_.lock.unlock();
00137 }
00138
00139 if (sol->found || ptc())
00140 break;
00141
00142 loopLockCounter_.lock();
00143 if (loopCounter_ == 0)
00144 loopLock_.lock();
00145 loopCounter_++;
00146 loopLockCounter_.unlock();
00147
00148
00149 TreeData &tree = startTree ? tStart_ : tGoal_;
00150 startTree = !startTree;
00151 TreeData &otherTree = startTree ? tStart_ : tGoal_;
00152
00153 Motion *existing = selectMotion(rng, tree);
00154 if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
00155 continue;
00156
00157
00158 Motion *motion = new Motion(si_);
00159 si_->copyState(motion->state, xstate);
00160 motion->parent = existing;
00161 motion->root = existing->root;
00162
00163 existing->lock.lock();
00164 existing->children.push_back(motion);
00165 existing->lock.unlock();
00166
00167 addMotion(tree, motion);
00168
00169 if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
00170 {
00171 sol->lock.lock();
00172 if (!sol->found)
00173 {
00174 sol->found = true;
00175 PathGeometric *path = new PathGeometric(si_);
00176 for (unsigned int i = 0 ; i < solution.size() ; ++i)
00177 path->append(solution[i]->state);
00178 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
00179 }
00180 sol->lock.unlock();
00181 }
00182
00183
00184 loopLockCounter_.lock();
00185 loopCounter_--;
00186 if (loopCounter_ == 0)
00187 loopLock_.unlock();
00188 loopLockCounter_.unlock();
00189 }
00190
00191 si_->freeState(xstate);
00192 }
00193
00194 ompl::base::PlannerStatus ompl::geometric::pSBL::solve(const base::PlannerTerminationCondition &ptc)
00195 {
00196 base::GoalState *goal = dynamic_cast<base::GoalState*>(pdef_->getGoal().get());
00197
00198 if (!goal)
00199 {
00200 logError("Unknown type of goal (or goal undefined)");
00201 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
00202 }
00203
00204 while (const base::State *st = pis_.nextStart())
00205 {
00206 Motion *motion = new Motion(si_);
00207 si_->copyState(motion->state, st);
00208 motion->valid = true;
00209 motion->root = motion->state;
00210 addMotion(tStart_, motion);
00211 }
00212
00213 if (tGoal_.size == 0)
00214 {
00215 if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
00216 {
00217 Motion *motion = new Motion(si_);
00218 si_->copyState(motion->state, goal->getState());
00219 motion->valid = true;
00220 motion->root = motion->state;
00221 addMotion(tGoal_, motion);
00222 }
00223 else
00224 logError("Goal state is invalid!");
00225 }
00226
00227 if (tStart_.size == 0)
00228 {
00229 logError("Motion planning start tree could not be initialized!");
00230 return base::PlannerStatus::INVALID_START;
00231 }
00232 if (tGoal_.size == 0)
00233 {
00234 logError("Motion planning goal tree could not be initialized!");
00235 return base::PlannerStatus::INVALID_GOAL;
00236 }
00237
00238 samplerArray_.resize(threadCount_);
00239
00240 logInform("Starting with %d states", (int)(tStart_.size + tGoal_.size));
00241
00242 SolutionInfo sol;
00243 sol.found = false;
00244 loopCounter_ = 0;
00245
00246 std::vector<boost::thread*> th(threadCount_);
00247 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00248 th[i] = new boost::thread(boost::bind(&pSBL::threadSolve, this, i, ptc, &sol));
00249 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00250 {
00251 th[i]->join();
00252 delete th[i];
00253 }
00254
00255 logInform("Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
00256 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
00257
00258 return sol.found ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00259 }
00260
00261 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
00262 {
00263 Grid<MotionInfo>::Coord coord;
00264 projectionEvaluator_->computeCoordinates(motion->state, coord);
00265
00266 otherTree.lock.lock();
00267 Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
00268
00269 if (cell && !cell->data.empty())
00270 {
00271 Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
00272 otherTree.lock.unlock();
00273
00274 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
00275 {
00276 Motion *connect = new Motion(si_);
00277
00278 si_->copyState(connect->state, connectOther->state);
00279 connect->parent = motion;
00280 connect->root = motion->root;
00281
00282 motion->lock.lock();
00283 motion->children.push_back(connect);
00284 motion->lock.unlock();
00285
00286 addMotion(tree, connect);
00287
00288 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
00289 {
00290 if (start)
00291 connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->state, connectOther->state);
00292 else
00293 connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
00294
00295
00296
00297 std::vector<Motion*> mpath1;
00298 while (motion != NULL)
00299 {
00300 mpath1.push_back(motion);
00301 motion = motion->parent;
00302 }
00303
00304 std::vector<Motion*> mpath2;
00305 while (connectOther != NULL)
00306 {
00307 mpath2.push_back(connectOther);
00308 connectOther = connectOther->parent;
00309 }
00310
00311 if (!start)
00312 mpath1.swap(mpath2);
00313
00314 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00315 solution.push_back(mpath1[i]);
00316 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
00317
00318 return true;
00319 }
00320 }
00321 }
00322 else
00323 otherTree.lock.unlock();
00324
00325 return false;
00326 }
00327
00328 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
00329 {
00330 std::vector<Motion*> mpath;
00331
00332
00333 while (motion != NULL)
00334 {
00335 mpath.push_back(motion);
00336 motion = motion->parent;
00337 }
00338
00339 bool result = true;
00340
00341
00342 for (int i = mpath.size() - 1 ; result && i >= 0 ; --i)
00343 {
00344 mpath[i]->lock.lock();
00345 if (!mpath[i]->valid)
00346 {
00347 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
00348 mpath[i]->valid = true;
00349 else
00350 {
00351
00352 PendingRemoveMotion prm;
00353 prm.tree = &tree;
00354 prm.motion = mpath[i];
00355 removeList_.lock.lock();
00356 removeList_.motions.push_back(prm);
00357 removeList_.lock.unlock();
00358 result = false;
00359 }
00360 }
00361 mpath[i]->lock.unlock();
00362 }
00363
00364 return result;
00365 }
00366
00367 ompl::geometric::pSBL::Motion* ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree)
00368 {
00369 tree.lock.lock ();
00370 GridCell* cell = tree.pdf.sample(rng.uniform01());
00371 Motion* result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : NULL;
00372 tree.lock.unlock ();
00373 return result;
00374 }
00375
00376 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen)
00377 {
00378
00379 seen[motion] = true;
00380
00381 Grid<MotionInfo>::Coord coord;
00382 projectionEvaluator_->computeCoordinates(motion->state, coord);
00383 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00384 if (cell)
00385 {
00386 for (unsigned int i = 0 ; i < cell->data.size(); ++i)
00387 if (cell->data[i] == motion)
00388 {
00389 cell->data.erase(cell->data.begin() + i);
00390 tree.size--;
00391 break;
00392 }
00393 if (cell->data.empty())
00394 {
00395 tree.pdf.remove(cell->data.elem_);
00396 tree.grid.remove(cell);
00397 tree.grid.destroyCell(cell);
00398 }
00399 else
00400 {
00401 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00402 }
00403 }
00404
00405
00406
00407 if (motion->parent)
00408 {
00409 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00410 if (motion->parent->children[i] == motion)
00411 {
00412 motion->parent->children.erase(motion->parent->children.begin() + i);
00413 break;
00414 }
00415 }
00416
00417
00418 for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00419 {
00420 motion->children[i]->parent = NULL;
00421 removeMotion(tree, motion->children[i], seen);
00422 }
00423
00424 if (motion->state)
00425 si_->freeState(motion->state);
00426 delete motion;
00427 }
00428
00429 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
00430 {
00431 Grid<MotionInfo>::Coord coord;
00432 projectionEvaluator_->computeCoordinates(motion->state, coord);
00433 tree.lock.lock();
00434 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00435 if (cell)
00436 {
00437 cell->data.push_back(motion);
00438 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00439 }
00440 else
00441 {
00442 cell = tree.grid.createCell(coord);
00443 cell->data.push_back(motion);
00444 tree.grid.add(cell);
00445 cell->data.elem_ = tree.pdf.add(cell, 1.0);
00446 }
00447 tree.size++;
00448 tree.lock.unlock();
00449 }
00450
00451 void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
00452 {
00453 Planner::getPlannerData(data);
00454
00455 std::vector<MotionInfo> motions;
00456 tStart_.grid.getContent(motions);
00457
00458 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00459 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00460 if (motions[i][j]->parent == NULL)
00461 data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
00462 else
00463 data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
00464 base::PlannerDataVertex(motions[i][j]->state, 1));
00465
00466 motions.clear();
00467 tGoal_.grid.getContent(motions);
00468 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00469 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00470 if (motions[i][j]->parent == NULL)
00471 data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
00472 else
00473
00474 data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2),
00475 base::PlannerDataVertex(motions[i][j]->parent->state, 2));
00476
00477 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
00478 }
00479
00480 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads)
00481 {
00482 assert(nthreads > 0);
00483 threadCount_ = nthreads;
00484 }