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-2019, 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 // ctor
29 CRawlog::CRawlog() : m_seqOfActObs(), m_commentTexts() {}
30 // dtor
31 CRawlog::~CRawlog() { clear(); }
33 {
34  m_seqOfActObs.clear();
35  m_commentTexts.text.clear();
36 }
37 
38 void CRawlog::addObservations(CSensoryFrame& observations)
39 {
40  m_seqOfActObs.push_back(std::dynamic_pointer_cast<CSerializable>(
41  observations.duplicateGetSmartPtr()));
42 }
43 
44 void CRawlog::addActions(CActionCollection& actions)
45 {
46  m_seqOfActObs.push_back(std::dynamic_pointer_cast<CSerializable>(
47  actions.duplicateGetSmartPtr()));
48 }
49 
50 void CRawlog::addActionsMemoryReference(const CActionCollection::Ptr& action)
51 {
52  m_seqOfActObs.push_back(action);
53 }
54 
55 void CRawlog::addObservationsMemoryReference(
56  const CSensoryFrame::Ptr& observations)
57 {
58  m_seqOfActObs.push_back(observations);
59 }
60 void CRawlog::addGenericObject(const CSerializable::Ptr& obj)
61 {
62  m_seqOfActObs.push_back(obj);
63 }
64 
65 void CRawlog::addObservationMemoryReference(
66  const CObservation::Ptr& observation)
67 {
68  if (IS_CLASS(*observation, CObservationComment))
69  {
71  std::dynamic_pointer_cast<CObservationComment>(observation);
72  m_commentTexts = *o;
73  }
74  else
75  m_seqOfActObs.push_back(observation);
76 }
77 
78 void CRawlog::addAction(CAction& action)
79 {
80  CActionCollection::Ptr temp = std::make_shared<CActionCollection>();
81  temp->insert(action);
82  m_seqOfActObs.push_back(temp);
83 }
84 
85 size_t CRawlog::size() const { return m_seqOfActObs.size(); }
86 CActionCollection::Ptr CRawlog::getAsAction(size_t index) const
87 {
89 
90  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
91 
92  CSerializable::Ptr obj = m_seqOfActObs[index];
93 
94  if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
95  return std::dynamic_pointer_cast<CActionCollection>(obj);
96  else
98  "Element at index %i is not a CActionCollection", (int)index);
99  MRPT_END
100 }
101 
102 CObservation::Ptr CRawlog::getAsObservation(size_t index) const
103 {
104  MRPT_START
105 
106  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
107 
108  CSerializable::Ptr obj = m_seqOfActObs[index];
109 
110  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
111  return std::dynamic_pointer_cast<CObservation>(obj);
112  else
114  "Element at index %i is not a CObservation", (int)index);
115  MRPT_END
116 }
117 
118 CSerializable::Ptr CRawlog::getAsGeneric(size_t index) const
119 {
120  MRPT_START
121  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
122 
123  return m_seqOfActObs[index];
124  MRPT_END
125 }
126 
127 CRawlog::TEntryType CRawlog::getType(size_t index) const
128 {
129  MRPT_START
130  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
131 
132  const CSerializable::Ptr& obj = m_seqOfActObs[index];
133 
134  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
135  return etObservation;
136  else if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
137  return etActionCollection;
138  else if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
139  return etSensoryFrame;
140  else
141  return etOther;
142 
143  MRPT_END
144 }
145 
146 CSensoryFrame::Ptr CRawlog::getAsObservations(size_t index) const
147 {
148  MRPT_START
149  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
150 
151  CSerializable::Ptr obj = m_seqOfActObs[index];
152 
153  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CSensoryFrame)))
154  return std::dynamic_pointer_cast<CSensoryFrame>(obj);
155  else
157  "Element at index %i is not a CSensoryFrame", (int)index);
158  MRPT_END
159 }
160 
161 uint8_t CRawlog::serializeGetVersion() const { return 1; }
162 void CRawlog::serializeTo(mrpt::serialization::CArchive& out) const
163 {
164  out.WriteAs<uint32_t>(m_seqOfActObs.size());
165  for (const auto& a : m_seqOfActObs) out << a;
166  out << m_commentTexts;
167 }
168 
169 void CRawlog::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
170 {
171  switch (version)
172  {
173  case 0:
174  case 1:
175  {
176  clear();
177  m_seqOfActObs.resize(in.ReadAs<uint32_t>());
178  for (auto& a : m_seqOfActObs) a = in.ReadObject();
179  in >> m_commentTexts;
180  }
181  break;
182  default:
184  };
185 }
186 
187 bool CRawlog::loadFromRawLogFile(
188  const std::string& fileName, bool non_obs_objects_are_legal)
189 {
190  // Open for read.
192  if (!fi.open(fileName)) return false;
193  auto fs = archiveFrom(fi);
194 
195  clear(); // Clear first
196 
197  // OK: read objects:
198  bool keepReading = true;
199  while (keepReading)
200  {
201  CSerializable::Ptr newObj;
202  try
203  {
204  fs >> newObj;
205  bool add_obj = false;
206  // Check type:
207  if (newObj->GetRuntimeClass() == CLASS_ID(CRawlog))
208  {
209  // It is an entire object: Copy and finish:
210  CRawlog::Ptr ao = std::dynamic_pointer_cast<CRawlog>(newObj);
211  this->swap(*ao);
212  return true;
213  }
214  else if (newObj->GetRuntimeClass()->derivedFrom(
216  {
217  if (IS_CLASS(*newObj, CObservationComment))
218  {
220  std::dynamic_pointer_cast<CObservationComment>(newObj);
221  m_commentTexts = *o;
222  }
223  else
224  {
225  add_obj = true;
226  }
227  }
228  else if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
229  {
230  add_obj = true;
231  }
232  else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
233  {
234  add_obj = true;
235  }
236  else
237  {
238  // Other classes:
239  if (non_obs_objects_are_legal)
240  {
241  add_obj = true;
242  }
243  else
244  {
245  keepReading = false;
246  }
247  }
248  if (add_obj) m_seqOfActObs.push_back(newObj);
249  }
250  catch (CExceptionEOF&)
251  { // EOF, just finish the loop
252  keepReading = false;
253  }
254  catch (const std::exception& e)
255  {
256  std::cerr << mrpt::exception_to_str(e) << std::endl;
257  keepReading = false;
258  }
259  catch (...)
260  {
261  keepReading = false;
262  }
263  }
264  return true;
265 }
266 
267 void CRawlog::remove(size_t index)
268 {
269  MRPT_START
270  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
271  m_seqOfActObs.erase(m_seqOfActObs.begin() + index);
272  MRPT_END
273 }
274 
275 void CRawlog::remove(size_t first_index, size_t last_index)
276 {
277  MRPT_START
278  if (first_index >= m_seqOfActObs.size() ||
279  last_index >= m_seqOfActObs.size())
280  THROW_EXCEPTION("Index out of bounds");
281  m_seqOfActObs.erase(
282  m_seqOfActObs.begin() + first_index,
283  m_seqOfActObs.begin() + last_index + 1);
284  MRPT_END
285 }
286 
287 bool CRawlog::saveToRawLogFile(const std::string& fileName) const
288 {
289  try
290  {
291  CFileGZOutputStream fo(fileName);
292  auto f = archiveFrom(fo);
293  if (!m_commentTexts.text.empty()) f << m_commentTexts;
294  for (const auto& m_seqOfActOb : m_seqOfActObs) f << *m_seqOfActOb;
295  return true;
296  }
297  catch (const std::exception& e)
298  {
299  std::cerr << mrpt::exception_to_str(e) << std::endl;
300  return false;
301  }
302 }
303 
304 void CRawlog::swap(CRawlog& obj)
305 {
306  if (this == &obj) return;
307  m_seqOfActObs.swap(obj.m_seqOfActObs);
308  std::swap(m_commentTexts, obj.m_commentTexts);
309 }
310 
311 bool CRawlog::readActionObservationPair(
312  CArchive& inStream, CActionCollection::Ptr& action,
313  CSensoryFrame::Ptr& observations, size_t& rawlogEntry)
314 {
315  try
316  {
317  // Load pose change from the rawlog:
318  action.reset();
319  while (!action)
320  {
322  inStream >> obj;
323  if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
324  {
325  action = std::dynamic_pointer_cast<CActionCollection>(obj);
326  }
327  else
328  {
329  obj.reset();
330  }
331  rawlogEntry++;
332  };
333 
334  // Load sensory frame from the rawlog:
335  observations.reset();
336  while (!observations)
337  {
339  inStream >> obj;
340  if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
341  {
342  observations = std::dynamic_pointer_cast<CSensoryFrame>(obj);
343  }
344  else
345  {
346  obj.reset();
347  }
348  rawlogEntry++;
349  }
350  return true;
351  }
352  catch (CExceptionEOF&)
353  {
354  return false;
355  }
356  catch (const std::exception& e)
357  {
358  std::cerr << "[CRawlog::readActionObservationPair] Found exception:"
359  << std::endl
360  << mrpt::exception_to_str(e) << std::endl;
361  return false;
362  }
363  catch (...)
364  {
365  std::cerr << "Untyped exception reading rawlog file!!" << std::endl;
366  return false;
367  }
368 }
369 
370 bool CRawlog::getActionObservationPairOrObservation(
371  CArchive& inStream, CActionCollection::Ptr& action,
372  CSensoryFrame::Ptr& observations, CObservation::Ptr& observation,
373  size_t& rawlogEntry)
374 {
375  try
376  {
377  // Load pose change from the rawlog:
378  observations.reset();
379  observation.reset();
380  action.reset();
381  while (!action)
382  {
384  inStream >> obj;
386  {
387  action = std::dynamic_pointer_cast<CActionCollection>(obj);
388  }
389  else if (IS_DERIVED(*obj, CObservation))
390  {
391  observation = std::dynamic_pointer_cast<CObservation>(obj);
392  rawlogEntry++;
393  return true;
394  }
395  else
396  obj.reset();
397  rawlogEntry++;
398  };
399 
400  // Load sensory frame from the rawlog:
401  observations.reset();
402  while (!observations)
403  {
405  inStream >> obj;
406  if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
407  {
408  observations = std::dynamic_pointer_cast<CSensoryFrame>(obj);
409  }
410  rawlogEntry++;
411  }
412  return true;
413  }
414  catch (CExceptionEOF&)
415  {
416  return false;
417  }
418  catch (const std::exception& e)
419  {
420  std::cerr << "[CRawlog::readActionObservationPair] Found exception:"
421  << std::endl
422  << mrpt::exception_to_str(e) << std::endl;
423  return false;
424  }
425  catch (...)
426  {
427  std::cerr << "Untyped exception reading rawlog file!!" << std::endl;
428  return false;
429  }
430 }
431 
432 void CRawlog::findObservationsByClassInRange(
434  const mrpt::rtti::TRuntimeClassId* class_type,
435  TListTimeAndObservations& out_found, size_t guess_start_position) const
436 {
437  MRPT_UNUSED_PARAM(guess_start_position);
438  MRPT_START
439 
440  out_found.clear();
441 
442  if (m_seqOfActObs.empty()) return;
443 
444  // Find the first appearance of time_start:
445  // ---------------------------------------------------
446  auto first = m_seqOfActObs.begin();
447  const auto last = m_seqOfActObs.end();
448  {
449  // The following is based on lower_bound:
450  size_t count, step;
451  count = std::distance(first, last);
452  while (count > 0)
453  {
454  auto it = first;
455  step = count / 2;
456  std::advance(it, step);
457 
458  // The comparison function:
459  TTimeStamp this_timestamp;
460  if ((*it)->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
461  {
463  std::dynamic_pointer_cast<CObservation>(*it);
464  this_timestamp = o->timestamp;
465  ASSERT_(this_timestamp != INVALID_TIMESTAMP);
466  }
467  else
469  "Element found which is not derived from CObservation");
470 
471  if (this_timestamp < time_start) // *it < time_start
472  {
473  first = ++it;
474  count -= step + 1;
475  }
476  else
477  count = step;
478  }
479  // "first" is our guy
480  }
481 
482  // Iterate until we get out of the time window:
483  while (first != last)
484  {
485  TTimeStamp this_timestamp;
486  if ((*first)->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
487  {
489  std::dynamic_pointer_cast<CObservation>(*first);
490  this_timestamp = o->timestamp;
491  ASSERT_(this_timestamp != INVALID_TIMESTAMP);
492 
493  if (this_timestamp < time_end)
494  {
495  if (o->GetRuntimeClass()->derivedFrom(class_type))
496  out_found.insert(TTimeObservationPair(this_timestamp, o));
497  }
498  else
499  {
500  break; // end of time window!
501  }
502  }
503  else
505  "Element found which is not derived from CObservation");
506 
507  first++;
508  }
509 
510  MRPT_END
511 }
512 
513 bool CRawlog::getActionObservationPair(
514  CActionCollection::Ptr& action, CSensoryFrame::Ptr& observations,
515  size_t& rawlogEntry) const
516 {
517  try
518  {
519  while (getType(rawlogEntry) != CRawlog::etActionCollection)
520  {
521  rawlogEntry++;
522  }
523  action = getAsAction(rawlogEntry++);
524 
525  while (getType(rawlogEntry) != CRawlog::etSensoryFrame)
526  {
527  rawlogEntry++;
528  }
529  observations = getAsObservations(rawlogEntry++);
530 
531  return true;
532  }
533  catch (const std::exception& e)
534  {
535  std::cerr << mrpt::exception_to_str(e) << std::endl;
536  return false;
537  }
538  catch (...)
539  {
540  std::cerr << "Untyped exception getting act-obs pair from rawlog!!"
541  << std::endl;
542  return false;
543  }
544 }
545 
546 void CRawlog::getCommentText(std::string& t) const { t = m_commentTexts.text; }
547 std::string CRawlog::getCommentText() const { return m_commentTexts.text; }
548 void CRawlog::getCommentTextAsConfigFile(
549  mrpt::config::CConfigFileMemory& memCfg) const
550 {
551  memCfg.setContent(m_commentTexts.text);
552 }
553 
554 void CRawlog::setCommentText(const std::string& t) { m_commentTexts.text = t; }
555 std::string CRawlog::detectImagesDirectory(const std::string& str)
556 {
557  const std::string rawlog_path = extractFileDirectory(str);
558  std::string temptative_img_path =
559  rawlog_path + extractFileName(str) + std::string("_Images");
560  if (mrpt::system::fileExists(temptative_img_path))
561  return temptative_img_path;
562  else if (mrpt::system::fileExists(
563  temptative_img_path =
564  (rawlog_path + extractFileName(str) +
565  std::string("_images"))))
566  return temptative_img_path;
567  else if (mrpt::system::fileExists(
568  temptative_img_path =
569  (rawlog_path + extractFileName(str) +
570  std::string("_IMAGES"))))
571  return temptative_img_path;
572  else
573  return rawlog_path + "Images";
574 }
This class implements a config file-like interface over a memory-stored string list.
GLuint GLuint GLsizei count
Definition: glext.h:3532
This "observation" is actually a placeholder for a text block with comments or additional parameters ...
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble t
Definition: glext.h:3695
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLint * first
Definition: glext.h:3833
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
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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:591
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
GLuint index
Definition: glext.h:4068
#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
GLsizei const GLchar ** string
Definition: glext.h:4116
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:92
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
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.
GLsizeiptr size
Definition: glext.h:3934
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
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
#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:1891
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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019