MRPT  1.9.9
CRawlog.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
15 #include <mrpt/obs/CRawlog.h>
17 #include <mrpt/system/filesystem.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::io;
21 using namespace mrpt::obs;
22 using namespace mrpt::poses;
23 using namespace mrpt::system;
24 using namespace mrpt::serialization;
25 
27 
28 void CRawlog::clear()
29 {
30  m_seqOfActObs.clear();
31  m_commentTexts.text.clear();
32 }
33 
34 void CRawlog::insert(CSensoryFrame& observations)
35 {
36  m_seqOfActObs.push_back(std::dynamic_pointer_cast<CSerializable>(
37  observations.duplicateGetSmartPtr()));
38 }
39 
40 void CRawlog::insert(CActionCollection& actions)
41 {
42  m_seqOfActObs.push_back(std::dynamic_pointer_cast<CSerializable>(
43  actions.duplicateGetSmartPtr()));
44 }
45 void CRawlog::insert(const CSerializable::Ptr& obj)
46 {
47  if (IS_CLASS(*obj, CObservationComment))
48  {
50  std::dynamic_pointer_cast<CObservationComment>(obj);
51  m_commentTexts = *o;
52  }
53  else
54  m_seqOfActObs.push_back(obj);
55 }
56 
57 void CRawlog::insert(CAction& action)
58 {
59  CActionCollection::Ptr temp = std::make_shared<CActionCollection>();
60  temp->insert(action);
61  m_seqOfActObs.push_back(temp);
62 }
63 
64 size_t CRawlog::size() const { return m_seqOfActObs.size(); }
65 CActionCollection::Ptr CRawlog::getAsAction(size_t index) const
66 {
68 
69  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
70 
71  CSerializable::Ptr obj = m_seqOfActObs[index];
72 
73  if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
74  return std::dynamic_pointer_cast<CActionCollection>(obj);
75  else
77  "Element at index %i is not a CActionCollection", (int)index);
78  MRPT_END
79 }
80 
81 CObservation::Ptr CRawlog::getAsObservation(size_t index) const
82 {
84 
85  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
86 
87  CSerializable::Ptr obj = m_seqOfActObs[index];
88 
89  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
90  return std::dynamic_pointer_cast<CObservation>(obj);
91  else
93  "Element at index %i is not a CObservation", (int)index);
94  MRPT_END
95 }
96 
97 CSerializable::Ptr CRawlog::getAsGeneric(size_t index) const
98 {
100  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
101 
102  return m_seqOfActObs[index];
103  MRPT_END
104 }
105 
106 CRawlog::TEntryType CRawlog::getType(size_t index) const
107 {
108  MRPT_START
109  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
110 
111  const CSerializable::Ptr& obj = m_seqOfActObs[index];
112 
113  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
114  return etObservation;
115  else if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
116  return etActionCollection;
117  else if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
118  return etSensoryFrame;
119  else
120  return etOther;
121 
122  MRPT_END
123 }
124 
125 CSensoryFrame::Ptr CRawlog::getAsObservations(size_t index) const
126 {
127  MRPT_START
128  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
129 
130  CSerializable::Ptr obj = m_seqOfActObs[index];
131 
132  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CSensoryFrame)))
133  return std::dynamic_pointer_cast<CSensoryFrame>(obj);
134  else
136  "Element at index %i is not a CSensoryFrame", (int)index);
137  MRPT_END
138 }
139 
140 uint8_t CRawlog::serializeGetVersion() const { return 1; }
141 void CRawlog::serializeTo(mrpt::serialization::CArchive& out) const
142 {
143  out.WriteAs<uint32_t>(m_seqOfActObs.size());
144  for (const auto& a : m_seqOfActObs) out << a;
145  out << m_commentTexts;
146 }
147 
148 void CRawlog::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
149 {
150  switch (version)
151  {
152  case 0:
153  case 1:
154  {
155  clear();
156  m_seqOfActObs.resize(in.ReadAs<uint32_t>());
157  for (auto& a : m_seqOfActObs) a = in.ReadObject();
158  in >> m_commentTexts;
159  }
160  break;
161  default:
163  };
164 }
165 
166 bool CRawlog::loadFromRawLogFile(
167  const std::string& fileName, bool non_obs_objects_are_legal)
168 {
169  // Open for read.
171  if (!fi.open(fileName)) return false;
172  auto fs = archiveFrom(fi);
173 
174  clear(); // Clear first
175 
176  // OK: read objects:
177  bool keepReading = true;
178  while (keepReading)
179  {
180  CSerializable::Ptr newObj;
181  try
182  {
183  fs >> newObj;
184  bool add_obj = false;
185  // Check type:
186  if (newObj->GetRuntimeClass() == CLASS_ID(CRawlog))
187  {
188  // It is an entire object: Copy and finish:
189  CRawlog::Ptr ao = std::dynamic_pointer_cast<CRawlog>(newObj);
190  this->swap(*ao);
191  return true;
192  }
193  else if (newObj->GetRuntimeClass()->derivedFrom(
195  {
196  if (IS_CLASS(*newObj, CObservationComment))
197  {
199  std::dynamic_pointer_cast<CObservationComment>(newObj);
200  m_commentTexts = *o;
201  }
202  else
203  {
204  add_obj = true;
205  }
206  }
207  else if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
208  {
209  add_obj = true;
210  }
211  else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
212  {
213  add_obj = true;
214  }
215  else
216  {
217  // Other classes:
218  if (non_obs_objects_are_legal)
219  {
220  add_obj = true;
221  }
222  else
223  {
224  keepReading = false;
225  }
226  }
227  if (add_obj) m_seqOfActObs.push_back(newObj);
228  }
229  catch (CExceptionEOF&)
230  { // EOF, just finish the loop
231  keepReading = false;
232  }
233  catch (const std::exception& e)
234  {
235  std::cerr << mrpt::exception_to_str(e) << std::endl;
236  keepReading = false;
237  }
238  catch (...)
239  {
240  keepReading = false;
241  }
242  }
243  return true;
244 }
245 
246 void CRawlog::remove(size_t index)
247 {
248  MRPT_START
249  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
250  m_seqOfActObs.erase(m_seqOfActObs.begin() + index);
251  MRPT_END
252 }
253 
254 void CRawlog::remove(size_t first_index, size_t last_index)
255 {
256  MRPT_START
257  if (first_index >= m_seqOfActObs.size() ||
258  last_index >= m_seqOfActObs.size())
259  THROW_EXCEPTION("Index out of bounds");
260  m_seqOfActObs.erase(
261  m_seqOfActObs.begin() + first_index,
262  m_seqOfActObs.begin() + last_index + 1);
263  MRPT_END
264 }
265 
266 bool CRawlog::saveToRawLogFile(const std::string& fileName) const
267 {
268  try
269  {
270  CFileGZOutputStream fo(fileName);
271  auto f = archiveFrom(fo);
272  if (!m_commentTexts.text.empty()) f << m_commentTexts;
273  for (const auto& m_seqOfActOb : m_seqOfActObs) f << *m_seqOfActOb;
274  return true;
275  }
276  catch (const std::exception& e)
277  {
278  std::cerr << mrpt::exception_to_str(e) << std::endl;
279  return false;
280  }
281 }
282 
283 void CRawlog::swap(CRawlog& obj)
284 {
285  if (this == &obj) return;
286  m_seqOfActObs.swap(obj.m_seqOfActObs);
287  std::swap(m_commentTexts, obj.m_commentTexts);
288 }
289 
290 bool CRawlog::readActionObservationPair(
291  CArchive& inStream, CActionCollection::Ptr& action,
292  CSensoryFrame::Ptr& observations, size_t& rawlogEntry)
293 {
294  try
295  {
296  // Load pose change from the rawlog:
297  action.reset();
298  while (!action)
299  {
300  CSerializable::Ptr obj;
301  inStream >> obj;
302  if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
303  {
304  action = std::dynamic_pointer_cast<CActionCollection>(obj);
305  }
306  else
307  {
308  obj.reset();
309  }
310  rawlogEntry++;
311  };
312 
313  // Load sensory frame from the rawlog:
314  observations.reset();
315  while (!observations)
316  {
317  CSerializable::Ptr obj;
318  inStream >> obj;
319  if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
320  {
321  observations = std::dynamic_pointer_cast<CSensoryFrame>(obj);
322  }
323  else
324  {
325  obj.reset();
326  }
327  rawlogEntry++;
328  }
329  return true;
330  }
331  catch (CExceptionEOF&)
332  {
333  return false;
334  }
335  catch (const std::exception& e)
336  {
337  std::cerr << "[CRawlog::readActionObservationPair] Found exception:"
338  << std::endl
339  << mrpt::exception_to_str(e) << std::endl;
340  return false;
341  }
342  catch (...)
343  {
344  std::cerr << "Untyped exception reading rawlog file!!" << std::endl;
345  return false;
346  }
347 }
348 
349 bool CRawlog::getActionObservationPairOrObservation(
350  CArchive& inStream, CActionCollection::Ptr& action,
351  CSensoryFrame::Ptr& observations, CObservation::Ptr& observation,
352  size_t& rawlogEntry)
353 {
354  try
355  {
356  // Load pose change from the rawlog:
357  observations.reset();
358  observation.reset();
359  action.reset();
360  while (!action)
361  {
362  CSerializable::Ptr obj;
363  inStream >> obj;
364  if (IS_CLASS(*obj, CActionCollection))
365  {
366  action = std::dynamic_pointer_cast<CActionCollection>(obj);
367  }
368  else if (IS_DERIVED(*obj, CObservation))
369  {
370  observation = std::dynamic_pointer_cast<CObservation>(obj);
371  rawlogEntry++;
372  return true;
373  }
374  else
375  obj.reset();
376  rawlogEntry++;
377  };
378 
379  // Load sensory frame from the rawlog:
380  observations.reset();
381  while (!observations)
382  {
383  CSerializable::Ptr obj;
384  inStream >> obj;
385  if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
386  {
387  observations = std::dynamic_pointer_cast<CSensoryFrame>(obj);
388  }
389  rawlogEntry++;
390  }
391  return true;
392  }
393  catch (CExceptionEOF&)
394  {
395  return false;
396  }
397  catch (const std::exception& e)
398  {
399  std::cerr << "[CRawlog::readActionObservationPair] Found exception:"
400  << std::endl
401  << mrpt::exception_to_str(e) << std::endl;
402  return false;
403  }
404  catch (...)
405  {
406  std::cerr << "Untyped exception reading rawlog file!!" << std::endl;
407  return false;
408  }
409 }
410 
411 void CRawlog::findObservationsByClassInRange(
413  const mrpt::rtti::TRuntimeClassId* class_type,
414  TListTimeAndObservations& out_found,
415  [[maybe_unused]] size_t guess_start_position) const
416 {
417  MRPT_START
418 
419  out_found.clear();
420 
421  if (m_seqOfActObs.empty()) return;
422 
423  // Find the first appearance of time_start:
424  // ---------------------------------------------------
425  auto first = m_seqOfActObs.begin();
426  const auto last = m_seqOfActObs.end();
427  {
428  // The following is based on lower_bound:
429  size_t count, step;
430  count = std::distance(first, last);
431  while (count > 0)
432  {
433  auto it = first;
434  step = count / 2;
435  std::advance(it, step);
436 
437  // The comparison function:
438  TTimeStamp this_timestamp;
439  if ((*it)->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
440  {
442  std::dynamic_pointer_cast<CObservation>(*it);
443  this_timestamp = o->timestamp;
444  ASSERT_(this_timestamp != INVALID_TIMESTAMP);
445  }
446  else
448  "Element found which is not derived from CObservation");
449 
450  if (this_timestamp < time_start) // *it < time_start
451  {
452  first = ++it;
453  count -= step + 1;
454  }
455  else
456  count = step;
457  }
458  // "first" is our guy
459  }
460 
461  // Iterate until we get out of the time window:
462  while (first != last)
463  {
464  TTimeStamp this_timestamp;
465  if ((*first)->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
466  {
468  std::dynamic_pointer_cast<CObservation>(*first);
469  this_timestamp = o->timestamp;
470  ASSERT_(this_timestamp != INVALID_TIMESTAMP);
471 
472  if (this_timestamp < time_end)
473  {
474  if (o->GetRuntimeClass()->derivedFrom(class_type))
475  out_found.insert(TTimeObservationPair(this_timestamp, o));
476  }
477  else
478  {
479  break; // end of time window!
480  }
481  }
482  else
484  "Element found which is not derived from CObservation");
485 
486  first++;
487  }
488 
489  MRPT_END
490 }
491 
492 bool CRawlog::getActionObservationPair(
493  CActionCollection::Ptr& action, CSensoryFrame::Ptr& observations,
494  size_t& rawlogEntry) const
495 {
496  try
497  {
498  while (getType(rawlogEntry) != CRawlog::etActionCollection)
499  {
500  rawlogEntry++;
501  }
502  action = getAsAction(rawlogEntry++);
503 
504  while (getType(rawlogEntry) != CRawlog::etSensoryFrame)
505  {
506  rawlogEntry++;
507  }
508  observations = getAsObservations(rawlogEntry++);
509 
510  return true;
511  }
512  catch (const std::exception& e)
513  {
514  std::cerr << mrpt::exception_to_str(e) << std::endl;
515  return false;
516  }
517  catch (...)
518  {
519  std::cerr << "Untyped exception getting act-obs pair from rawlog!!"
520  << std::endl;
521  return false;
522  }
523 }
524 
525 void CRawlog::getCommentText(std::string& t) const { t = m_commentTexts.text; }
526 std::string CRawlog::getCommentText() const { return m_commentTexts.text; }
527 void CRawlog::getCommentTextAsConfigFile(
528  mrpt::config::CConfigFileMemory& memCfg) const
529 {
530  memCfg.setContent(m_commentTexts.text);
531 }
532 
533 void CRawlog::setCommentText(const std::string& t) { m_commentTexts.text = t; }
534 std::string CRawlog::detectImagesDirectory(const std::string& str)
535 {
536  const std::string rawlog_path = extractFileDirectory(str);
537  std::string temptative_img_path =
538  rawlog_path + extractFileName(str) + std::string("_Images");
539  if (mrpt::system::fileExists(temptative_img_path))
540  return temptative_img_path;
541  else if (mrpt::system::fileExists(
542  temptative_img_path =
543  (rawlog_path + extractFileName(str) +
544  std::string("_images"))))
545  return temptative_img_path;
546  else if (mrpt::system::fileExists(
547  temptative_img_path =
548  (rawlog_path + extractFileName(str) +
549  std::string("_IMAGES"))))
550  return temptative_img_path;
551  else
552  return rawlog_path + "Images";
553 }
This class implements a config file-like interface over a memory-stored string list.
This "observation" is actually a placeholder for a text block with comments or additional parameters ...
#define MRPT_START
Definition: exceptions.h:241
TListObjects m_seqOfActObs
The list where the objects really are in.
Definition: CRawlog.h:72
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
size_t size(const MATRIXLIKE &m, const int dim)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
A structure that holds runtime class type information.
Definition: CObject.h:31
Declares a class for storing a collection of robot actions.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
bool open(const std::string &fileName, mrpt::optional_ref< std::string > error_msg=std::nullopt)
Opens the file for read.
std::pair< mrpt::system::TTimeStamp, CObservation::Ptr > TTimeObservationPair
For usage with CRawlog classes.
Definition: CRawlog.h:21
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
std::multimap< mrpt::system::TTimeStamp, CObservation::Ptr > TListTimeAndObservations
For usage with CRawlog classes.
Definition: CRawlog.h:24
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
Definition: CObject.h:151
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:65
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Declares a class for storing a robot action.
Definition: CAction.h:24
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void setContent(const std::vector< std::string > &stringList)
Changes the contents of the virtual "config file".
TEntryType
The type of each entry in a rawlog.
Definition: CRawlog.h:95
#define MRPT_END
Definition: exceptions.h:245
CSerializable::Ptr ReadObject()
Reads an object from stream, its class determined at runtime, and returns a smart pointer to the obje...
Definition: CArchive.h:178
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Saves data to a file and transparently compress the data using the given compression level...
std::string extractFileName(const std::string &filePath)
Extract just the name (without extension) of a filename from a complete path plus name plus extension...
Definition: filesystem.cpp:62
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
CObservationComment m_commentTexts
Comments of the rawlog.
Definition: CRawlog.h:75
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
std::string extractFileDirectory(const std::string &filePath)
Extract the whole path (the directory) of a filename from a complete path plus name plus extension...
Definition: filesystem.cpp:78
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1871
mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const
Makes a deep copy of the object and returns a smart pointer to it.
Definition: CObject.h:204
Used in mrpt::serialization::CArchive.
Definition: CArchive.h:36



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: b2c5333a7 Sat Mar 28 09:52:26 2020 +0100 at sáb mar 28 10:00:10 CET 2020