LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
lazysp_incsp_lpastar.h
Go to the documentation of this file.
1 
10 namespace pr_bgl
11 {
12 
20 template <class Graph, class WMap,
21  class HeuristicMap, class PredecessorMap, class DistanceMap, class DistanceLookaheadMap,
22  typename CompareFunction, typename CombineFunction>
23 class lazysp_incsp_lpastar
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<WMap>::value_type weight_type;
29  typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VIndexMap;
30 
31  class map_heuristic
32  {
33  public:
34  HeuristicMap heuristic_map;
35  map_heuristic(HeuristicMap heuristic_map): heuristic_map(heuristic_map) {}
36  inline weight_type operator()(Vertex u)
37  {
38  return get(heuristic_map, u);
39  }
40  };
41 
42  Graph & g;
43  Vertex v_start;
44  Vertex v_goal;
45  WMap w_map;
46  PredecessorMap predecessor_map;
47  DistanceMap distance_map;
48  weight_type inf;
49 
50  // lpa* instance
51  pr_bgl::lpastar<Graph,
52  map_heuristic,
53  boost::astar_visitor<boost::null_visitor>,
54  PredecessorMap,
55  DistanceMap,
56  DistanceLookaheadMap,
57  WMap,
58  VIndexMap,
59  CompareFunction, CombineFunction,
60  weight_type, weight_type
61  > lpastar;
62 
63  lazysp_incsp_lpastar(
64  Graph & g, Vertex v_start, Vertex v_goal, WMap w_map,
65  HeuristicMap heuristic_map,
66  PredecessorMap predecessor_map, DistanceMap distance_map,
67  DistanceLookaheadMap distance_lookahead_map,
68  weight_type goal_margin,
69  CompareFunction compare, CombineFunction combine,
70  weight_type inf, weight_type zero):
71  g(g), v_start(v_start), v_goal(v_goal), w_map(w_map),
72  predecessor_map(predecessor_map),
73  distance_map(distance_map),
74  inf(inf),
75  lpastar(g, v_start, v_goal,
76  map_heuristic(heuristic_map),
77  boost::make_astar_visitor(boost::null_visitor()),
78  predecessor_map,
79  distance_map,
80  distance_lookahead_map,
81  w_map,
82  get(boost::vertex_index, g), // index_map
83  compare, combine, inf, zero,
84  goal_margin)
85  {
86  }
87 
88  weight_type solve(const Graph & g, Vertex v_start, Vertex v_goal,
89  WMap wmap, std::vector<Edge> & path)
90  {
91  // this stops at the goal vertex
92  lpastar.compute_shortest_path();
93 
94  // no solution?
95  if (get(distance_map,v_goal) == inf)
96  return inf;
97 
98  // get path
99  path.clear();
100  for (Vertex v_walk=v_goal; v_walk!=v_start;)
101  {
102  Vertex v_pred = get(predecessor_map,v_walk);
103  std::pair<Edge,bool> ret = edge(v_pred, v_walk, g);
104  BOOST_ASSERT(ret.second);
105  path.push_back(ret.first);
106  v_walk = v_pred;
107  }
108  std::reverse(path.begin(),path.end());
109 
110  return get(distance_map,v_goal);
111  }
112 
113  void update_notify(Edge euv)
114  {
115  Vertex u = source(euv,g);
116  Vertex v = target(euv,g);
117  // update forward edge
118  weight_type wuv = get(w_map,euv);
119  lpastar.update_predecessor(u, v, wuv);
120  lpastar.update_vertex(v);
121  // update reverse edge
122  Edge evu = edge(v,u,g).first;
123  weight_type wvu = get(w_map,evu);
124  lpastar.update_predecessor(v, u, wvu);
125  lpastar.update_vertex(u);
126  }
127 };
128 
129 template <class Graph, class WMap, class HeuristicMap, class PredecessorMap, class DistanceMap, class DistanceLookaheadMap, typename CompareFunction, typename CombineFunction>
130 lazysp_incsp_lpastar<Graph,WMap,HeuristicMap,PredecessorMap,DistanceMap,DistanceLookaheadMap,CompareFunction,CombineFunction>
131 make_lazysp_incsp_lpastar(
132  Graph & g,
133  typename boost::graph_traits<Graph>::vertex_descriptor v_start,
134  typename boost::graph_traits<Graph>::vertex_descriptor v_goal,
135  WMap w_map, HeuristicMap heuristic_map,
136  PredecessorMap predecessor_map, DistanceMap distance_map,
137  DistanceLookaheadMap distance_lookahead_map,
138  typename boost::property_traits<WMap>::value_type goal_margin,
139  CompareFunction compare, CombineFunction combine,
140  typename boost::property_traits<WMap>::value_type inf,
141  typename boost::property_traits<WMap>::value_type zero)
142 {
143  return lazysp_incsp_lpastar<Graph,WMap,HeuristicMap,PredecessorMap,DistanceMap,DistanceLookaheadMap,CompareFunction,CombineFunction>(
144  g, v_start, v_goal, w_map, heuristic_map, predecessor_map, distance_map, distance_lookahead_map, goal_margin, compare, combine, inf, zero);
145 }
146 
147 
148 /* uses a reversed version of lpastar under the hood!
149  * heuristic_map is therefore assumed to be the distance to the v_start vertex!
150  * wmap is still the cost for the forward edges.
151  * assume that vertex descriptors are the same
152  */
153 template <class Graph, class WMap,
154  class HeuristicMap, class PredecessorMap, class DistanceMap, class DistanceLookaheadMap,
155  typename CompareFunction, typename CombineFunction>
156 class lazysp_incsp_rlpastar
157 {
158 public:
159  typedef typename boost::reverse_graph<Graph, Graph&> RevGraph;
160  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
161  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
162  typedef typename boost::property_traits<WMap>::value_type weight_type;
163  typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VIndexMap;
164 
165  class map_heuristic
166  {
167  public:
168  HeuristicMap heuristic_map;
169  map_heuristic(HeuristicMap heuristic_map): heuristic_map(heuristic_map) {}
170  inline weight_type operator()(Vertex u)
171  {
172  return get(heuristic_map, u);
173  }
174  };
175 
176  Graph & g;
177  RevGraph rg;
178  Vertex v_start;
179  Vertex v_goal;
180  WMap w_map;
181  PredecessorMap predecessor_map;
182  DistanceMap distance_map;
183  weight_type inf;
184 
185  // lpa* instance (on reverse graph!)
186  pr_bgl::lpastar<
187  RevGraph,
188  map_heuristic,
189  boost::astar_visitor<boost::null_visitor>,
190  PredecessorMap,
191  DistanceMap,
192  DistanceLookaheadMap,
193  //WMap,
194  boost::detail::reverse_graph_edge_property_map<WMap>,
195  VIndexMap,
196  CompareFunction, CombineFunction,
197  weight_type, weight_type
198  > lpastar;
199 
200  lazysp_incsp_rlpastar(
201  Graph & g, Vertex v_start, Vertex v_goal, WMap w_map,
202  HeuristicMap heuristic_map,
203  PredecessorMap predecessor_map, DistanceMap distance_map,
204  DistanceLookaheadMap distance_lookahead_map,
205  weight_type goal_margin,
206  CompareFunction compare, CombineFunction combine,
207  weight_type inf, weight_type zero):
208  g(g), rg(g), v_start(v_start), v_goal(v_goal), w_map(w_map),
209  predecessor_map(predecessor_map),
210  distance_map(distance_map),
211  inf(inf),
212  lpastar(rg,
213  v_goal, v_start,
214  map_heuristic(heuristic_map), // already reversed
215  boost::make_astar_visitor(boost::null_visitor()),
216  predecessor_map,
217  distance_map,
218  distance_lookahead_map,
219  //w_map, // must reverse this somehow!
220  boost::detail::reverse_graph_edge_property_map<WMap>(w_map),
221  get(boost::vertex_index, g), // index_map
222  compare, combine, inf, zero,
223  goal_margin)
224  {
225  }
226 
227  weight_type solve(const Graph & g, Vertex v_start, Vertex v_goal,
228  WMap wmap, std::vector<Edge> & path)
229  {
230  // this stops at the goal vertex
231  lpastar.compute_shortest_path();
232 
233  // no solution?
234  if (get(distance_map,v_start) == inf)
235  return inf;
236 
237  // get path
238  path.clear();
239  for (Vertex v_walk=v_start; v_walk!=v_goal;)
240  {
241  Vertex v_succ = get(predecessor_map,v_walk);
242  std::pair<Edge,bool> ret = edge(v_walk, v_succ, g);
243  BOOST_ASSERT(ret.second);
244  path.push_back(ret.first);
245  v_walk = v_succ;
246  }
247 
248  return get(distance_map,v_start);
249  }
250 
251  void update_notify(Edge euv)
252  {
253  Vertex u = source(euv,g);
254  Vertex v = target(euv,g);
255  // update forward edge
256  weight_type wuv = get(w_map,euv);
257  lpastar.update_predecessor(u, v, wuv);
258  lpastar.update_vertex(v);
259  // update reverse edge
260  Edge evu = edge(v,u,g).first;
261  weight_type wvu = get(w_map,evu);
262  lpastar.update_predecessor(v, u, wvu);
263  lpastar.update_vertex(u);
264  }
265 };
266 
267 template <class Graph, class WMap, class HeuristicMap, class PredecessorMap, class DistanceMap, class DistanceLookaheadMap, typename CompareFunction, typename CombineFunction>
268 lazysp_incsp_rlpastar<Graph,WMap,HeuristicMap,PredecessorMap,DistanceMap,DistanceLookaheadMap,CompareFunction,CombineFunction>
269 make_lazysp_incsp_rlpastar(
270  Graph & g,
271  typename boost::graph_traits<Graph>::vertex_descriptor v_start,
272  typename boost::graph_traits<Graph>::vertex_descriptor v_goal,
273  WMap w_map, HeuristicMap heuristic_map,
274  PredecessorMap predecessor_map, DistanceMap distance_map,
275  DistanceLookaheadMap distance_lookahead_map,
276  typename boost::property_traits<WMap>::value_type goal_margin,
277  CompareFunction compare, CombineFunction combine,
278  typename boost::property_traits<WMap>::value_type inf,
279  typename boost::property_traits<WMap>::value_type zero)
280 {
281  return lazysp_incsp_rlpastar<Graph,WMap,HeuristicMap,PredecessorMap,DistanceMap,DistanceLookaheadMap,CompareFunction,CombineFunction>(
282  g, v_start, v_goal, w_map, heuristic_map, predecessor_map, distance_map, distance_lookahead_map, goal_margin, compare, combine, inf, zero);
283 }
284 
285 } // namespace pr_bgl
pr_bgl::lpastar
Implements the Lifelong Planning A* incremental search algorithm.
Definition: lpastar.h:34
pr_bgl::lazysp_incsp_lpastar::map_heuristic
Definition: lazysp_incsp_lpastar.h:31
pr_bgl::lazysp_incsp_rlpastar
Definition: lazysp_incsp_lpastar.h:156
pr_bgl::lazysp_incsp_lpastar
Adaptor to use pr_bgl::lpastar as the inner sp algorithm for pr_bgl::lazysp.
Definition: lazysp_incsp_lpastar.h:23
pr_bgl::lazysp_incsp_rlpastar::map_heuristic
Definition: lazysp_incsp_lpastar.h:165
Generated on Sat Sep 3 2016 01:25:38 for pr_bgl by   doxygen 1.8.6 using Read the Docs