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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines