MRPT  1.9.9
CRawlog.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/filesystem.h>
13 #include <mrpt/obs/CRawlog.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(
41  std::dynamic_pointer_cast<CSerializable>(
42  observations.duplicateGetSmartPtr()));
43 }
44 
45 void CRawlog::addActions(CActionCollection& actions)
46 {
47  m_seqOfActObs.push_back(
48  std::dynamic_pointer_cast<CSerializable>(
49  actions.duplicateGetSmartPtr()));
50 }
51 
52 void CRawlog::addActionsMemoryReference(const CActionCollection::Ptr& action)
53 {
54  m_seqOfActObs.push_back(action);
55 }
56 
57 void CRawlog::addObservationsMemoryReference(
58  const CSensoryFrame::Ptr& observations)
59 {
60  m_seqOfActObs.push_back(observations);
61 }
62 void CRawlog::addGenericObject(const CSerializable::Ptr& obj)
63 {
64  m_seqOfActObs.push_back(obj);
65 }
66 
67 void CRawlog::addObservationMemoryReference(
68  const CObservation::Ptr& observation)
69 {
70  if (IS_CLASS(observation, CObservationComment))
71  {
73  std::dynamic_pointer_cast<CObservationComment>(observation);
74  m_commentTexts = *o;
75  }
76  else
77  m_seqOfActObs.push_back(observation);
78 }
79 
80 void CRawlog::addAction(CAction& action)
81 {
83  mrpt::make_aligned_shared<CActionCollection>();
84  temp->insert(action);
85  m_seqOfActObs.push_back(temp);
86 }
87 
88 size_t CRawlog::size() const { return m_seqOfActObs.size(); }
89 CActionCollection::Ptr CRawlog::getAsAction(size_t index) const
90 {
92 
93  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
94 
95  CSerializable::Ptr obj = m_seqOfActObs[index];
96 
97  if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
98  return std::dynamic_pointer_cast<CActionCollection>(obj);
99  else
101  "Element at index %i is not a CActionCollection", (int)index);
102  MRPT_END
103 }
104 
105 CObservation::Ptr CRawlog::getAsObservation(size_t index) const
106 {
107  MRPT_START
108 
109  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
110 
111  CSerializable::Ptr obj = m_seqOfActObs[index];
112 
113  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
114  return std::dynamic_pointer_cast<CObservation>(obj);
115  else
117  "Element at index %i is not a CObservation", (int)index);
118  MRPT_END
119 }
120 
121 CSerializable::Ptr CRawlog::getAsGeneric(size_t index) const
122 {
123  MRPT_START
124  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
125 
126  return m_seqOfActObs[index];
127  MRPT_END
128 }
129 
130 CRawlog::TEntryType CRawlog::getType(size_t index) const
131 {
132  MRPT_START
133  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
134 
135  const CSerializable::Ptr& obj = m_seqOfActObs[index];
136 
137  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
138  return etObservation;
139  else if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
140  return etActionCollection;
141  else if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
142  return etSensoryFrame;
143  else
144  return etOther;
145 
146  MRPT_END
147 }
148 
149 CSensoryFrame::Ptr CRawlog::getAsObservations(size_t index) const
150 {
151  MRPT_START
152  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
153 
154  CSerializable::Ptr obj = m_seqOfActObs[index];
155 
156  if (obj->GetRuntimeClass()->derivedFrom(CLASS_ID(CSensoryFrame)))
157  return std::dynamic_pointer_cast<CSensoryFrame>(obj);
158  else
160  "Element at index %i is not a CSensoryFrame", (int)index);
161  MRPT_END
162 }
163 
164 uint8_t CRawlog::serializeGetVersion() const { return 1; }
165 void CRawlog::serializeTo(mrpt::serialization::CArchive& out) const
166 {
167  out.WriteAs<uint32_t>(m_seqOfActObs.size());
168  for (const auto& a : m_seqOfActObs) out << a;
169  out << m_commentTexts;
170 }
171 
172 void CRawlog::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
173 {
174  switch (version)
175  {
176  case 0:
177  case 1:
178  {
179  clear();
180  m_seqOfActObs.resize(in.ReadAs<uint32_t>());
181  for (auto& a : m_seqOfActObs) a = in.ReadObject();
182  in >> m_commentTexts;
183  }
184  break;
185  default:
187  };
188 }
189 
190 bool CRawlog::loadFromRawLogFile(
191  const std::string& fileName, bool non_obs_objects_are_legal)
192 {
193  // Open for read.
194  CFileGZInputStream fi(fileName);
195  if (!fi.fileOpenCorrectly()) return false;
196  auto fs = archiveFrom(fi);
197 
198  clear(); // Clear first
199 
200  // OK: read objects:
201  bool keepReading = true;
202  while (keepReading)
203  {
204  CSerializable::Ptr newObj;
205  try
206  {
207  fs >> newObj;
208  bool add_obj = false;
209  // Check type:
210  if (newObj->GetRuntimeClass() == CLASS_ID(CRawlog))
211  {
212  // It is an entire object: Copy and finish:
213  CRawlog::Ptr ao = std::dynamic_pointer_cast<CRawlog>(newObj);
214  this->swap(*ao);
215  return true;
216  }
217  else if (
218  newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
219  {
220  if (IS_CLASS(newObj, CObservationComment))
221  {
223  std::dynamic_pointer_cast<CObservationComment>(newObj);
224  m_commentTexts = *o;
225  }
226  else
227  {
228  add_obj = true;
229  }
230  }
231  else if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
232  {
233  add_obj = true;
234  }
235  else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
236  {
237  add_obj = true;
238  }
239  else
240  {
241  // Other classes:
242  if (non_obs_objects_are_legal)
243  {
244  add_obj = true;
245  }
246  else
247  {
248  keepReading = false;
249  }
250  }
251  if (add_obj) m_seqOfActObs.push_back(newObj);
252  }
253  catch (CExceptionEOF&)
254  { // EOF, just finish the loop
255  keepReading = false;
256  }
257  catch (std::exception& e)
258  {
259  std::cerr << e.what() << std::endl;
260  keepReading = false;
261  }
262  catch (...)
263  {
264  keepReading = false;
265  }
266  }
267  return true;
268 }
269 
270 void CRawlog::remove(size_t index)
271 {
272  MRPT_START
273  if (index >= m_seqOfActObs.size()) THROW_EXCEPTION("Index out of bounds");
274  m_seqOfActObs.erase(m_seqOfActObs.begin() + index);
275  MRPT_END
276 }
277 
278 void CRawlog::remove(size_t first_index, size_t last_index)
279 {
280  MRPT_START
281  if (first_index >= m_seqOfActObs.size() ||
282  last_index >= m_seqOfActObs.size())
283  THROW_EXCEPTION("Index out of bounds");
284  m_seqOfActObs.erase(
285  m_seqOfActObs.begin() + first_index,
286  m_seqOfActObs.begin() + last_index + 1);
287  MRPT_END
288 }
289 
290 bool CRawlog::saveToRawLogFile(const std::string& fileName) const
291 {
292  try
293  {
294  CFileGZOutputStream fo(fileName);
295  auto f = archiveFrom(fo);
296  if (!m_commentTexts.text.empty()) f << m_commentTexts;
297  for (size_t i = 0; i < m_seqOfActObs.size(); i++)
298  f << *m_seqOfActObs[i];
299  return true;
300  }
301  catch (...)
302  {
303  return false;
304  }
305 }
306 
307 void CRawlog::swap(CRawlog& obj)
308 {
309  if (this == &obj) return;
310  m_seqOfActObs.swap(obj.m_seqOfActObs);
311  std::swap(m_commentTexts, obj.m_commentTexts);
312 }
313 
314 bool CRawlog::readActionObservationPair(
315  CArchive& inStream, CActionCollection::Ptr& action,
316  CSensoryFrame::Ptr& observations, size_t& rawlogEntry)
317 {
318  try
319  {
320  // Load pose change from the rawlog:
321  action.reset();
322  while (!action)
323  {
325  inStream >> obj;
326  if (obj->GetRuntimeClass() == CLASS_ID(CActionCollection))
327  {
328  action = std::dynamic_pointer_cast<CActionCollection>(obj);
329  }
330  else
331  {
332  obj.reset();
333  }
334  rawlogEntry++;
335  };
336 
337  // Load sensory frame from the rawlog:
338  observations.reset();
339  while (!observations)
340  {
342  inStream >> obj;
343  if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
344  {
345  observations = std::dynamic_pointer_cast<CSensoryFrame>(obj);
346  }
347  else
348  {
349  obj.reset();
350  }
351  rawlogEntry++;
352  }
353  return true;
354  }
355  catch (CExceptionEOF&)
356  {
357  return false;
358  }
359  catch (std::exception& e)
360  {
361  std::cerr << "[CRawlog::readActionObservationPair] Found exception:"
362  << std::endl
363  << e.what() << std::endl;
364  return false;
365  }
366  catch (...)
367  {
368  std::cerr << "Untyped exception reading rawlog file!!" << std::endl;
369  return false;
370  }
371 }
372 
373 bool CRawlog::getActionObservationPairOrObservation(
374  CArchive& inStream, CActionCollection::Ptr& action,
375  CSensoryFrame::Ptr& observations, CObservation::Ptr& observation,
376  size_t& rawlogEntry)
377 {
378  try
379  {
380  // Load pose change from the rawlog:
381  observations.reset();
382  observation.reset();
383  action.reset();
384  while (!action)
385  {
387  inStream >> obj;
389  {
390  action = std::dynamic_pointer_cast<CActionCollection>(obj);
391  }
392  else if (IS_DERIVED(obj, CObservation))
393  {
394  observation = std::dynamic_pointer_cast<CObservation>(obj);
395  rawlogEntry++;
396  return true;
397  }
398  else
399  obj.reset();
400  rawlogEntry++;
401  };
402 
403  // Load sensory frame from the rawlog:
404  observations.reset();
405  while (!observations)
406  {
408  inStream >> obj;
409  if (obj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
410  {
411  observations = std::dynamic_pointer_cast<CSensoryFrame>(obj);
412  }
413  rawlogEntry++;
414  }
415  return true;
416  }
417  catch (CExceptionEOF&)
418  {
419  return false;
420  }
421  catch (std::exception& e)
422  {
423  std::cerr << "[CRawlog::readActionObservationPair] Found exception:"
424  << std::endl
425  << e.what() << std::endl;
426  return false;
427  }
428  catch (...)
429  {
430  std::cerr << "Untyped exception reading rawlog file!!" << std::endl;
431  return false;
432  }
433 }
434 
435 void CRawlog::findObservationsByClassInRange(
437  const mrpt::rtti::TRuntimeClassId* class_type,
438  TListTimeAndObservations& out_found, size_t guess_start_position) const
439 {
440  MRPT_UNUSED_PARAM(guess_start_position);
441  MRPT_START
442 
443  out_found.clear();
444 
445  if (m_seqOfActObs.empty()) return;
446 
447  // Find the first appearance of time_start:
448  // ---------------------------------------------------
449  TListObjects::const_iterator first = m_seqOfActObs.begin();
450  const TListObjects::const_iterator last = m_seqOfActObs.end();
451  {
452  // The following is based on lower_bound:
453  size_t count, step;
454  count = std::distance(first, last);
455  while (count > 0)
456  {
458  step = count / 2;
459  std::advance(it, step);
460 
461  // The comparison function:
462  TTimeStamp this_timestamp;
463  if ((*it)->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
464  {
466  std::dynamic_pointer_cast<CObservation>(*it);
467  this_timestamp = o->timestamp;
468  ASSERT_(this_timestamp != INVALID_TIMESTAMP);
469  }
470  else
472  "Element found which is not derived from CObservation");
473 
474  if (this_timestamp < time_start) // *it < time_start
475  {
476  first = ++it;
477  count -= step + 1;
478  }
479  else
480  count = step;
481  }
482  // "first" is our guy
483  }
484 
485  // Iterate until we get out of the time window:
486  while (first != last)
487  {
488  TTimeStamp this_timestamp;
489  if ((*first)->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
490  {
492  std::dynamic_pointer_cast<CObservation>(*first);
493  this_timestamp = o->timestamp;
494  ASSERT_(this_timestamp != INVALID_TIMESTAMP);
495 
496  if (this_timestamp < time_end)
497  {
498  if (o->GetRuntimeClass()->derivedFrom(class_type))
499  out_found.insert(TTimeObservationPair(this_timestamp, o));
500  }
501  else
502  {
503  break; // end of time window!
504  }
505  }
506  else
508  "Element found which is not derived from CObservation");
509 
510  first++;
511  }
512 
513  MRPT_END
514 }
515 
516 bool CRawlog::getActionObservationPair(
517  CActionCollection::Ptr& action, CSensoryFrame::Ptr& observations,
518  size_t& rawlogEntry) const
519 {
520  try
521  {
522  while (getType(rawlogEntry) != CRawlog::etActionCollection)
523  {
524  rawlogEntry++;
525  }
526  action = getAsAction(rawlogEntry++);
527 
528  while (getType(rawlogEntry) != CRawlog::etSensoryFrame)
529  {
530  rawlogEntry++;
531  }
532  observations = getAsObservations(rawlogEntry++);
533 
534  return true;
535  }
536  catch (std::exception&)
537  {
538  return false;
539  }
540  catch (...)
541  {
542  std::cerr << "Untyped exception getting act-obs pair from rawlog!!"
543  << std::endl;
544  return false;
545  }
546 }
547 
548 void CRawlog::getCommentText(std::string& t) const { t = m_commentTexts.text; }
549 std::string CRawlog::getCommentText() const { return m_commentTexts.text; }
550 void CRawlog::getCommentTextAsConfigFile(
551  mrpt::config::CConfigFileMemory& memCfg) const
552 {
553  memCfg.setContent(m_commentTexts.text);
554 }
555 
556 void CRawlog::setCommentText(const std::string& t) { m_commentTexts.text = t; }
557 std::string CRawlog::detectImagesDirectory(const std::string& str)
558 {
559  const std::string rawlog_path = extractFileDirectory(str);
560  std::string temptative_img_path =
561  rawlog_path + extractFileName(str) + std::string("_Images");
562  if (mrpt::system::fileExists(temptative_img_path))
563  return temptative_img_path;
564  else if (
566  temptative_img_path =
567  (rawlog_path + extractFileName(str) + std::string("_images"))))
568  return temptative_img_path;
569  else if (
571  temptative_img_path =
572  (rawlog_path + extractFileName(str) + std::string("_IMAGES"))))
573  return temptative_img_path;
574  else
575  return rawlog_path + "Images";
576 }
This class implements a config file-like interface over a memory-stored string list.
GLuint GLuint GLsizei count
Definition: glext.h:3528
This "observation" is actually a placeholder for a text block with comments or additional parameters ...
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble t
Definition: glext.h:3689
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLint * first
Definition: glext.h:3827
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:127
A structure that holds runtime class type information.
Definition: CObject.h:30
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:156
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
Declares a class for storing a collection of robot actions.
unsigned char uint8_t
Definition: rptypes.h:41
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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:555
std::pair< mrpt::system::TTimeStamp, CObservation::Ptr > TTimeObservationPair
For usage with CRawlog classes.
Definition: CRawlog.h:22
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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 IS_DERIVED(ptrObj, class_name)
Evaluates to true if a pointer to an object (derived from mrpt::rtti::CObject) is an instance of the ...
Definition: CObject.h:108
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:84
std::multimap< mrpt::system::TTimeStamp, CObservation::Ptr > TListTimeAndObservations
For usage with CRawlog classes.
Definition: CRawlog.h:25
GLuint index
Definition: glext.h:4054
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:66
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:52
GLsizei const GLchar ** string
Definition: glext.h:4101
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:27
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:52
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:93
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
Transparently opens a compressed "gz" file and reads uncompressed data from it.
GLsizeiptr size
Definition: glext.h:3923
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:61
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:186
const Scalar * const_iterator
Definition: eigen_plugins.h:27
#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:77
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
Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object w...
Definition: CObject.h:168
Used in mrpt::serialization::CArchive.
Definition: CArchive.h:34
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020