shortest_paths.cpp

Go to the documentation of this file.

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00039 #include <topological_roadmap/shortest_paths.h>
00040 #include <boost/property_map/property_map.hpp>
00041 #include <boost/graph/dijkstra_shortest_paths.hpp>
00042 #include <boost/make_shared.hpp>
00043 #include <boost/foreach.hpp>
00044 #include <ros/ros.h>
00045 #include <list>
00046 
00047 namespace topological_roadmap
00048 {
00049 
00050 namespace msg=topological_nav_msgs;
00051 
00052 using std::vector;
00053 using std::map;
00054 using std::list;
00055 using std::pair;
00056 using boost::optional;
00057 
00058 typedef map<GraphVertex, double> DistanceMap;
00059 typedef map<GraphVertex, GraphVertex> PredMap;
00060 typedef pair<DistanceMap, PredMap> DijkstraResult;
00061 
00062 DijkstraResult dijkstra (const Roadmap& r, const unsigned src)
00063 {
00064   using boost::associative_property_map;
00065   
00066   // Define some additional map types to store the weights and
00067   // indices used within Dijkstra's algorithm
00068   typedef map<GraphEdge, double> WeightMap;
00069   typedef map<GraphVertex, unsigned> IndexMap;
00070 
00071   // dijkstra_shortest_paths further requires std::map to be
00072   // wrapped in a "Property Map"
00073   typedef associative_property_map<DistanceMap> DistancePMap;
00074   typedef associative_property_map<PredMap> PredPMap;
00075   typedef associative_property_map<WeightMap> WeightPMap;
00076   typedef associative_property_map<IndexMap> IndexPMap;
00077 
00078   // OK, step 1: set the indices arbitrarily, because bgl won't do this for you
00079   IndexMap imap;
00080   IndexPMap index_pmap(imap);
00081   { 
00082     unsigned ind=0;
00083     BOOST_FOREACH (const GraphVertex v, vertices(r))
00084       imap[v] = ind++;
00085   }
00086 
00087   // Step 2: set the weights, using the edge lengths
00088   WeightMap wmap;
00089   WeightPMap weight_pmap(wmap);
00090   BOOST_FOREACH (const GraphEdge e, edges(r))
00091     wmap[e] = r[e].cost; 
00092 
00093   // Step 3: set up output maps
00094   DistanceMap dmap;
00095   DistancePMap distance_pmap(dmap);
00096   PredMap pmap;
00097   PredPMap predecessor_pmap(pmap);
00098 
00099   // Step 4: make the call
00100   boost::dijkstra_shortest_paths(r, r.node(src), weight_map(weight_pmap).\
                                 vertex_index_map(index_pmap).\
                                 distance_map(distance_pmap).\
                                 predecessor_map(predecessor_pmap));
00101 
00102   // Step 5: Return
00103   return DijkstraResult(dmap, pmap);  
00104 }
00105 
00106 
00107 struct SingleSourceShortestPaths
00108 {
00109   unsigned src;
00110   GraphVertex v;
00111   DijkstraResult res;
00112   const Roadmap& r;
00113   SingleSourceShortestPaths (unsigned src, GraphVertex v, DijkstraResult res,
00114                              const Roadmap& r) :
00115     src(src), v(v), res(res), r(r)
00116   {}
00117 };
00118 
00119 ResultPtr shortestPaths (const Roadmap& r, const unsigned src)
00120 {
00121   return boost::make_shared<SingleSourceShortestPaths>(src, r.node(src),
00122                                                        dijkstra(r, src), r);
00123 }
00124 
00125 optional<Path> extractPath (DijkstraResult& res,
00126                             const unsigned src,
00127                             const unsigned dest,
00128                             const Roadmap& r)
00129 {
00130   BOOST_FOREACH (const PredMap::value_type& e, res.second)
00131   {
00132     const unsigned n=r[e.first].id;
00133     ROS_DEBUG_NAMED ("shortest_paths",
00134                      "Predecessor of %u is %u and cost of %u is %.2f",
00135                      n, r[e.second].id, n, res.first[e.first]);
00136   }
00137   optional<Path> p;
00138   const GraphVertex w = r.node(dest);
00139   if (dest == src || res.second[w]!=w)
00140   {
00141     list<unsigned> nodes;
00142     for (GraphVertex x=w; x!=res.second[x]; x=res.second[x]) {
00143       const unsigned n=r[x].id;
00144       nodes.push_front(n);
00145     }
00146     nodes.push_front(src);
00147     p = Path();
00148     copy(nodes.begin(), nodes.end(), back_inserter(p->first));
00149   }
00150   return p;
00151 }
00152 
00153 optional<Path> extractPath (ResultPtr res, const unsigned dest)
00154 {
00155   return extractPath (res->res, res->src, dest, res->r);
00156 }
00157 
00158 } // namespace
00159