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