All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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/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         /* create a motion */
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                 /* extract the motions and put them in solution vector */
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     /* construct the solution path */
00302     while (motion != NULL)
00303     {
00304         mpath.push_back(motion);
00305         motion = motion->parent;
00306     }
00307 
00308     bool result = true;
00309 
00310     /* check the path */
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                 // remember we need to remove this motion
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     /* remove from grid */
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     /* remove self from parent list */
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     /* remove children */
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 }