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