LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
FamilyTagCache.h
Go to the documentation of this file.
1 
7 namespace ompl_lemur
8 {
9 
40 template <class VTagMap, class ETagMap>
41 class FamilyTagCache: public TagCache<VTagMap,ETagMap>
42 {
43 public:
44 
45  // ok, we need access to the FamilyUtilityChecker object
46  FamilyUtilityCheckerPtr _checker;
47 
48  // we pre-computed mapping vectors given the fixed input family
49  // for each cached set
50  // for loading
51  struct CachedSet
52  {
53  std::string name; // set name
54  std::string filename;
55  std::string header;
56  size_t var;
57  std::vector<size_t> load_valid_map; // map if state is valid
58  std::vector<size_t> load_invalid_map; // map if state is invalid
59 #if 0
60  std::vector<char> save_map; // from tag to 'U' 'V' or 'I'
61 #endif
62  // live stuff
63  FILE * fp;
64  };
65  std::map<size_t, CachedSet> _cached_sets;
66 
67  FamilyTagCache(FamilyUtilityCheckerPtr checker):
68  _checker(checker)
69  {
70  }
71 
72  void addCachedSet(std::string set_name, std::string cache_filename, std::string header)
73  {
74  CachedSet newset;
75 
76  newset.name = set_name;
77  newset.filename = cache_filename;
78  newset.header = header;
79  newset.fp = 0;
80 
81  // find var for this target set
82  // this will throw if it's not found!
83  newset.var = _checker->getSetIndex(set_name);
84 
85 #if 0
86  // compute save maps
87  newset.save_map.resize(num_vertices(_checker->_g));
88 #endif
89 
90  // compute load maps
91  // these are now extended on loadBegin() from the underlying checker's numTags()
92  // and then calculated on-demand!
93  //newset.load_valid_map.resize(num_vertices(_checker->_g));
94  //newset.load_invalid_map.resize(num_vertices(_checker->_g));
95 
96  _cached_sets.insert(std::make_pair(newset.var, newset));
97  }
98 
99  // below is used by the planner
100 
101  bool hasChanged()
102  {
103  return false;
104  }
105 
106  void loadBegin()
107  {
108  for (typename std::map<size_t, CachedSet>::iterator
109  iset=_cached_sets.begin(); iset!=_cached_sets.end(); iset++)
110  {
111  const char * filename = iset->second.filename.c_str();
112  iset->second.fp = fopen(filename, "r");
113  if (!iset->second.fp)
114  {
115  OMPL_INFORM("Family tag cache file \"%s\" not found.", filename);
116  continue;
117  }
118  // make sure header matches
119  size_t size_expected = iset->second.header.size();
120  std::string header_read(size_expected, ' ');
121  size_t size_read = fread(&header_read[0], 1, size_expected, iset->second.fp);
122  if (size_read != size_expected || header_read != iset->second.header)
123  {
124  OMPL_ERROR("Error, header mismatch in family tag cache file \"%s\".", filename);
125  fclose(iset->second.fp);
126  iset->second.fp = 0;
127  continue;
128  }
129  // extend the load maps with 0's
130  iset->second.load_valid_map.resize(_checker->numTags(), 0);
131  iset->second.load_invalid_map.resize(_checker->numTags(), 0);
132  }
133  }
134 
135  void loadBatch(size_t batch,
136  VTagMap v_tag_map, size_t v_from, size_t v_to,
137  ETagMap e_tag_map, size_t e_from, size_t e_to)
138  {
139  if (batch != 0 || v_from != 0 || e_from != 0)
140  {
141  OMPL_ERROR("We can only load the first batch for now.");
142  return;
143  }
144  for (typename std::map<size_t, CachedSet>::iterator
145  iset=_cached_sets.begin(); iset!=_cached_sets.end(); iset++)
146  {
147  OMPL_INFORM("Loading batch %lu for set \"%s\" from file \"%s\" ...",
148  batch, iset->second.name.c_str(), iset->second.filename.c_str());
149  if (!iset->second.fp)
150  continue;
151  // read batch header
152  size_t read_batch;
153  size_t read_vertices;
154  size_t read_edges;
155  int n_stored = fscanf(iset->second.fp, "batch %lu num_vertices %lu num_edges %lu\n",
156  &read_batch, &read_vertices, &read_edges);
157  if (n_stored != 3 || read_vertices != (v_to-v_from) || read_edges != (e_to-e_from))
158  {
159  OMPL_ERROR("Batch mismatch from file.");
160  continue;
161  }
162  // get ready to fread
163  char buffer[65536];
164  size_t bytes_read;
165  // read vertex start
166  bytes_read = fread(buffer, 1, 9, iset->second.fp);
167  if (bytes_read != 9 || strncmp(buffer,"vertices ",9)!=0)
168  {
169  OMPL_ERROR("Vertex mismatch from file.");
170  continue;
171  }
172  // read vertices loop
173  size_t v_index;
174  for (v_index=v_from; v_index<v_to;)
175  {
176  size_t bytes = sizeof(buffer);
177  if (bytes > v_to - v_index)
178  bytes = v_to - v_index;
179  bytes_read = fread(buffer, 1, bytes, iset->second.fp);
180  if (bytes_read != bytes)
181  {
182  OMPL_ERROR("Vertex mismatch from file.");
183  break;
184  }
185  for (size_t i=0; i<bytes; i++,v_index++)
186  {
187  size_t new_tag;
188  switch (buffer[i])
189  {
190  case 'U': break;
191  case 'V':
192  new_tag = iset->second.load_valid_map[v_tag_map[v_index]];
193  if (!new_tag)
194  {
195  new_tag = _checker->tagIfSetKnown(v_tag_map[v_index], iset->second.var, true);
196  iset->second.load_valid_map[v_tag_map[v_index]] = new_tag;
197  }
198  v_tag_map[v_index] = new_tag;
199  break;
200  case 'I':
201  new_tag = iset->second.load_invalid_map[v_tag_map[v_index]];
202  if (!new_tag)
203  {
204  new_tag = _checker->tagIfSetKnown(v_tag_map[v_index], iset->second.var, false);
205  iset->second.load_valid_map[v_tag_map[v_index]] = new_tag;
206  }
207  v_tag_map[v_index] = new_tag;
208  break;
209  default:
210  OMPL_ERROR("Unknown character: %c", buffer[i]);
211  }
212  }
213  }
214  if (v_index<v_to) // premature break
215  continue;
216  // read edge start
217  bytes_read = fread(buffer, 1, 7, iset->second.fp);
218  if (bytes_read != 7 || strncmp(buffer,"\nedges ",7)!=0)
219  {
220  OMPL_ERROR("Edge mismatch from file.");
221  continue;
222  }
223  // read edges loop
224  size_t e_index;
225  for (e_index=e_from; e_index<e_to;)
226  {
227  size_t bytes = sizeof(buffer);
228  if (bytes > e_to - e_index)
229  bytes = e_to - e_index;
230  bytes_read = fread(buffer, 1, bytes, iset->second.fp);
231  if (bytes_read != bytes)
232  {
233  OMPL_ERROR("Edge mismatch from file.");
234  break;
235  }
236  for (size_t i=0; i<bytes; i++,e_index++)
237  {
238  size_t new_tag;
239  switch (buffer[i])
240  {
241  case 'U': break;
242  case 'V':
243  new_tag = iset->second.load_valid_map[e_tag_map[e_index]];
244  if (!new_tag)
245  {
246  new_tag = _checker->tagIfSetKnown(e_tag_map[e_index], iset->second.var, true);
247  iset->second.load_valid_map[e_tag_map[e_index]] = new_tag;
248  }
249  e_tag_map[e_index] = new_tag;
250  break;
251  case 'I':
252  new_tag = iset->second.load_invalid_map[e_tag_map[e_index]];
253  if (!new_tag)
254  {
255  new_tag = _checker->tagIfSetKnown(e_tag_map[e_index], iset->second.var, false);
256  iset->second.load_valid_map[e_tag_map[e_index]] = new_tag;
257  }
258  e_tag_map[e_index] = new_tag;
259  break;
260  default:
261  OMPL_ERROR("Unknown character: %c", buffer[i]);
262  }
263  }
264  }
265  if (e_index<e_to) // premature break
266  continue;
267  // read edge end
268  bytes_read = fread(buffer, 1, 1, iset->second.fp);
269  if (bytes_read != 1 || strncmp(buffer,"\n ",1)!=0)
270  {
271  OMPL_ERROR("Edge mismatch from file.");
272  continue;
273  }
274  }
275  }
276 
277  void loadEnd()
278  {
279  for (typename std::map<size_t, CachedSet>::iterator
280  iset=_cached_sets.begin(); iset!=_cached_sets.end(); iset++)
281  {
282  if (iset->second.fp)
283  fclose(iset->second.fp);
284  }
285  }
286 
287  void saveBegin()
288  {
289  for (typename std::map<size_t, CachedSet>::iterator
290  iset=_cached_sets.begin(); iset!=_cached_sets.end(); iset++)
291  {
292  const char * filename = iset->second.filename.c_str();
293  iset->second.fp = fopen(filename, "w");
294  if (!iset->second.fp)
295  {
296  OMPL_ERROR("Could not save to file \"%s\".", filename);
297  continue;
298  }
299  // write header
300  fprintf(iset->second.fp, "%s", iset->second.header.c_str());
301  }
302  }
303 
304  void saveBatch(size_t batch,
305  VTagMap v_tag_map, size_t v_from, size_t v_to,
306  ETagMap e_tag_map, size_t e_from, size_t e_to)
307  {
308  printf("new save not implemented!\n");
309 #if 0
310  for (typename std::map<size_t, CachedSet>::iterator
311  iset=_cached_sets.begin(); iset!=_cached_sets.end(); iset++)
312  {
313  if (!iset->second.fp)
314  continue;
315  // batch header
316  fprintf(iset->second.fp, "batch %lu num_vertices %lu num_edges %lu\n",
317  batch, v_to - v_from, e_to - e_from);
318  // save vertices
319  fprintf(iset->second.fp, "vertices ");
320  for (size_t v_index=v_from; v_index<v_to; v_index++)
321  {
322  size_t & tag = v_tag_map[v_index];
323  fprintf(iset->second.fp, "%c", iset->second.save_map[tag]);
324  }
325  fprintf(iset->second.fp, "\n");
326  // save edges
327  fprintf(iset->second.fp, "edges ");
328  for (size_t e_index=e_from; e_index<e_to; e_index++)
329  {
330  size_t & tag = e_tag_map[e_index];
331  fprintf(iset->second.fp, "%c", iset->second.save_map[tag]);
332  }
333  fprintf(iset->second.fp, "\n");
334  }
335 #endif
336  }
337 
338  void saveEnd()
339  {
340  for (typename std::map<size_t, CachedSet>::iterator
341  iset=_cached_sets.begin(); iset!=_cached_sets.end(); iset++)
342  {
343  if (iset->second.fp)
344  fclose(iset->second.fp);
345  }
346  }
347 };
348 
349 } // namespace ompl_lemur
Definition: TagCache.h:12
#define OMPL_ERROR(fmt,...)
Tag cache which uses a family checker to save/load particular sets.
Definition: FamilyTagCache.h:41
Definition: FamilyTagCache.h:51
#define OMPL_INFORM(fmt,...)