LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
GenerateFromRoadmap.h
Go to the documentation of this file.
1 
7 /* requires:
8 #include <ompl/datastructures/NearestNeighborsLinear.h>
9 #include <pr_bgl/throw_map.h>
10 */
11 
12 namespace ompl_lemur
13 {
14 
15 template <typename Vertex, class VState>
16 double generate_from_roadmap_dist(ompl::base::StateSpacePtr space, VState vstate_map, Vertex va, Vertex vb)
17 {
18  return space->distance(get(vstate_map,va), get(vstate_map,vb));
19 }
20 
21 // here's some code which just generates one batch of a roadmap into a graph
22 template <template<class> class RoadmapTemplate,
23  class Graph, class VState, class EDistance, class VBatch, class EBatch, class VShadow>
24 void generate_from_roadmap(
26  Graph & g,
27  VState vstate_map,
28  EDistance edistance_map,
29  VBatch vbatch_map,
30  EBatch ebatch_map,
31  VShadow vshadow_map,
32  const std::map<std::string, std::string> & roadmap_params,
33  size_t num_batches)
34 {
35  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
36  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
37 
38  // construct a simple (temporary) linear nearest neighbor object
40  NN mynn;
41  mynn.setDistanceFunction(boost::bind(generate_from_roadmap_dist<Vertex,VState>, space, vstate_map, _1, _2));
42 
43  // construct bogus edge vector map
44  typedef pr_bgl::throw_map<size_t, Edge> EVector;
45  EVector evector_map;
46 
47  // construct a RoadmapArgs object
49  RoadmapArgs args(space, g, vstate_map, edistance_map, vbatch_map, ebatch_map, vshadow_map, evector_map, &mynn);
50 
51  // construct a Roadmap object
52  RoadmapTemplate<RoadmapArgs> roadmap(args);
53 
54  // set its parameters
55  for (std::map<std::string, std::string>::const_iterator
56  it=roadmap_params.begin(); it!=roadmap_params.end(); it++)
57  {
58  roadmap.params.setParam(it->first, it->second);
59  }
60 
61  // initialize roadmap
62  roadmap.initialize();
63 
64  // generate
65  for (size_t ibatch=0; ibatch<num_batches; ibatch++)
66  roadmap.generate();
67 }
68 
69 } // namespace ompl_lemur
Definition: NearestNeighborsLinearBGL.h:11
Definition: Roadmap.h:11