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 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 
00052     Planner::declareParam<double>("range", this, &pSBL::setRange, &pSBL::getRange);
00053     Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount);
00054 }
00055 
00056 ompl::geometric::pSBL::~pSBL(void)
00057 {
00058     freeMemory();
00059 }
00060 
00061 void ompl::geometric::pSBL::setup(void)
00062 {
00063     Planner::setup();
00064     tools::SelfConfig sc(si_, getName());
00065     sc.configureProjectionEvaluator(projectionEvaluator_);
00066     sc.configurePlannerRange(maxDistance_);
00067 
00068     tStart_.grid.setDimension(projectionEvaluator_->getDimension());
00069     tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
00070 }
00071 
00072 void ompl::geometric::pSBL::clear(void)
00073 {
00074     Planner::clear();
00075 
00076     samplerArray_.clear();
00077 
00078     freeMemory();
00079 
00080     tStart_.grid.clear();
00081     tStart_.size = 0;
00082     tStart_.pdf.clear();
00083 
00084     tGoal_.grid.clear();
00085     tGoal_.size = 0;
00086     tGoal_.pdf.clear();
00087 
00088     removeList_.motions.clear();
00089 }
00090 
00091 void ompl::geometric::pSBL::freeGridMotions(Grid<MotionInfo> &grid)
00092 {
00093     for (Grid<MotionInfo>::iterator it = grid.begin(); it != grid.end() ; ++it)
00094     {
00095         for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00096         {
00097             if (it->second->data[i]->state)
00098                 si_->freeState(it->second->data[i]->state);
00099             delete it->second->data[i];
00100         }
00101     }
00102 }
00103 
00104 void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
00105 {
00106     checkValidity();
00107     base::GoalState *goal = static_cast<base::GoalState*>(pdef_->getGoal().get());
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                 goal->addSolutionPath(base::PathPtr(path), false, 0.0);
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 bool ompl::geometric::pSBL::solve(const base::PlannerTerminationCondition &ptc)
00194 {
00195     base::GoalState *goal = dynamic_cast<base::GoalState*>(pdef_->getGoal().get());
00196 
00197     if (!goal)
00198     {
00199         msg_.error("Unknown type of goal (or goal undefined)");
00200         return false;
00201     }
00202 
00203     while (const base::State *st = pis_.nextStart())
00204     {
00205         Motion *motion = new Motion(si_);
00206         si_->copyState(motion->state, st);
00207         motion->valid = true;
00208         motion->root = motion->state;
00209         addMotion(tStart_, motion);
00210     }
00211 
00212     if (tGoal_.size == 0)
00213     {
00214         if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
00215         {
00216             Motion *motion = new Motion(si_);
00217             si_->copyState(motion->state, goal->getState());
00218             motion->valid = true;
00219             motion->root = motion->state;
00220             addMotion(tGoal_, motion);
00221         }
00222         else
00223             msg_.error("Goal state is invalid!");
00224     }
00225 
00226     if (tStart_.size == 0 || tGoal_.size == 0)
00227     {
00228         msg_.error("Motion planning trees could not be initialized!");
00229         return false;
00230     }
00231 
00232     samplerArray_.resize(threadCount_);
00233 
00234     msg_.inform("Starting with %d states", (int)(tStart_.size + tGoal_.size));
00235 
00236     SolutionInfo sol;
00237     sol.found = false;
00238     loopCounter_ = 0;
00239 
00240     std::vector<boost::thread*> th(threadCount_);
00241     for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00242         th[i] = new boost::thread(boost::bind(&pSBL::threadSolve, this, i, ptc, &sol));
00243     for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00244     {
00245         th[i]->join();
00246         delete th[i];
00247     }
00248 
00249     msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
00250              tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
00251 
00252     return sol.found;
00253 }
00254 
00255 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
00256 {
00257     Grid<MotionInfo>::Coord coord;
00258     projectionEvaluator_->computeCoordinates(motion->state, coord);
00259 
00260     otherTree.lock.lock();
00261     Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
00262 
00263     if (cell && !cell->data.empty())
00264     {
00265         Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
00266         otherTree.lock.unlock();
00267 
00268         if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
00269         {
00270             Motion *connect = new Motion(si_);
00271 
00272             si_->copyState(connect->state, connectOther->state);
00273             connect->parent = motion;
00274             connect->root = motion->root;
00275 
00276             motion->lock.lock();
00277             motion->children.push_back(connect);
00278             motion->lock.unlock();
00279 
00280             addMotion(tree, connect);
00281 
00282             if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
00283             {
00284                 /* extract the motions and put them in solution vector */
00285 
00286                 std::vector<Motion*> mpath1;
00287                 while (motion != NULL)
00288                 {
00289                     mpath1.push_back(motion);
00290                     motion = motion->parent;
00291                 }
00292 
00293                 std::vector<Motion*> mpath2;
00294                 while (connectOther != NULL)
00295                 {
00296                     mpath2.push_back(connectOther);
00297                     connectOther = connectOther->parent;
00298                 }
00299 
00300                 if (!start)
00301                     mpath1.swap(mpath2);
00302 
00303                 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00304                     solution.push_back(mpath1[i]);
00305                 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
00306 
00307                 return true;
00308             }
00309         }
00310     }
00311     else
00312         otherTree.lock.unlock();
00313 
00314     return false;
00315 }
00316 
00317 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
00318 {
00319     std::vector<Motion*> mpath;
00320 
00321     /* construct the solution path */
00322     while (motion != NULL)
00323     {
00324         mpath.push_back(motion);
00325         motion = motion->parent;
00326     }
00327 
00328     bool result = true;
00329 
00330     /* check the path */
00331     for (int i = mpath.size() - 1 ; result && i >= 0 ; --i)
00332     {
00333         mpath[i]->lock.lock();
00334         if (!mpath[i]->valid)
00335         {
00336             if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
00337                 mpath[i]->valid = true;
00338             else
00339             {
00340                 // remember we need to remove this motion
00341                 PendingRemoveMotion prm;
00342                 prm.tree = &tree;
00343                 prm.motion = mpath[i];
00344                 removeList_.lock.lock();
00345                 removeList_.motions.push_back(prm);
00346                 removeList_.lock.unlock();
00347                 result = false;
00348             }
00349         }
00350         mpath[i]->lock.unlock();
00351     }
00352 
00353     return result;
00354 }
00355 
00356 ompl::geometric::pSBL::Motion* ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree)
00357 {
00358     tree.lock.lock ();
00359     GridCell* cell = tree.pdf.sample(rng.uniform01());
00360     Motion* result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : NULL;
00361     tree.lock.unlock ();
00362     return result;
00363 }
00364 
00365 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen)
00366 {
00367     /* remove from grid */
00368     seen[motion] = true;
00369 
00370     Grid<MotionInfo>::Coord coord;
00371     projectionEvaluator_->computeCoordinates(motion->state, coord);
00372     Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00373     if (cell)
00374     {
00375         for (unsigned int i = 0 ; i < cell->data.size(); ++i)
00376             if (cell->data[i] == motion)
00377             {
00378                 cell->data.erase(cell->data.begin() + i);
00379                 tree.size--;
00380                 break;
00381             }
00382         if (cell->data.empty())
00383         {
00384             tree.pdf.remove(cell->data.elem_);
00385             tree.grid.remove(cell);
00386             tree.grid.destroyCell(cell);
00387         }
00388         else
00389         {
00390             tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00391         }
00392     }
00393 
00394     /* remove self from parent list */
00395 
00396     if (motion->parent)
00397     {
00398         for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00399             if (motion->parent->children[i] == motion)
00400             {
00401                 motion->parent->children.erase(motion->parent->children.begin() + i);
00402                 break;
00403             }
00404     }
00405 
00406     /* remove children */
00407     for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00408     {
00409         motion->children[i]->parent = NULL;
00410         removeMotion(tree, motion->children[i], seen);
00411     }
00412 
00413     if (motion->state)
00414         si_->freeState(motion->state);
00415     delete motion;
00416 }
00417 
00418 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
00419 {
00420     Grid<MotionInfo>::Coord coord;
00421     projectionEvaluator_->computeCoordinates(motion->state, coord);
00422     tree.lock.lock();
00423     Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00424     if (cell)
00425     {
00426         cell->data.push_back(motion);
00427         tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00428     }
00429     else
00430     {
00431         cell = tree.grid.createCell(coord);
00432         cell->data.push_back(motion);
00433         tree.grid.add(cell);
00434         cell->data.elem_ = tree.pdf.add(cell, 1.0);
00435     }
00436     tree.size++;
00437     tree.lock.unlock();
00438 }
00439 
00440 void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
00441 {
00442     Planner::getPlannerData(data);
00443 
00444     std::vector<MotionInfo> motions;
00445     tStart_.grid.getContent(motions);
00446     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00447         for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00448         {
00449             data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00450             data.tagState(motions[i][j]->state, 1);
00451         }
00452 
00453     motions.clear();
00454     tGoal_.grid.getContent(motions);
00455     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00456         for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00457         {
00458             data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00459             data.tagState(motions[i][j]->state, 2);
00460         }
00461 }
00462 
00463 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads)
00464 {
00465     assert(nthreads > 0);
00466     threadCount_ = nthreads;
00467 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends