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