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