10 template <
class Graph_,
class VState_,
class EDistance_,
class VBatch_,
class EBatch_,
class VShadow_,
class EVector_,
class NN_>
14 typedef VState_ VState;
15 typedef EDistance_ EDistance;
16 typedef VBatch_ VBatch;
17 typedef EBatch_ EBatch;
18 typedef VShadow_ VShadow;
19 typedef EVector_ EVector;
24 EDistance distance_map;
25 VBatch vertex_batch_map;
26 EBatch edge_batch_map;
27 VShadow is_shadow_map;
28 EVector edge_vector_map;
34 EDistance distance_map,
35 VBatch vertex_batch_map,
36 EBatch edge_batch_map,
37 VShadow is_shadow_map,
38 EVector edge_vector_map,
41 state_map(state_map), distance_map(distance_map),
42 vertex_batch_map(vertex_batch_map), edge_batch_map(edge_batch_map),
43 is_shadow_map(is_shadow_map),
44 edge_vector_map(edge_vector_map), nn(nn)
75 template <
class RoadmapArgs>
79 const std::string name;
81 const size_t max_batches;
83 size_t num_batches_generated;
85 typename RoadmapArgs::Graph & g;
86 typename RoadmapArgs::VState state_map;
87 typename RoadmapArgs::EDistance distance_map;
88 typename RoadmapArgs::VBatch vertex_batch_map;
89 typename RoadmapArgs::EBatch edge_batch_map;
90 typename RoadmapArgs::VShadow is_shadow_map;
91 typename RoadmapArgs::EVector edge_vector_map;
92 typename RoadmapArgs::NN * nn;
99 max_batches(max_batches),
101 num_batches_generated(0),
103 state_map(args.state_map),
104 distance_map(args.distance_map),
105 vertex_batch_map(args.vertex_batch_map),
106 edge_batch_map(args.edge_batch_map),
107 is_shadow_map(args.is_shadow_map),
108 edge_vector_map(args.edge_vector_map),
117 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Roadmap*>));
118 return static_cast<T*
>(
this);
124 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Roadmap*>));
125 return static_cast<const T*
>(
this);
128 template<
typename T,
typename RoadmapType,
typename SetterType,
typename GetterType>
129 void declareParam(
const std::string &name,
const RoadmapType &roadmap,
const SetterType& setter,
const GetterType& getter,
const std::string &rangeSuggestion =
"")
131 params.
declareParam<T>(name, boost::bind(setter, roadmap, _1), boost::bind(getter, roadmap));
132 if (!rangeSuggestion.empty())
133 params[name].setRangeSuggestion(rangeSuggestion);
136 template<
typename T,
typename RoadmapType,
typename SetterType>
137 void declareParam(
const std::string &name,
const RoadmapType &roadmap,
const SetterType& setter,
const std::string &rangeSuggestion =
"")
139 params.
declareParam<T>(name, boost::bind(setter, roadmap, _1));
140 if (!rangeSuggestion.empty())
141 params[name].setRangeSuggestion(rangeSuggestion);
154 virtual double root_radius(std::size_t i_batch) = 0;
159 virtual void deserialize(
const std::string & ser_data) = 0;
170 virtual void serialize(std::string & ser_data) = 0;
173 template <
class RoadmapArgs,
template<
class>
class RoadmapTemplate>
178 return new RoadmapTemplate<RoadmapArgs>(args);
182 template <
class RoadmapArgs>
185 std::string roadmap_id;
187 roadmap_id +=
"type=" + roadmap->name;
189 std::map<std::string, std::string> roadmap_params;
190 roadmap->params.
getParams(roadmap_params);
192 for (std::map<std::string, std::string>::iterator
193 it=roadmap_params.begin(); it!=roadmap_params.end(); it++)
195 roadmap_id +=
" " + it->first +
"=" + it->second;
203 template <
class Graph,
class VState>
207 typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
212 NNOmplBatched(Graph & g, VState state_map,
215 g(g), state_map(state_map), space(space), ompl_nn(ompl_nn)
218 inline void nearestR(Vertex v_new,
double radius, std::vector< std::pair<Vertex,double> > & vrads_near)
223 std::vector<Vertex> vs_near;
224 ompl_nn->nearestR(v_new, radius, vs_near);
225 for (
unsigned int ui=0; ui<vs_near.size(); ui++)
227 if (vs_near[ui] == v_new)
229 double dist = space->distance(
230 get(state_map, v_new)->state,
231 get(state_map, vs_near[ui])->state);
232 vrads_near.push_back(std::make_pair(vs_near[ui],dist));
236 for (
unsigned int ui=ompl_nn->size(); ui<num_vertices(g); ui++)
238 Vertex v_other = vertex(ui, g);
239 if (v_other == v_new)
241 double dist = this->space->distance(
242 get(state_map, v_new)->state,
243 get(state_map, v_other)->state);
246 vrads_near.push_back(std::make_pair(v_other,dist));
252 printf(
"syncing NNOmplBatched ...\n");
253 for (
unsigned int ui=ompl_nn->size(); ui<num_vertices(g); ui++)
254 ompl_nn->add(vertex(ui,g));
virtual void generate()=0
Generates one additional batch.
Interface for generating roadmaps over OMPL state spaces into Boost Graph objects.
Definition: Roadmap.h:76
void getParams(std::map< std::string, std::string > ¶ms) const
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
virtual void initialize()=0
Initialize roadmap; must be called once after setting parameters.
virtual void deserialize(const std::string &ser_data)=0
Re-constitute the internal generator state from serialized data.
virtual double root_radius(std::size_t i_batch)=0
Calcuate the root radius to be used for connecting to potential root vertices.
Definition: Roadmap.h:174
virtual void serialize(std::string &ser_data)=0
Serialize the internal generator state into the passed string.