ompl/geometric/planners/sbl/src/pSBL.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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, "0.:1.:10000."); 00054 Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount, "1:64"); 00055 } 00056 00057 ompl::geometric::pSBL::~pSBL() 00058 { 00059 freeMemory(); 00060 } 00061 00062 void ompl::geometric::pSBL::setup() 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() 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 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 /* create a motion */ 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 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName()); 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 ompl::base::PlannerStatus ompl::geometric::pSBL::solve(const base::PlannerTerminationCondition &ptc) 00194 { 00195 checkValidity(); 00196 00197 base::GoalState *goal = dynamic_cast<base::GoalState*>(pdef_->getGoal().get()); 00198 00199 if (!goal) 00200 { 00201 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00202 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00203 } 00204 00205 while (const base::State *st = pis_.nextStart()) 00206 { 00207 Motion *motion = new Motion(si_); 00208 si_->copyState(motion->state, st); 00209 motion->valid = true; 00210 motion->root = motion->state; 00211 addMotion(tStart_, motion); 00212 } 00213 00214 if (tGoal_.size == 0) 00215 { 00216 if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState())) 00217 { 00218 Motion *motion = new Motion(si_); 00219 si_->copyState(motion->state, goal->getState()); 00220 motion->valid = true; 00221 motion->root = motion->state; 00222 addMotion(tGoal_, motion); 00223 } 00224 else 00225 OMPL_ERROR("%s: Goal state is invalid!", getName().c_str()); 00226 } 00227 00228 if (tStart_.size == 0) 00229 { 00230 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); 00231 return base::PlannerStatus::INVALID_START; 00232 } 00233 if (tGoal_.size == 0) 00234 { 00235 OMPL_ERROR("%s: Motion planning goal tree could not be initialized!", getName().c_str()); 00236 return base::PlannerStatus::INVALID_GOAL; 00237 } 00238 00239 samplerArray_.resize(threadCount_); 00240 00241 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(tStart_.size + tGoal_.size)); 00242 00243 SolutionInfo sol; 00244 sol.found = false; 00245 loopCounter_ = 0; 00246 00247 std::vector<boost::thread*> th(threadCount_); 00248 for (unsigned int i = 0 ; i < threadCount_ ; ++i) 00249 th[i] = new boost::thread(boost::bind(&pSBL::threadSolve, this, i, ptc, &sol)); 00250 for (unsigned int i = 0 ; i < threadCount_ ; ++i) 00251 { 00252 th[i]->join(); 00253 delete th[i]; 00254 } 00255 00256 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", 00257 getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, 00258 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size()); 00259 00260 return sol.found ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00261 } 00262 00263 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution) 00264 { 00265 Grid<MotionInfo>::Coord coord; 00266 projectionEvaluator_->computeCoordinates(motion->state, coord); 00267 00268 otherTree.lock.lock(); 00269 Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord); 00270 00271 if (cell && !cell->data.empty()) 00272 { 00273 Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)]; 00274 otherTree.lock.unlock(); 00275 00276 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root)) 00277 { 00278 Motion *connect = new Motion(si_); 00279 00280 si_->copyState(connect->state, connectOther->state); 00281 connect->parent = motion; 00282 connect->root = motion->root; 00283 00284 motion->lock.lock(); 00285 motion->children.push_back(connect); 00286 motion->lock.unlock(); 00287 00288 addMotion(tree, connect); 00289 00290 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther)) 00291 { 00292 if (start) 00293 connectionPoint_ = std::make_pair(motion->state, connectOther->state); 00294 else 00295 connectionPoint_ = std::make_pair(connectOther->state, motion->state); 00296 00297 /* extract the motions and put them in solution vector */ 00298 00299 std::vector<Motion*> mpath1; 00300 while (motion != NULL) 00301 { 00302 mpath1.push_back(motion); 00303 motion = motion->parent; 00304 } 00305 00306 std::vector<Motion*> mpath2; 00307 while (connectOther != NULL) 00308 { 00309 mpath2.push_back(connectOther); 00310 connectOther = connectOther->parent; 00311 } 00312 00313 if (!start) 00314 mpath1.swap(mpath2); 00315 00316 for (int i = mpath1.size() - 1 ; i >= 0 ; --i) 00317 solution.push_back(mpath1[i]); 00318 solution.insert(solution.end(), mpath2.begin(), mpath2.end()); 00319 00320 return true; 00321 } 00322 } 00323 } 00324 else 00325 otherTree.lock.unlock(); 00326 00327 return false; 00328 } 00329 00330 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion) 00331 { 00332 std::vector<Motion*> mpath; 00333 00334 /* construct the solution path */ 00335 while (motion != NULL) 00336 { 00337 mpath.push_back(motion); 00338 motion = motion->parent; 00339 } 00340 00341 bool result = true; 00342 00343 /* check the path */ 00344 for (int i = mpath.size() - 1 ; result && i >= 0 ; --i) 00345 { 00346 mpath[i]->lock.lock(); 00347 if (!mpath[i]->valid) 00348 { 00349 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state)) 00350 mpath[i]->valid = true; 00351 else 00352 { 00353 // remember we need to remove this motion 00354 PendingRemoveMotion prm; 00355 prm.tree = &tree; 00356 prm.motion = mpath[i]; 00357 removeList_.lock.lock(); 00358 removeList_.motions.push_back(prm); 00359 removeList_.lock.unlock(); 00360 result = false; 00361 } 00362 } 00363 mpath[i]->lock.unlock(); 00364 } 00365 00366 return result; 00367 } 00368 00369 ompl::geometric::pSBL::Motion* ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree) 00370 { 00371 tree.lock.lock (); 00372 GridCell* cell = tree.pdf.sample(rng.uniform01()); 00373 Motion *result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : NULL; 00374 tree.lock.unlock (); 00375 return result; 00376 } 00377 00378 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen) 00379 { 00380 /* remove from grid */ 00381 seen[motion] = true; 00382 00383 Grid<MotionInfo>::Coord coord; 00384 projectionEvaluator_->computeCoordinates(motion->state, coord); 00385 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord); 00386 if (cell) 00387 { 00388 for (unsigned int i = 0 ; i < cell->data.size(); ++i) 00389 if (cell->data[i] == motion) 00390 { 00391 cell->data.erase(cell->data.begin() + i); 00392 tree.size--; 00393 break; 00394 } 00395 if (cell->data.empty()) 00396 { 00397 tree.pdf.remove(cell->data.elem_); 00398 tree.grid.remove(cell); 00399 tree.grid.destroyCell(cell); 00400 } 00401 else 00402 { 00403 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size()); 00404 } 00405 } 00406 00407 /* remove self from parent list */ 00408 00409 if (motion->parent) 00410 { 00411 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i) 00412 if (motion->parent->children[i] == motion) 00413 { 00414 motion->parent->children.erase(motion->parent->children.begin() + i); 00415 break; 00416 } 00417 } 00418 00419 /* remove children */ 00420 for (unsigned int i = 0 ; i < motion->children.size() ; ++i) 00421 { 00422 motion->children[i]->parent = NULL; 00423 removeMotion(tree, motion->children[i], seen); 00424 } 00425 00426 if (motion->state) 00427 si_->freeState(motion->state); 00428 delete motion; 00429 } 00430 00431 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion) 00432 { 00433 Grid<MotionInfo>::Coord coord; 00434 projectionEvaluator_->computeCoordinates(motion->state, coord); 00435 tree.lock.lock(); 00436 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord); 00437 if (cell) 00438 { 00439 cell->data.push_back(motion); 00440 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size()); 00441 } 00442 else 00443 { 00444 cell = tree.grid.createCell(coord); 00445 cell->data.push_back(motion); 00446 tree.grid.add(cell); 00447 cell->data.elem_ = tree.pdf.add(cell, 1.0); 00448 } 00449 tree.size++; 00450 tree.lock.unlock(); 00451 } 00452 00453 void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const 00454 { 00455 Planner::getPlannerData(data); 00456 00457 std::vector<MotionInfo> motions; 00458 tStart_.grid.getContent(motions); 00459 00460 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00461 for (unsigned int j = 0 ; j < motions[i].size() ; ++j) 00462 if (motions[i][j]->parent == NULL) 00463 data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1)); 00464 else 00465 data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1), 00466 base::PlannerDataVertex(motions[i][j]->state, 1)); 00467 00468 motions.clear(); 00469 tGoal_.grid.getContent(motions); 00470 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00471 for (unsigned int j = 0 ; j < motions[i].size() ; ++j) 00472 if (motions[i][j]->parent == NULL) 00473 data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2)); 00474 else 00475 // The edges in the goal tree are reversed so that they are in the same direction as start tree 00476 data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2), 00477 base::PlannerDataVertex(motions[i][j]->parent->state, 2)); 00478 00479 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second)); 00480 } 00481 00482 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads) 00483 { 00484 assert(nthreads > 0); 00485 threadCount_ = nthreads; 00486 }