LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
lazysp_incsp_dijkstra.h
Go to the documentation of this file.
1 
10 namespace pr_bgl
11 {
12 
20 template <class Graph,
21  class PredecessorMap, class DistanceMap,
22  typename CompareFunction, typename CombineFunction>
24 {
25 public:
26  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
27  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
28  typedef typename boost::property_traits<DistanceMap>::value_type weight_type;
29 
32  {
33  public:
34  Vertex v_throw;
35  throw_visitor(Vertex v_throw): v_throw(v_throw) {}
36  inline void initialize_vertex(Vertex u, const Graph & g) {}
37  inline void examine_vertex(Vertex u, const Graph & g)
38  {
39  if (u == v_throw)
41  }
42  inline void examine_edge(Edge e, const Graph & g) {}
43  inline void discover_vertex(Vertex u, const Graph & g) {}
44  inline void edge_relaxed(Edge e, const Graph & g) {}
45  inline void edge_not_relaxed(Edge e, const Graph & g) {}
46  inline void finish_vertex(Vertex u, const Graph & g) {}
47  };
48 
49  PredecessorMap predecessor_map;
50  DistanceMap distance_map;
51  CompareFunction compare;
52  CombineFunction combine;
53  weight_type inf;
54  weight_type zero;
55 
57  PredecessorMap predecessor_map, DistanceMap distance_map,
58  CompareFunction compare, CombineFunction combine,
59  weight_type inf, weight_type zero):
60  predecessor_map(predecessor_map), distance_map(distance_map),
61  compare(compare), combine(combine), inf(inf), zero(zero)
62  {
63  }
64 
65  template <typename WMap>
66  weight_type solve(const Graph & g, Vertex v_start, Vertex v_goal,
67  WMap wmap, std::vector<Edge> & path)
68  {
69  try
70  {
71  boost::dijkstra_shortest_paths(
72  g,
73  v_start,
74  predecessor_map,
75  distance_map,
76  wmap,
77  get(boost::vertex_index, g), // implicit vertex index map
78  compare, combine, inf, zero,
79  throw_visitor(v_goal)
80  //boost::make_dijkstra_visitor(boost::null_visitor())
81  );
82  }
83  catch (const throw_visitor_exception & ex)
84  {
85  }
86 
87  if (get(distance_map,v_goal) == inf)
88  return inf;
89 
90  // get path
91  path.clear();
92  for (Vertex v_walk=v_goal; v_walk!=v_start;)
93  {
94  Vertex v_pred = get(predecessor_map, v_walk);
95  std::pair<Edge,bool> ret = edge(v_pred, v_walk, g);
96  BOOST_ASSERT(ret.second);
97  path.push_back(ret.first);
98  v_walk = v_pred;
99  }
100  std::reverse(path.begin(),path.end());
101 
102  return get(distance_map, v_goal);
103  }
104 
105  void update_notify(Edge e)
106  {
107  }
108 };
109 
110 template <class Graph, class PredecessorMap, class DistanceMap, typename CompareFunction, typename CombineFunction>
111 lazysp_incsp_dijkstra<Graph,PredecessorMap,DistanceMap,CompareFunction,CombineFunction>
112 make_lazysp_incsp_dijkstra(PredecessorMap predecessor_map, DistanceMap distance_map, CompareFunction compare, CombineFunction combine, typename boost::property_traits<DistanceMap>::value_type inf, typename boost::property_traits<DistanceMap>::value_type zero)
113 {
114  return lazysp_incsp_dijkstra<Graph,PredecessorMap,DistanceMap,CompareFunction,CombineFunction>(predecessor_map, distance_map, compare, combine, inf, zero);
115 }
116 
117 } // namespace pr_bgl
Definition: lazysp_incsp_dijkstra.h:31
Adaptor to use boost::dijkstra_shortest_paths as the inner sp algorithm for pr_bgl::lazysp.
Definition: lazysp_incsp_dijkstra.h:23
Definition: lazysp_incsp_dijkstra.h:30