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