ompl/geometric/planners/prm/src/SPARStwo.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers University 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: Andrew Dobson */
00036 
00037 #include "ompl/geometric/planners/prm/SPARStwo.h"
00038 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
00039 #include "ompl/base/goals/GoalSampleableRegion.h"
00040 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00041 #include "ompl/tools/config/SelfConfig.h"
00042 #include <boost/lambda/bind.hpp>
00043 #include <boost/graph/astar_search.hpp>
00044 #include <boost/graph/incremental_components.hpp>
00045 #include <boost/property_map/vector_property_map.hpp>
00046 #include <boost/foreach.hpp>
00047 
00048 #include "GoalVisitor.hpp"
00049 
00050 #define foreach BOOST_FOREACH
00051 #define foreach_reverse BOOST_REVERSE_FOREACH
00052 
00053 ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si) :
00054     base::Planner(si, "SPARStwo"),
00055     stretchFactor_(3.),
00056     sparseDeltaFraction_(.25),
00057     denseDeltaFraction_(.001),
00058     maxFailures_(5000),
00059     nearSamplePoints_((2*si_->getStateDimension())),
00060     stateProperty_(boost::get(vertex_state_t(), g_)),
00061     weightProperty_(boost::get(boost::edge_weight, g_)),
00062     colorProperty_(boost::get(vertex_color_t(), g_)),
00063     interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_)),
00064     disjointSets_(boost::get(boost::vertex_rank, g_),
00065                   boost::get(boost::vertex_predecessor, g_)),
00066     addedSolution_(false),
00067     consecutiveFailures_(0),
00068     sparseDelta_(0.),
00069     denseDelta_(0.),
00070     iterations_(0),
00071     bestCost_(std::numeric_limits<double>::quiet_NaN())
00072 {
00073     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00074     specs_.approximateSolutions = false;
00075     specs_.optimizingPaths = true;
00076     specs_.multithreaded = true;
00077 
00078     psimp_.reset(new PathSimplifier(si_));
00079 
00080     Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor, "1.1:0.1:3.0");
00081     Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction, &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
00082     Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction, &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
00083     Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures, "100:10:3000");
00084 
00085     addPlannerProgressProperty("iterations INTEGER",
00086                                boost::bind(&SPARStwo::getIterationCount, this));
00087     addPlannerProgressProperty("best cost REAL",
00088                                boost::bind(&SPARStwo::getBestCost, this));
00089 }
00090 
00091 ompl::geometric::SPARStwo::~SPARStwo()
00092 {
00093     freeMemory();
00094 }
00095 
00096 void ompl::geometric::SPARStwo::setup()
00097 {
00098     Planner::setup();
00099     if (!nn_)
00100         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
00101     nn_->setDistanceFunction(boost::bind(&SPARStwo::distanceFunction, this, _1, _2));
00102     double maxExt = si_->getMaximumExtent();
00103     sparseDelta_ = sparseDeltaFraction_ * maxExt;
00104     denseDelta_ = denseDeltaFraction_ * maxExt;
00105 
00106     // Setup optimization objective
00107     //
00108     // If no optimization objective was specified, then default to
00109     // optimizing path length as computed by the distance() function
00110     // in the state space.
00111     if (pdef_)
00112     {
00113         if (pdef_->hasOptimizationObjective())
00114         {
00115             opt_ = pdef_->getOptimizationObjective();
00116             if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt_.get()))
00117                 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence for other optimizaton objectives is not guaranteed.", getName().c_str());
00118         }
00119         else
00120             opt_.reset(new base::PathLengthOptimizationObjective(si_));
00121     }
00122     else
00123     {
00124         OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
00125         setup_ = false;
00126     }
00127 }
00128 
00129 void ompl::geometric::SPARStwo::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00130 {
00131     Planner::setProblemDefinition(pdef);
00132     clearQuery();
00133 }
00134 
00135 void ompl::geometric::SPARStwo::clearQuery()
00136 {
00137     startM_.clear();
00138     goalM_.clear();
00139     pis_.restart();
00140 }
00141 
00142 void ompl::geometric::SPARStwo::clear()
00143 {
00144     Planner::clear();
00145     clearQuery();
00146     resetFailures();
00147     iterations_ = 0;
00148     bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
00149     freeMemory();
00150     if (nn_)
00151         nn_->clear();
00152 }
00153 
00154 void ompl::geometric::SPARStwo::freeMemory()
00155 {
00156     Planner::clear();
00157     sampler_.reset();
00158     simpleSampler_.reset();
00159 
00160     foreach (Vertex v, boost::vertices(g_))
00161     {
00162         foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values)
00163             d.clear(si_);
00164         if( stateProperty_[v] != NULL )
00165             si_->freeState(stateProperty_[v]);
00166         stateProperty_[v] = NULL;
00167     }
00168     g_.clear();
00169 
00170     if (nn_)
00171         nn_->clear();
00172 }
00173 
00174 bool ompl::geometric::SPARStwo::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution)
00175 {
00176     base::Goal *g = pdef_->getGoal().get();
00177     base::Cost sol_cost(opt_->infiniteCost());
00178     foreach (Vertex start, starts)
00179         foreach (Vertex goal, goals)
00180         {
00181             // we lock because the connected components algorithm is incremental and may change disjointSets_
00182             graphMutex_.lock();
00183             bool same_component = sameComponent(start, goal);
00184             graphMutex_.unlock();
00185 
00186             if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
00187             {
00188                 base::PathPtr p = constructSolution(start, goal);
00189                 if (p)
00190                 {
00191                     base::Cost pathCost = p->cost(opt_);
00192                     if (opt_->isCostBetterThan(pathCost, bestCost_))
00193                         bestCost_ = pathCost;
00194                     // Check if optimization objective is satisfied
00195                     if (opt_->isSatisfied(pathCost))
00196                     {
00197                         solution = p;
00198                         return true;
00199                     }
00200                     else if (opt_->isCostBetterThan(pathCost, sol_cost))
00201                     {
00202                         solution = p;
00203                         sol_cost = pathCost;
00204                     }
00205                 }
00206             }
00207         }
00208     return false;
00209 }
00210 
00211 bool ompl::geometric::SPARStwo::sameComponent(Vertex m1, Vertex m2)
00212 {
00213     return boost::same_component(m1, m2, disjointSets_);
00214 }
00215 
00216 bool ompl::geometric::SPARStwo::reachedFailureLimit() const
00217 {
00218     return consecutiveFailures_ >= maxFailures_;
00219 }
00220 
00221 bool ompl::geometric::SPARStwo::reachedTerminationCriterion() const
00222 {
00223     return consecutiveFailures_ >= maxFailures_ || addedSolution_;
00224 }
00225 
00226 void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail)
00227 {
00228     if (stopOnMaxFail)
00229     {
00230         resetFailures();
00231         base::PlannerTerminationCondition ptcOrFail =
00232             base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedFailureLimit, this)));
00233         constructRoadmap(ptcOrFail);
00234     }
00235     else
00236         constructRoadmap(ptc);
00237 }
00238 
00239 void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationCondition &ptc)
00240 {
00241     checkQueryStateInitialization();
00242 
00243     if (!isSetup())
00244         setup();
00245     if (!sampler_)
00246         sampler_ = si_->allocValidStateSampler();
00247     if (!simpleSampler_)
00248         simpleSampler_ = si_->allocStateSampler();
00249 
00250     base::State *qNew = si_->allocState();
00251     base::State *workState = si_->allocState();
00252 
00253     /* The whole neighborhood set which has been most recently computed */
00254     std::vector<Vertex> graphNeighborhood;
00255     /* The visible neighborhood set which has been most recently computed */
00256     std::vector<Vertex> visibleNeighborhood;
00257 
00258     bestCost_ = opt_->infiniteCost();
00259     while (ptc == false)
00260     {
00261         ++iterations_;
00262         ++consecutiveFailures_;
00263 
00264         //Generate a single sample, and attempt to connect it to nearest neighbors.
00265         if (!sampler_->sample(qNew))
00266             continue;
00267 
00268         findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
00269 
00270         if (!checkAddCoverage(qNew, visibleNeighborhood))
00271             if (!checkAddConnectivity(qNew, visibleNeighborhood))
00272                 if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
00273                 {
00274                     if (visibleNeighborhood.size() > 0)
00275                     {
00276                         std::map<Vertex, base::State*> closeRepresentatives;
00277                         findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
00278                         for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
00279                         {
00280                             updatePairPoints(visibleNeighborhood[0], qNew, it->first, it->second);
00281                             updatePairPoints(it->first, it->second, visibleNeighborhood[0], qNew);
00282                         }
00283                         checkAddPath(visibleNeighborhood[0]);
00284                         for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
00285                         {
00286                             checkAddPath(it->first);
00287                             si_->freeState(it->second);
00288                         }
00289                     }
00290                 }
00291     }
00292     si_->freeState(workState);
00293     si_->freeState(qNew);
00294 }
00295 
00296 void ompl::geometric::SPARStwo::checkQueryStateInitialization()
00297 {
00298     boost::mutex::scoped_lock _(graphMutex_);
00299     if (boost::num_vertices(g_) < 1)
00300     {
00301         queryVertex_ = boost::add_vertex( g_ );
00302         stateProperty_[queryVertex_] = NULL;
00303     }
00304 }
00305 
00306 ompl::base::PlannerStatus ompl::geometric::SPARStwo::solve(const base::PlannerTerminationCondition &ptc)
00307 {
00308     checkValidity();
00309     checkQueryStateInitialization();
00310 
00311     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00312 
00313     if (!goal)
00314     {
00315         OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
00316         return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
00317     }
00318 
00319     // Add the valid start states as milestones
00320     while (const base::State *st = pis_.nextStart())
00321         startM_.push_back(addGuard(si_->cloneState(st), START));
00322     if (startM_.empty())
00323     {
00324         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00325         return base::PlannerStatus::INVALID_START;
00326     }
00327 
00328     if (!goal->couldSample())
00329     {
00330         OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
00331         return base::PlannerStatus::INVALID_GOAL;
00332     }
00333 
00334     // Add the valid goal states as milestones
00335     while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
00336         goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
00337     if (goalM_.empty())
00338     {
00339         OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
00340         return base::PlannerStatus::INVALID_GOAL;
00341     }
00342 
00343     unsigned int nrStartStates = boost::num_vertices(g_) - 1;  // don't count query vertex
00344     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nrStartStates);
00345 
00346     // Reset addedSolution_ member
00347     addedSolution_ = false;
00348     resetFailures();
00349     base::PathPtr sol;
00350     base::PlannerTerminationCondition ptcOrFail =
00351         base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedFailureLimit, this)));
00352     boost::thread slnThread(boost::bind(&SPARStwo::checkForSolution, this, ptcOrFail, boost::ref(sol)));
00353 
00354     //Construct planner termination condition which also takes M into account
00355     base::PlannerTerminationCondition ptcOrStop =
00356         base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&SPARStwo::reachedTerminationCriterion, this)));
00357     constructRoadmap(ptcOrStop);
00358 
00359     // Ensure slnThread is ceased before exiting solve
00360     slnThread.join();
00361 
00362     OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
00363 
00364     if (sol)
00365         pdef_->addSolutionPath(sol, false, -1.0, getName());
00366 
00367     // Return true if any solution was found.
00368     return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00369 }
00370 
00371 void ompl::geometric::SPARStwo::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
00372 {
00373     base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00374     while (!ptc && !addedSolution_)
00375     {
00376         // Check for any new goal states
00377         if (goal->maxSampleCount() > goalM_.size())
00378         {
00379             const base::State *st = pis_.nextGoal();
00380             if (st)
00381                 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
00382         }
00383 
00384         // Check for a solution
00385         addedSolution_ = haveSolution(startM_, goalM_, solution);
00386         // Sleep for 1ms
00387         if (!addedSolution_)
00388             boost::this_thread::sleep(boost::posix_time::milliseconds(1));
00389     }
00390 }
00391 
00392 bool ompl::geometric::SPARStwo::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
00393 {
00394     if (visibleNeighborhood.size() > 0)
00395         return false;
00396     //No free paths means we add for coverage
00397     addGuard(si_->cloneState(qNew), COVERAGE);
00398     return true;
00399 }
00400 
00401 bool ompl::geometric::SPARStwo::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
00402 {
00403     std::vector<Vertex> links;
00404     if (visibleNeighborhood.size() > 1)
00405     {
00406         //For each neighbor
00407         for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
00408             //For each other neighbor
00409             for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
00410                 //If they are in different components
00411                 if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
00412                 {
00413                     links.push_back(visibleNeighborhood[i]);
00414                     links.push_back(visibleNeighborhood[j]);
00415                 }
00416 
00417         if (links.size() > 0)
00418         {
00419             //Add the node
00420             Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY);
00421 
00422             for (std::size_t i = 0; i < links.size() ; ++i)
00423                 //If there's no edge
00424                 if (!boost::edge(g, links[i], g_).second)
00425                     //And the components haven't been united by previous links
00426                     if (!sameComponent(links[i], g))
00427                         connectGuards(g, links[i]);
00428             return true;
00429         }
00430     }
00431     return false;
00432 }
00433 
00434 bool ompl::geometric::SPARStwo::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood)
00435 {
00436     //If we have more than 1 or 0 neighbors
00437     if (visibleNeighborhood.size() > 1)
00438         if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
00439             //If our two closest neighbors don't share an edge
00440             if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
00441             {
00442                 //If they can be directly connected
00443                 if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
00444                 {
00445                     //Connect them
00446                     connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
00447                     //And report that we added to the roadmap
00448                     resetFailures();
00449                     //Report success
00450                     return true;
00451                 }
00452                 else
00453                 {
00454                     //Add the new node to the graph, to bridge the interface
00455                     Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
00456                     connectGuards(v, visibleNeighborhood[0]);
00457                     connectGuards(v, visibleNeighborhood[1]);
00458                     //Report success
00459                     return true;
00460                 }
00461             }
00462     return false;
00463 }
00464 
00465 bool ompl::geometric::SPARStwo::checkAddPath( Vertex v )
00466 {
00467     bool ret = false;
00468 
00469     std::vector< Vertex > rs;
00470     foreach( Vertex r, boost::adjacent_vertices( v, g_ ) )
00471         rs.push_back(r);
00472 
00473     /* Candidate x vertices as described in the method, filled by function computeX(). */
00474     std::vector<Vertex> Xs;
00475 
00476     /* Candidate v" vertices as described in the method, filled by function computeVPP(). */
00477     std::vector<Vertex> VPPs;
00478 
00479     for (std::size_t i = 0; i < rs.size() && !ret; ++i)
00480     {
00481         Vertex r = rs[i];
00482         computeVPP(v, r, VPPs);
00483         foreach (Vertex rp, VPPs)
00484         {
00485             //First, compute the longest path through the graph
00486             computeX(v, r, rp, Xs);
00487             double rm_dist = 0.0;
00488             foreach( Vertex rpp, Xs)
00489             {
00490                 double tmp_dist = (si_->distance( stateProperty_[r], stateProperty_[v] )
00491                     + si_->distance( stateProperty_[v], stateProperty_[rpp] ) )/2.0;
00492                 if( tmp_dist > rm_dist )
00493                     rm_dist = tmp_dist;
00494             }
00495 
00496             InterfaceData& d = getData( v, r, rp );
00497 
00498             //Then, if the spanner property is violated
00499             if (rm_dist > stretchFactor_ * d.d_)
00500             {
00501                 ret = true; //Report that we added for the path
00502                 if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
00503                     connectGuards(r, rp);
00504                 else
00505                 {
00506                     PathGeometric *p = new PathGeometric( si_ );
00507                     if (r < rp)
00508                     {
00509                         p->append(d.sigmaA_);
00510                         p->append(d.pointA_);
00511                         p->append(stateProperty_[v]);
00512                         p->append(d.pointB_);
00513                         p->append(d.sigmaB_);
00514                     }
00515                     else
00516                     {
00517                         p->append(d.sigmaB_);
00518                         p->append(d.pointB_);
00519                         p->append(stateProperty_[v]);
00520                         p->append(d.pointA_);
00521                         p->append(d.sigmaA_);
00522                     }
00523 
00524                     psimp_->reduceVertices(*p, 10);
00525                     psimp_->shortcutPath(*p, 50);
00526 
00527                     if (p->checkAndRepair(100).second)
00528                     {
00529                         Vertex prior = r;
00530                         Vertex vnew;
00531                         std::vector<base::State*>& states = p->getStates();
00532 
00533                         foreach (base::State *st, states)
00534                         {
00535                             // no need to clone st, since we will destroy p; we just copy the pointer
00536                             vnew = addGuard(st , QUALITY);
00537 
00538                             connectGuards(prior, vnew);
00539                             prior = vnew;
00540                         }
00541                         // clear the states, so memory is not freed twice
00542                         states.clear();
00543                         connectGuards(prior, rp);
00544                     }
00545 
00546                     delete p;
00547                 }
00548             }
00549         }
00550     }
00551 
00552     return ret;
00553 }
00554 
00555 void ompl::geometric::SPARStwo::resetFailures()
00556 {
00557     consecutiveFailures_ = 0;
00558 }
00559 
00560 void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood)
00561 {
00562     visibleNeighborhood.clear();
00563     stateProperty_[ queryVertex_ ] = st;
00564     nn_->nearestR( queryVertex_, sparseDelta_, graphNeighborhood);
00565     stateProperty_[ queryVertex_ ] = NULL;
00566 
00567     //Now that we got the neighbors from the NN, we must remove any we can't see
00568     for (std::size_t i = 0; i < graphNeighborhood.size() ; ++i )
00569         if (si_->checkMotion(st, stateProperty_[graphNeighborhood[i]]))
00570             visibleNeighborhood.push_back(graphNeighborhood[i]);
00571 }
00572 
00573 void ompl::geometric::SPARStwo::approachGraph( Vertex v )
00574 {
00575     std::vector< Vertex > hold;
00576     nn_->nearestR( v, sparseDelta_, hold );
00577 
00578     std::vector< Vertex > neigh;
00579     for (std::size_t i = 0; i < hold.size(); ++i)
00580         if (si_->checkMotion( stateProperty_[v], stateProperty_[hold[i]]))
00581             neigh.push_back( hold[i] );
00582 
00583     foreach (Vertex vp, neigh)
00584         connectGuards(v, vp);
00585 }
00586 
00587 ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::findGraphRepresentative(base::State *st)
00588 {
00589     std::vector<Vertex> nbh;
00590     stateProperty_[ queryVertex_ ] = st;
00591     nn_->nearestR( queryVertex_, sparseDelta_, nbh);
00592     stateProperty_[queryVertex_] = NULL;
00593 
00594     Vertex result = boost::graph_traits<Graph>::null_vertex();
00595 
00596     for (std::size_t i = 0 ; i< nbh.size() ; ++i)
00597         if (si_->checkMotion(st, stateProperty_[nbh[i]]))
00598         {
00599             result = nbh[i];
00600             break;
00601         }
00602     return result;
00603 }
00604 
00605 void ompl::geometric::SPARStwo::findCloseRepresentatives(base::State *workArea, const base::State *qNew, const Vertex qRep,
00606                                                          std::map<Vertex, base::State*> &closeRepresentatives,
00607                                                          const base::PlannerTerminationCondition &ptc)
00608 {
00609     for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
00610         si_->freeState(it->second);
00611     closeRepresentatives.clear();
00612 
00613     // Then, begin searching the space around him
00614     for (unsigned int i = 0 ; i < nearSamplePoints_ ; ++i)
00615     {
00616         do
00617         {
00618             sampler_->sampleNear(workArea, qNew, denseDelta_);
00619         } while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ || !si_->checkMotion(qNew, workArea)) && ptc == false);
00620 
00621         // if we were not successful at sampling a desirable state, we are out of time
00622         if (ptc == true)
00623             break;
00624 
00625         // Compute who his graph neighbors are
00626         Vertex representative = findGraphRepresentative(workArea);
00627 
00628         // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
00629         if (representative != boost::graph_traits<Graph>::null_vertex())
00630         {
00631             //If his representative is different than qNew
00632             if (qRep != representative)
00633                 //And we haven't already tracked this representative
00634                 if (closeRepresentatives.find(representative) == closeRepresentatives.end())
00635                     //Track the representative
00636                     closeRepresentatives[representative] = si_->cloneState(workArea);
00637         }
00638         else
00639         {
00640             //This guy can't be seen by anybody, so we should take this opportunity to add him
00641             addGuard(si_->cloneState(workArea), COVERAGE);
00642 
00643             //We should also stop our efforts to add a dense path
00644             for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
00645                 si_->freeState(it->second);
00646             closeRepresentatives.clear();
00647             break;
00648         }
00649     }
00650 }
00651 
00652 void ompl::geometric::SPARStwo::updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
00653 {
00654     //First of all, we need to compute all candidate r'
00655     std::vector<Vertex> VPPs;
00656     computeVPP(rep, r, VPPs);
00657 
00658     //Then, for each pair Pv(r,r')
00659     foreach (Vertex rp, VPPs)
00660         //Try updating the pair info
00661         distanceCheck(rep, q, r, s, rp);
00662 }
00663 
00664 void ompl::geometric::SPARStwo::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)
00665 {
00666     VPPs.clear();
00667     foreach( Vertex cvpp, boost::adjacent_vertices( v, g_ ) )
00668         if( cvpp != vp )
00669             if( !boost::edge( cvpp, vp, g_ ).second )
00670                 VPPs.push_back( cvpp );
00671 }
00672 
00673 void ompl::geometric::SPARStwo::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)
00674 {
00675     Xs.clear();
00676 
00677     foreach (Vertex cx, boost::adjacent_vertices(vpp, g_))
00678         if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
00679         {
00680             InterfaceData& d = getData( v, vpp, cx );
00681             if ((vpp < cx && d.pointA_) || (cx < vpp && d.pointB_))
00682                 Xs.push_back( cx );
00683         }
00684     Xs.push_back(vpp);
00685 }
00686 
00687 ompl::geometric::SPARStwo::VertexPair ompl::geometric::SPARStwo::index( Vertex vp, Vertex vpp )
00688 {
00689     if( vp < vpp )
00690         return VertexPair( vp, vpp );
00691     else if( vpp < vp )
00692         return VertexPair( vpp, vp );
00693     else
00694         throw Exception( name_, "Trying to get an index where the pairs are the same point!");
00695 }
00696 
00697 ompl::geometric::SPARStwo::InterfaceData& ompl::geometric::SPARStwo::getData( Vertex v, Vertex vp, Vertex vpp )
00698 {
00699     return interfaceDataProperty_[v].interfaceHash[index( vp, vpp )];
00700 }
00701 
00702 void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
00703 {
00704     //Get the info for the current representative-neighbors pair
00705     InterfaceData& d = getData( rep, r, rp );
00706 
00707     if (r < rp) // FIRST points represent r (the guy discovered through sampling)
00708     {
00709         if (d.pointA_ == NULL) // If the point we're considering replacing (P_v(r,.)) isn't there
00710             //Then we know we're doing better, so add it
00711             d.setFirst(q, s, si_);
00712         else //Otherwise, he is there,
00713         {
00714             if (d.pointB_ == NULL) //But if the other guy doesn't exist, we can't compare.
00715             {
00716                 //Should probably keep the one that is further away from rep?  Not known what to do in this case.
00717                 // TODO: is this not part of the algorithm?
00718             }
00719             else //We know both of these points exist, so we can check some distances
00720                 if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_))
00721                     //Distance with the new point is good, so set it.
00722                     d.setFirst( q, s, si_ );
00723         }
00724     }
00725     else // SECOND points represent r (the guy discovered through sampling)
00726     {
00727         if (d.pointB_ == NULL) //If the point we're considering replacing (P_V(.,r)) isn't there...
00728             //Then we must be doing better, so add it
00729             d.setSecond(q, s, si_);
00730         else //Otherwise, he is there
00731         {
00732             if (d.pointA_ == NULL) //But if the other guy doesn't exist, we can't compare.
00733             {
00734                 //Should we be doing something cool here?
00735             }
00736             else
00737                 if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_))
00738                     //Distance with the new point is good, so set it
00739                     d.setSecond(q, s, si_);
00740         }
00741     }
00742 
00743     // Lastly, save what we have discovered
00744     interfaceDataProperty_[rep].interfaceHash[index(r, rp)] = d;
00745 }
00746 
00747 void ompl::geometric::SPARStwo::abandonLists(base::State *st)
00748 {
00749     stateProperty_[ queryVertex_ ] = st;
00750 
00751     std::vector< Vertex > hold;
00752     nn_->nearestR( queryVertex_, sparseDelta_, hold );
00753 
00754     stateProperty_[queryVertex_] = NULL;
00755 
00756     //For each of the vertices
00757     foreach (Vertex v, hold)
00758     {
00759         foreach (VertexPair r, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_keys)
00760             interfaceDataProperty_[v].interfaceHash[r].clear(si_);
00761     }
00762 }
00763 
00764 ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::addGuard(base::State *state, GuardType type)
00765 {
00766     boost::mutex::scoped_lock _(graphMutex_);
00767 
00768     Vertex m = boost::add_vertex(g_);
00769     stateProperty_[m] = state;
00770     colorProperty_[m] = type;
00771 
00772     assert(si_->isValid(state));
00773     abandonLists(state);
00774 
00775     disjointSets_.make_set(m);
00776     nn_->add(m);
00777     resetFailures();
00778 
00779     return m;
00780 }
00781 
00782 void ompl::geometric::SPARStwo::connectGuards(Vertex v, Vertex vp)
00783 {
00784     assert(v <= milestoneCount());
00785     assert(vp <= milestoneCount());
00786 
00787     const base::Cost weight(costHeuristic(v, vp));
00788     const Graph::edge_property_type properties(weight);
00789     boost::mutex::scoped_lock _(graphMutex_);
00790     boost::add_edge(v, vp, properties, g_);
00791     disjointSets_.union_set(v, vp);
00792 }
00793 
00794 ompl::base::PathPtr ompl::geometric::SPARStwo::constructSolution(const Vertex start, const Vertex goal) const
00795 {
00796     boost::mutex::scoped_lock _(graphMutex_);
00797 
00798     boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
00799 
00800     try
00801     {
00802         boost::astar_search(g_, start,
00803                             boost::bind(&SPARStwo::costHeuristic, this, _1, goal),
00804                             boost::predecessor_map(prev).
00805                             distance_compare(boost::bind(&base::OptimizationObjective::
00806                                                          isCostBetterThan, opt_.get(), _1, _2)).
00807                             distance_combine(boost::bind(&base::OptimizationObjective::
00808                                                          combineCosts, opt_.get(), _1, _2)).
00809                             distance_inf(opt_->infiniteCost()).
00810                             distance_zero(opt_->identityCost()).
00811                             visitor(AStarGoalVisitor<Vertex>(goal)));
00812     }
00813     catch (AStarFoundGoal&)
00814     {
00815     }
00816 
00817     if (prev[goal] == goal)
00818         throw Exception(name_, "Could not find solution path");
00819     else
00820     {
00821         PathGeometric *p = new PathGeometric(si_);
00822         for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
00823             p->append(stateProperty_[pos]);
00824         p->append(stateProperty_[start]);
00825         p->reverse();
00826 
00827         return base::PathPtr(p);
00828     }
00829 }
00830 
00831 void ompl::geometric::SPARStwo::printDebug(std::ostream &out) const
00832 {
00833     out << "SPARStwo Debug Output: " << std::endl;
00834     out << "  Settings: " << std::endl;
00835     out << "    Max Failures: " << getMaxFailures() << std::endl;
00836     out << "    Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
00837     out << "    Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
00838     out << "    Stretch Factor: " << getStretchFactor() << std::endl;
00839     out << "  Status: " << std::endl;
00840     out << "    Milestone Count: " << milestoneCount() << std::endl;
00841     out << "    Iterations: " << getIterationCount() << std::endl;
00842     out << "    Consecutive Failures: " << consecutiveFailures_ << std::endl;
00843 }
00844 
00845 void ompl::geometric::SPARStwo::getPlannerData(base::PlannerData &data) const
00846 {
00847     Planner::getPlannerData(data);
00848 
00849     // Explicitly add start and goal states:
00850     for (size_t i = 0; i < startM_.size(); ++i)
00851         data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], (int)START));
00852 
00853     for (size_t i = 0; i < goalM_.size(); ++i)
00854         data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], (int)GOAL));
00855 
00856     // If there are even edges here
00857     if (boost::num_edges( g_ ) > 0)
00858     {
00859         // Adding edges and all other vertices simultaneously
00860         foreach (const Edge e, boost::edges(g_))
00861         {
00862             const Vertex v1 = boost::source(e, g_);
00863             const Vertex v2 = boost::target(e, g_);
00864             data.addEdge(base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]),
00865                          base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]));
00866 
00867             // Add the reverse edge, since we're constructing an undirected roadmap
00868             data.addEdge(base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]),
00869                          base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]));
00870         }
00871     }
00872     else
00873         OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str());
00874 
00875     // Make sure to add edge-less nodes as well
00876     foreach (const Vertex n, boost::vertices(g_))
00877         if (boost::out_degree(n, g_) == 0)
00878             data.addVertex(base::PlannerDataVertex(stateProperty_[n], (int)colorProperty_[n]));
00879 }
00880 
00881 ompl::base::Cost ompl::geometric::SPARStwo::costHeuristic(Vertex u, Vertex v) const
00882 {
00883     return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
00884 }
00885 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines