10 template <
class RoadmapArgs>
13 typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
14 typedef typename GraphTypes::vertex_descriptor Vertex;
15 typedef typename GraphTypes::vertex_iterator VertexIter;
16 typedef typename GraphTypes::edge_descriptor Edge;
17 typedef typename GraphTypes::edge_iterator EdgeIter;
20 std::string _filename;
31 throw std::runtime_error(
"RoadmapFromFile only supports rel vector state spaces!");
33 this->
template declareParam<std::string>(
"filename",
this,
34 &RoadmapFromFile::setFilename,
35 &RoadmapFromFile::getFilename);
36 this->
template declareParam<double>(
"root_radius",
this,
37 &RoadmapFromFile::setRootRadius,
38 &RoadmapFromFile::getRootRadius);
41 void setFilename(std::string filename)
43 if (filename == _filename)
45 if (this->initialized)
46 throw std::runtime_error(
"cannot set filename, already initialized!");
50 std::string getFilename()
const
57 if (root_radius == _root_radius)
59 if (this->initialized)
60 throw std::runtime_error(
"cannot set root_radius, already initialized!");
64 double getRootRadius()
const
71 std::vector<std::string> missings;
73 missings.push_back(
"filename");
74 if (_root_radius == 0.0)
75 missings.push_back(
"root_radius");
78 std::string str =
"Cannot initialize, parameters not set:";
79 for (
unsigned int ui=0; ui<missings.size(); ui++)
80 str +=
" " + missings[ui];
81 throw std::runtime_error(str);
83 this->initialized =
true;
88 throw std::runtime_error(
"RoadmapFromFile deserialize from ser_data not supported!");
101 if (this->max_batches < this->num_batches_generated + 1)
102 throw std::runtime_error(
"this roadmap gen doesnt support that many batches!");
105 fp.open(_filename.c_str());
107 boost::dynamic_properties props;
109 props.property(
"state",
110 ompl_lemur::make_rvstate_map_string_adaptor(
113 boost::read_graphml(fp, this->g, props);
115 VertexIter vi, vi_end;
116 for (boost::tie(vi,vi_end)=vertices(this->g); vi!=vi_end; ++vi)
118 put(this->vertex_batch_map, *vi, 0);
119 put(this->is_shadow_map, *vi,
false);
124 for (boost::tie(ei,ei_end)=edges(this->g); ei!=ei_end; ++ei)
126 put(this->edge_batch_map, *ei, 0);
129 put(this->distance_map, *ei, this->space->distance(state1, state2));
132 this->num_batches_generated++;
137 throw std::runtime_error(
"RoadmapFromFile serialize to ser_data not supported!");
Definition: RoadmapFromFile.h:11
void deserialize(const std::string &ser_data)
Re-constitute the internal generator state from serialized data.
Definition: RoadmapFromFile.h:86
Interface for generating roadmaps over OMPL state spaces into Boost Graph objects.
Definition: Roadmap.h:76
void generate()
Generates one additional batch.
Definition: RoadmapFromFile.h:99
void serialize(std::string &ser_data)
Serialize the internal generator state into the passed string.
Definition: RoadmapFromFile.h:135
double root_radius(std::size_t i_batch)
Calcuate the root radius to be used for connecting to potential root vertices.
Definition: RoadmapFromFile.h:92
void initialize()
Initialize roadmap; must be called once after setting parameters.
Definition: RoadmapFromFile.h:69