All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/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);
00054     Planner::declareParam<unsigned int>("thread_count", this, &pSBL::setThreadCount, &pSBL::getThreadCount);
00055 }
00056 
00057 ompl::geometric::pSBL::~pSBL(void)
00058 {
00059     freeMemory();
00060 }
00061 
00062 void ompl::geometric::pSBL::setup(void)
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(void)
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     checkValidity();
00109     RNG              rng;
00110 
00111     std::vector<Motion*> solution;
00112     base::State *xstate = si_->allocState();
00113     bool      startTree = rng.uniformBool();
00114 
00115     while (!sol->found && ptc() == false)
00116     {
00117         bool retry = true;
00118         while (retry && !sol->found && ptc() == false)
00119         {
00120             removeList_.lock.lock();
00121             if (!removeList_.motions.empty())
00122             {
00123                 if (loopLock_.try_lock())
00124                 {
00125                     retry = false;
00126                     std::map<Motion*, bool> seen;
00127                     for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
00128                         if (seen.find(removeList_.motions[i].motion) == seen.end())
00129                             removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
00130                     removeList_.motions.clear();
00131                     loopLock_.unlock();
00132                 }
00133             }
00134             else
00135                 retry = false;
00136             removeList_.lock.unlock();
00137         }
00138 
00139         if (sol->found || ptc())
00140             break;
00141 
00142         loopLockCounter_.lock();
00143         if (loopCounter_ == 0)
00144             loopLock_.lock();
00145         loopCounter_++;
00146         loopLockCounter_.unlock();
00147 
00148 
00149         TreeData &tree      = startTree ? tStart_ : tGoal_;
00150         startTree = !startTree;
00151         TreeData &otherTree = startTree ? tStart_ : tGoal_;
00152 
00153         Motion *existing = selectMotion(rng, tree);
00154         if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
00155             continue;
00156 
00157         /* create a motion */
00158         Motion *motion = new Motion(si_);
00159         si_->copyState(motion->state, xstate);
00160         motion->parent = existing;
00161         motion->root = existing->root;
00162 
00163         existing->lock.lock();
00164         existing->children.push_back(motion);
00165         existing->lock.unlock();
00166 
00167         addMotion(tree, motion);
00168 
00169         if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
00170         {
00171             sol->lock.lock();
00172             if (!sol->found)
00173             {
00174                 sol->found = true;
00175                 PathGeometric *path = new PathGeometric(si_);
00176                 for (unsigned int i = 0 ; i < solution.size() ; ++i)
00177                     path->append(solution[i]->state);
00178                 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
00179             }
00180             sol->lock.unlock();
00181         }
00182 
00183 
00184         loopLockCounter_.lock();
00185         loopCounter_--;
00186         if (loopCounter_ == 0)
00187             loopLock_.unlock();
00188         loopLockCounter_.unlock();
00189     }
00190 
00191     si_->freeState(xstate);
00192 }
00193 
00194 ompl::base::PlannerStatus ompl::geometric::pSBL::solve(const base::PlannerTerminationCondition &ptc)
00195 {
00196     base::GoalState *goal = dynamic_cast<base::GoalState*>(pdef_->getGoal().get());
00197 
00198     if (!goal)
00199     {
00200         logError("Unknown type of goal (or goal undefined)");
00201         return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
00202     }
00203 
00204     while (const base::State *st = pis_.nextStart())
00205     {
00206         Motion *motion = new Motion(si_);
00207         si_->copyState(motion->state, st);
00208         motion->valid = true;
00209         motion->root = motion->state;
00210         addMotion(tStart_, motion);
00211     }
00212 
00213     if (tGoal_.size == 0)
00214     {
00215         if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
00216         {
00217             Motion *motion = new Motion(si_);
00218             si_->copyState(motion->state, goal->getState());
00219             motion->valid = true;
00220             motion->root = motion->state;
00221             addMotion(tGoal_, motion);
00222         }
00223         else
00224             logError("Goal state is invalid!");
00225     }
00226 
00227     if (tStart_.size == 0)
00228     {
00229         logError("Motion planning start tree could not be initialized!");
00230         return base::PlannerStatus::INVALID_START;
00231     }
00232     if (tGoal_.size == 0)
00233     {
00234         logError("Motion planning goal tree could not be initialized!");
00235         return base::PlannerStatus::INVALID_GOAL;
00236     }
00237 
00238     samplerArray_.resize(threadCount_);
00239 
00240     logInform("Starting with %d states", (int)(tStart_.size + tGoal_.size));
00241 
00242     SolutionInfo sol;
00243     sol.found = false;
00244     loopCounter_ = 0;
00245 
00246     std::vector<boost::thread*> th(threadCount_);
00247     for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00248         th[i] = new boost::thread(boost::bind(&pSBL::threadSolve, this, i, ptc, &sol));
00249     for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00250     {
00251         th[i]->join();
00252         delete th[i];
00253     }
00254 
00255     logInform("Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
00256              tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
00257 
00258     return sol.found ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00259 }
00260 
00261 bool ompl::geometric::pSBL::checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
00262 {
00263     Grid<MotionInfo>::Coord coord;
00264     projectionEvaluator_->computeCoordinates(motion->state, coord);
00265 
00266     otherTree.lock.lock();
00267     Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
00268 
00269     if (cell && !cell->data.empty())
00270     {
00271         Motion *connectOther = cell->data[rng.uniformInt(0, cell->data.size() - 1)];
00272         otherTree.lock.unlock();
00273 
00274         if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
00275         {
00276             Motion *connect = new Motion(si_);
00277 
00278             si_->copyState(connect->state, connectOther->state);
00279             connect->parent = motion;
00280             connect->root = motion->root;
00281 
00282             motion->lock.lock();
00283             motion->children.push_back(connect);
00284             motion->lock.unlock();
00285 
00286             addMotion(tree, connect);
00287 
00288             if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
00289             {
00290                 if (start)
00291                     connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->state, connectOther->state);
00292                 else
00293                     connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
00294 
00295                 /* extract the motions and put them in solution vector */
00296 
00297                 std::vector<Motion*> mpath1;
00298                 while (motion != NULL)
00299                 {
00300                     mpath1.push_back(motion);
00301                     motion = motion->parent;
00302                 }
00303 
00304                 std::vector<Motion*> mpath2;
00305                 while (connectOther != NULL)
00306                 {
00307                     mpath2.push_back(connectOther);
00308                     connectOther = connectOther->parent;
00309                 }
00310 
00311                 if (!start)
00312                     mpath1.swap(mpath2);
00313 
00314                 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00315                     solution.push_back(mpath1[i]);
00316                 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
00317 
00318                 return true;
00319             }
00320         }
00321     }
00322     else
00323         otherTree.lock.unlock();
00324 
00325     return false;
00326 }
00327 
00328 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
00329 {
00330     std::vector<Motion*> mpath;
00331 
00332     /* construct the solution path */
00333     while (motion != NULL)
00334     {
00335         mpath.push_back(motion);
00336         motion = motion->parent;
00337     }
00338 
00339     bool result = true;
00340 
00341     /* check the path */
00342     for (int i = mpath.size() - 1 ; result && i >= 0 ; --i)
00343     {
00344         mpath[i]->lock.lock();
00345         if (!mpath[i]->valid)
00346         {
00347             if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
00348                 mpath[i]->valid = true;
00349             else
00350             {
00351                 // remember we need to remove this motion
00352                 PendingRemoveMotion prm;
00353                 prm.tree = &tree;
00354                 prm.motion = mpath[i];
00355                 removeList_.lock.lock();
00356                 removeList_.motions.push_back(prm);
00357                 removeList_.lock.unlock();
00358                 result = false;
00359             }
00360         }
00361         mpath[i]->lock.unlock();
00362     }
00363 
00364     return result;
00365 }
00366 
00367 ompl::geometric::pSBL::Motion* ompl::geometric::pSBL::selectMotion(RNG &rng, TreeData &tree)
00368 {
00369     tree.lock.lock ();
00370     GridCell* cell = tree.pdf.sample(rng.uniform01());
00371     Motion* result = cell && !cell->data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : NULL;
00372     tree.lock.unlock ();
00373     return result;
00374 }
00375 
00376 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen)
00377 {
00378     /* remove from grid */
00379     seen[motion] = true;
00380 
00381     Grid<MotionInfo>::Coord coord;
00382     projectionEvaluator_->computeCoordinates(motion->state, coord);
00383     Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00384     if (cell)
00385     {
00386         for (unsigned int i = 0 ; i < cell->data.size(); ++i)
00387             if (cell->data[i] == motion)
00388             {
00389                 cell->data.erase(cell->data.begin() + i);
00390                 tree.size--;
00391                 break;
00392             }
00393         if (cell->data.empty())
00394         {
00395             tree.pdf.remove(cell->data.elem_);
00396             tree.grid.remove(cell);
00397             tree.grid.destroyCell(cell);
00398         }
00399         else
00400         {
00401             tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00402         }
00403     }
00404 
00405     /* remove self from parent list */
00406 
00407     if (motion->parent)
00408     {
00409         for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00410             if (motion->parent->children[i] == motion)
00411             {
00412                 motion->parent->children.erase(motion->parent->children.begin() + i);
00413                 break;
00414             }
00415     }
00416 
00417     /* remove children */
00418     for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00419     {
00420         motion->children[i]->parent = NULL;
00421         removeMotion(tree, motion->children[i], seen);
00422     }
00423 
00424     if (motion->state)
00425         si_->freeState(motion->state);
00426     delete motion;
00427 }
00428 
00429 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
00430 {
00431     Grid<MotionInfo>::Coord coord;
00432     projectionEvaluator_->computeCoordinates(motion->state, coord);
00433     tree.lock.lock();
00434     Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00435     if (cell)
00436     {
00437         cell->data.push_back(motion);
00438         tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00439     }
00440     else
00441     {
00442         cell = tree.grid.createCell(coord);
00443         cell->data.push_back(motion);
00444         tree.grid.add(cell);
00445         cell->data.elem_ = tree.pdf.add(cell, 1.0);
00446     }
00447     tree.size++;
00448     tree.lock.unlock();
00449 }
00450 
00451 void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
00452 {
00453     Planner::getPlannerData(data);
00454 
00455     std::vector<MotionInfo> motions;
00456     tStart_.grid.getContent(motions);
00457 
00458     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00459         for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00460             if (motions[i][j]->parent == NULL)
00461                 data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
00462             else
00463                 data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
00464                              base::PlannerDataVertex(motions[i][j]->state, 1));
00465 
00466     motions.clear();
00467     tGoal_.grid.getContent(motions);
00468     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00469         for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00470             if (motions[i][j]->parent == NULL)
00471                 data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
00472             else
00473                 // The edges in the goal tree are reversed so that they are in the same direction as start tree
00474                 data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2),
00475                              base::PlannerDataVertex(motions[i][j]->parent->state, 2));
00476 
00477     data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
00478 }
00479 
00480 void ompl::geometric::pSBL::setThreadCount(unsigned int nthreads)
00481 {
00482     assert(nthreads > 0);
00483     threadCount_ = nthreads;
00484 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines