29 template <
class RoadmapArgs>
32 typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
33 typedef typename GraphTypes::vertex_descriptor Vertex;
34 typedef typename GraphTypes::edge_descriptor Edge;
37 const boost::shared_ptr< ompl_lemur::Roadmap<RoadmapArgs> > _roadmap_wrapped;
41 bool _is_cache_required;
44 std::string _cache_filename;
46 std::ifstream _infile;
47 size_t _infile_num_batches;
50 std::vector<size_t> vertices_in_subgraph;
51 std::vector<size_t> edges_in_subgraph;
57 "Cached" + roadmap_wrapped->name,
58 roadmap_wrapped->max_batches),
59 _roadmap_wrapped(roadmap_wrapped),
60 _dim(roadmap_wrapped->space->getDimension()),
61 _is_cache_required(
false)
66 throw std::runtime_error(
"RoadmapCached only supports real vector state spaces!");
69 this->params.include(_roadmap_wrapped->params);
71 this->
template declareParam<bool>(
"is_cache_required",
this,
72 &RoadmapCached::setIsCacheRequired,
73 &RoadmapCached::getIsCacheRequired);
77 void setIsCacheRequired(
bool is_cache_required)
79 if (is_cache_required == _is_cache_required)
81 if (this->initialized)
82 throw std::runtime_error(
"cannot set is_cache_required, already initialized!");
83 _is_cache_required = is_cache_required;
86 bool getIsCacheRequired()
const
88 return _is_cache_required;
94 std::string
id =
"space_id " + ompl_lemur::space_id(this->space)
95 +
" roadmap_id " + ompl_lemur::roadmap_id(_roadmap_wrapped.get());
96 _cache_filename =
"or_lemur/roadmap-" + OpenRAVE::utils::GetMD5HashString(
id) +
".bin";
98 _infile_num_batches = 0;
99 bool file_found =
false;
104 std::string path = OpenRAVE::RaveFindDatabaseFile(_cache_filename,
true);
107 RAVELOG_WARN(
"Could not find cached roadmap file: %s\n", _cache_filename.c_str());
110 _infile.open(path.c_str(), std::ofstream::binary);
111 if (!_infile.is_open())
113 RAVELOG_WARN(
"Could not open cached roadmap file: %s\n", _cache_filename.c_str());
116 _infile.read((
char *)&mysizet,
sizeof(mysizet));
119 RAVELOG_WARN(
"Could not read cached roadmap file: %s\n", _cache_filename.c_str());
122 _infile_num_batches = mysizet;
124 RAVELOG_INFO(
"Found a file with %lu batches: %s\n", _infile_num_batches, _cache_filename.c_str());
128 if (!file_found && _is_cache_required)
130 throw std::runtime_error(
"RoadmapCached: cache file not found!");
134 _roadmap_wrapped->initialize();
136 this->initialized =
true;
139 void deserialize(
const std::string & ser_data)
141 throw std::runtime_error(
"RoadmapCached deserialize from ser_data not supported!");
144 double root_radius(std::size_t i_batch)
146 return _roadmap_wrapped->root_radius(i_batch);
152 if (this->num_batches_generated < _infile_num_batches)
155 size_t num_vertices_subgraph;
156 size_t num_edges_subgraph;
157 _infile.read((
char *)&num_vertices_subgraph,
sizeof(
size_t));
158 _infile.read((
char *)&num_edges_subgraph,
sizeof(
size_t));
159 RAVELOG_INFO(
"Found %lu vertices and %lu edges in cached subgraph.\n",
160 num_vertices_subgraph, num_edges_subgraph);
163 for (
size_t v_index=num_vertices(this->g); v_index<num_vertices_subgraph; v_index++)
169 _infile.read((
char *)&is_shadow,
sizeof(
size_t));
170 _infile.read((
char *)v_values, _dim*
sizeof(
double));
173 throw std::runtime_error(
"error reading vertex from cache file!");
179 Vertex v_new = add_vertex(this->g);
180 put(this->vertex_batch_map, v_new, this->num_batches_generated);
181 put(this->is_shadow_map, v_new, is_shadow ?
true :
false);
182 put(this->state_map, v_new, v_state);
183 this->nn->add(v_new);
187 for (
size_t e_index=num_edges(this->g); e_index<num_edges_subgraph; e_index++)
193 _infile.read((
char *)&vidx_a,
sizeof(
size_t));
194 _infile.read((
char *)&vidx_b,
sizeof(
size_t));
195 _infile.read((
char *)&dist,
sizeof(
double));
198 throw std::runtime_error(
"error reading edge from cache file!");
202 Vertex v_a = vertex(vidx_a, this->g);
203 Vertex v_b = vertex(vidx_b, this->g);
204 Edge e = add_edge(v_a, v_b, this->g).first;
205 put(this->distance_map, e, dist);
206 put(this->edge_batch_map, e, this->num_batches_generated);
210 throw std::runtime_error(
"error reading from cache file!");
213 this->num_batches_generated++;
220 if (_infile_num_batches
221 && this->num_batches_generated
222 && !(_roadmap_wrapped->num_batches_generated))
229 _infile.read((
char *)&ser_data_len,
sizeof(
size_t));
230 std::string ser_data;
231 ser_data.resize(ser_data_len);
232 _infile.read(&ser_data[0], ser_data_len);
233 _roadmap_wrapped->deserialize(ser_data);
235 _roadmap_wrapped->num_batches_generated = this->num_batches_generated;
240 _roadmap_wrapped->generate();
242 this->num_batches_generated = _roadmap_wrapped->num_batches_generated;
248 vertices_in_subgraph.push_back(num_vertices(this->g));
249 edges_in_subgraph.push_back(num_edges(this->g));
252 void save_file()
const
258 if (_roadmap_wrapped->num_batches_generated <= _infile_num_batches)
261 RAVELOG_INFO(
"Saving file ...\n");
263 std::string path = OpenRAVE::RaveFindDatabaseFile(_cache_filename,
false);
265 throw OpenRAVE::openrave_exception(
"couldn't find a place to write rave database entry!");
266 boost::filesystem::create_directories(boost::filesystem::path(path).parent_path());
268 std::ofstream outfile(path.c_str(), std::ofstream::binary);
272 mysizet = _roadmap_wrapped->num_batches_generated;
273 outfile.write((
const char*)&mysizet,
sizeof(mysizet));
278 for (
size_t i_batch=0; i_batch<_roadmap_wrapped->num_batches_generated; i_batch++)
280 mysizet = vertices_in_subgraph[i_batch];
281 outfile.write((
const char*)&mysizet,
sizeof(mysizet));
282 mysizet = edges_in_subgraph[i_batch];
283 outfile.write((
const char*)&mysizet,
sizeof(mysizet));
286 for (; v_index<vertices_in_subgraph[i_batch]; v_index++)
288 Vertex v = vertex(v_index,this->g);
290 mysizet =
get(this->is_shadow_map, v) ? 1 : 0;
291 outfile.write((
const char*)&mysizet,
sizeof(mysizet));
295 outfile.write((
const char*)v_values, _dim*
sizeof(
double));
299 for (; e_index<edges_in_subgraph[i_batch]; e_index++)
306 Edge e =
get(this->edge_vector_map, e_index);
307 Vertex v_a = source(e, this->g);
308 Vertex v_b = target(e, this->g);
312 dist =
get(this->distance_map, e);
314 outfile.write((
const char*)&vidx_a,
sizeof(
size_t));
315 outfile.write((
const char*)&vidx_b,
sizeof(
size_t));
316 outfile.write((
const char*)&dist,
sizeof(
double));
322 _roadmap_wrapped->serialize(data);
323 mysizet = data.size();
324 outfile.write((
const char*)&mysizet,
sizeof(mysizet));
325 outfile.write(data.data(), data.size());
328 void serialize(std::string & ser_data)
330 throw std::runtime_error(
"RoadmapCached serialize to ser_data not supported!");
335 template <
class RoadmapArgs>
339 boost::function<ompl_lemur::Roadmap<RoadmapArgs> * (RoadmapArgs args)> _factory;
349 boost::shared_ptr< ompl_lemur::Roadmap<RoadmapArgs> > roadmap_wrapped(_factory(args));
Definition: RoadmapCached.h:336
Definition: RoadmapCached.h:30