MRPT  1.9.9
CArchive.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 "serialization-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/byte_manip.h>
13 #include <mrpt/core/exceptions.h>
18 #include <array>
19 #include <cstring> // strlen()
20 
21 using namespace mrpt::serialization;
22 
23 const uint8_t SERIALIZATION_END_FLAG = 0x88;
24 
25 size_t CArchive::ReadBuffer(void* Buffer, size_t Count)
26 {
27  ASSERT_(Buffer != nullptr);
28  if (Count)
29  {
30  size_t actuallyRead = this->read(Buffer, Count);
31  if (!actuallyRead)
32  {
34  "(EOF?) Cannot read requested number of bytes from stream");
35  }
36  else
37  {
38  return actuallyRead;
39  }
40  }
41  else
42  return 0;
43 }
44 
45 /*---------------------------------------------------------------
46 WriteBuffer
47 Writes a block of bytes to the stream.
48 ---------------------------------------------------------------*/
49 void CArchive::WriteBuffer(const void* Buffer, size_t Count)
50 {
51  ASSERT_(Buffer != nullptr);
52  if (Count)
53  if (Count != this->write(Buffer, Count))
54  THROW_EXCEPTION("Cannot write bytes to stream!");
55 }
56 
59 {
60  uint64_t rep = s.time_since_epoch().count();
61  return out << rep;
62 }
63 
66 {
67  uint64_t rep;
68  in >> rep;
70  return in;
71 }
72 
74  CArchive& out, const std::vector<bool>& a)
75 {
76  auto n = (uint32_t)a.size();
77  out << n;
78  if (n)
79  {
80  std::vector<uint8_t> b(n);
81  std::vector<bool>::const_iterator it;
82  std::vector<uint8_t>::iterator it2;
83  for (it = a.begin(), it2 = b.begin(); it != a.end(); ++it, ++it2)
84  *it2 = *it ? 1 : 0;
85  out.WriteBuffer((void*)&b[0], (int)(sizeof(b[0]) * n));
86  }
87  return out;
88 }
89 
91 {
92  auto n = (uint32_t)str.size();
93  out << n;
94  if (n) out.WriteBuffer(str.c_str(), n);
95  return out;
96 }
97 
98 /*---------------------------------------------------------------
99 Writes an object to the stream.
100 ---------------------------------------------------------------*/
102 {
103  MRPT_START
104 
105  // First, the "classname".
106  const char* className;
107  if (o != nullptr)
108  {
109  className = o->GetRuntimeClass()->className;
110  }
111  else
112  {
113  className = "nullptr";
114  }
115 
116  int8_t classNamLen = strlen(className);
117  int8_t classNamLen_mod = classNamLen | 0x80;
118 
119  (*this) << classNamLen_mod;
120  this->WriteBuffer(className, classNamLen);
121 
122  // Next, the version number:
123  if (o != nullptr)
124  {
125  // Version:
126  uint8_t version = o->serializeGetVersion();
127  (*this) << version;
128 
129  // and object data:
130  o->serializeTo(*this);
131  }
132 
133  // In MRPT 0.5.5 a end flag is introduced:
134  (*this) << SERIALIZATION_END_FLAG;
135 
136  MRPT_END
137 }
138 
140 {
141  WriteObject(pObj.get());
142  return *this;
143 }
144 
145 /** Write an object to a stream in the binary MRPT format. */
147 {
148  WriteObject(&obj);
149  return *this;
150 }
151 
153 {
154  pObj = ReadObject();
155  return *this;
156 }
157 
159 {
160  ReadObject(&obj);
161  return *this;
162 }
163 
164 namespace mrpt
165 {
166 namespace serialization
167 {
168 // For backward compatibility, since in MRPT<0.8.1 vector_XXX and
169 // std::vector<XXX> were exactly equivalent, now there're not.
170 namespace detail
171 {
172 template <typename VEC>
173 inline CArchive& writeStdVectorToStream(CArchive& s, const VEC& v)
174 {
175  const auto n = static_cast<uint32_t>(v.size());
176  s << n;
177  if (n) s.WriteBufferFixEndianness(&v[0], n);
178  return s;
179 }
180 template <typename VEC>
182 {
183  uint32_t n;
184  s >> n;
185  v.resize(n);
186  if (n) s.ReadBufferFixEndianness(&v[0], n);
187  return s;
188 }
189 } // namespace detail
190 } // namespace serialization
191 } // namespace mrpt
192 
193 // Write:
195  CArchive& s, const std::vector<float>& a)
196 {
197  return detail::writeStdVectorToStream(s, a);
198 }
201 {
202  return detail::writeStdVectorToStream(s, a);
203 }
205  CArchive& s, const std::vector<double>& a)
206 {
207  return detail::writeStdVectorToStream(s, a);
208 }
210  CArchive& s, const std::vector<int32_t>& a)
211 {
212  return detail::writeStdVectorToStream(s, a);
213 }
215  CArchive& s, const std::vector<uint32_t>& a)
216 {
217  return detail::writeStdVectorToStream(s, a);
218 }
220  CArchive& s, const std::vector<uint16_t>& a)
221 {
222  return detail::writeStdVectorToStream(s, a);
223 }
225  CArchive& s, const std::vector<int16_t>& a)
226 {
227  return detail::writeStdVectorToStream(s, a);
228 }
230  CArchive& s, const std::vector<int64_t>& a)
231 {
232  return detail::writeStdVectorToStream(s, a);
233 }
235  CArchive& s, const std::vector<uint8_t>& a)
236 {
237  return detail::writeStdVectorToStream(s, a);
238 }
240  CArchive& s, const std::vector<int8_t>& a)
241 {
242  return detail::writeStdVectorToStream(s, a);
243 }
244 
245 // Read:
247 {
248  return detail::readStdVectorToStream(s, a);
249 }
252 {
253  return detail::readStdVectorToStream(s, a);
254 }
256 {
257  return detail::readStdVectorToStream(s, a);
258 }
259 CArchive& mrpt::serialization::operator>>(CArchive& s, std::vector<int32_t>& a)
260 {
261  return detail::readStdVectorToStream(s, a);
262 }
263 CArchive& mrpt::serialization::operator>>(CArchive& s, std::vector<uint32_t>& a)
264 {
265  return detail::readStdVectorToStream(s, a);
266 }
267 CArchive& mrpt::serialization::operator>>(CArchive& s, std::vector<uint16_t>& a)
268 {
269  return detail::readStdVectorToStream(s, a);
270 }
271 CArchive& mrpt::serialization::operator>>(CArchive& s, std::vector<int16_t>& a)
272 {
273  return detail::readStdVectorToStream(s, a);
274 }
275 CArchive& mrpt::serialization::operator>>(CArchive& s, std::vector<int64_t>& a)
276 {
277  return detail::readStdVectorToStream(s, a);
278 }
279 CArchive& mrpt::serialization::operator>>(CArchive& s, std::vector<uint8_t>& a)
280 {
281  return detail::readStdVectorToStream(s, a);
282 }
284 {
285  return detail::readStdVectorToStream(s, a);
286 }
287 
288 #if MRPT_WORD_SIZE != 32 // If it's 32 bit, size_t <=> uint32_t
290  CArchive& s, const std::vector<size_t>& a)
291 {
292  return detail::writeStdVectorToStream(s, a);
293 }
295 {
296  return detail::readStdVectorToStream(s, a);
297 }
298 #endif
299 
301 {
302  uint32_t n;
303  in >> n;
304  a.resize(n);
305  if (n)
306  {
307  std::vector<uint8_t> b(n);
308  in.ReadBuffer((void*)&b[0], sizeof(b[0]) * n);
309  std::vector<uint8_t>::iterator it2;
310  std::vector<bool>::iterator it;
311  for (it = a.begin(), it2 = b.begin(); it != a.end(); ++it, ++it2)
312  *it = (*it2 != 0);
313  }
314  return in;
315 }
316 
318 {
319  uint32_t n;
320  in >> n;
321  str.resize(n);
322  if (n) in.ReadBuffer((void*)&str[0], n);
323  return in;
324 }
325 
326 //#define CARCHIVE_VERBOSE 1
327 #define CARCHIVE_VERBOSE 0
328 
330  std::string& strClassName, bool& isOldFormat, int8_t& version)
331 {
332  uint8_t lengthReadClassName = 255;
333  char readClassName[260];
334  readClassName[0] = 0;
335 
336  try
337  {
338  // First, read the class name: (exception is raised here if ZERO bytes
339  // read -> possibly an EOF)
340  if (sizeof(lengthReadClassName) !=
341  ReadBuffer(
342  (void*)&lengthReadClassName, sizeof(lengthReadClassName)))
343  THROW_EXCEPTION("Cannot read object header from stream! (EOF?)");
344 
345  // Is in old format (< MRPT 0.5.5)?
346  if (!(lengthReadClassName & 0x80))
347  {
348  isOldFormat = true;
349  uint8_t buf[3];
350  if (3 != ReadBuffer(buf, 3))
352  "Cannot read object header from stream! (EOF?)");
353  if (buf[0] || buf[1] || buf[2])
355  "Expecting 0x00 00 00 while parsing old streaming header "
356  "(Perhaps it's a gz-compressed stream? Use a GZ-stream for "
357  "reading)");
358  }
359  else
360  {
361  isOldFormat = false;
362  }
363 
364  // Remove MSB:
365  lengthReadClassName &= 0x7F;
366 
367  // Sensible class name size?
368  if (lengthReadClassName > 120)
370  "Class name has more than 120 chars. This probably means a "
371  "corrupted binary stream.");
372 
373  if (((size_t)lengthReadClassName) !=
374  ReadBuffer(readClassName, lengthReadClassName))
375  THROW_EXCEPTION("Cannot read object class name from stream!");
376 
377  readClassName[lengthReadClassName] = '\0';
378 
379  // Pass to string class:
380  strClassName = readClassName;
381 
382  // Next, the version number:
383  if (isOldFormat)
384  {
385  int32_t version_old;
386  // Handle big endian right:
387  if (sizeof(version_old) !=
388  ReadBufferFixEndianness(&version_old, 1 /*element count*/))
390  "Cannot read object streaming version from stream!");
391  ASSERT_(version_old >= 0 && version_old < 255);
392  version = int8_t(version_old);
393  }
394  else if (
395  strClassName != "nullptr" &&
396  sizeof(version) != ReadBuffer((void*)&version, sizeof(version)))
397  {
399  "Cannot read object streaming version from stream!");
400  }
401 
402 // In MRPT 0.5.5 an end flag was introduced:
403 #if CARCHIVE_VERBOSE
404  cerr << "[CArchive::ReadObject] readClassName:" << strClassName
405  << " version: " << version << endl;
406 #endif
407  }
408  catch (std::bad_alloc&)
409  {
410  throw;
411  }
412  catch (...)
413  {
414  if (lengthReadClassName == 255)
415  {
417  "Cannot read object due to EOF", CExceptionEOF);
418  }
419  else
420  {
422  "Exception while parsing typed object '%s' from stream!\n",
423  readClassName);
424  }
425  }
426 } // end method
427 
429  CSerializable* obj, const std::string& strClassName, bool isOldFormat,
430  int8_t version)
431 {
432  try
433  {
434  if (obj)
435  {
436  // Not de-serializing an "nullptr":
437  obj->serializeFrom(*this, version);
438  }
439  if (!isOldFormat)
440  {
441  uint8_t endFlag;
442  if (sizeof(endFlag) != ReadBuffer((void*)&endFlag, sizeof(endFlag)))
444  "Cannot read object streaming version from stream!");
445  if (endFlag != SERIALIZATION_END_FLAG)
447  "end-flag missing: There is a bug in the deserialization "
448  "method of class: '%s'",
449  strClassName.c_str());
450  }
451  }
452  catch (std::bad_alloc&)
453  {
454  throw;
455  }
456  catch (CExceptionEOF&)
457  {
458  throw;
459  }
460  catch (...)
461  {
463  "Exception while parsing typed object '%s' from stream!\n",
464  strClassName.c_str());
465  }
466 }
467 
468 /*---------------------------------------------------------------
469 Reads an object from stream, where its class is determined
470 by an existing object
471 exception std::exception On I/O error or undefined class.
472 ---------------------------------------------------------------*/
474 {
476 
477  std::string strClassName;
478  bool isOldFormat{false};
479  int8_t version{-1};
480 
481  internal_ReadObjectHeader(strClassName, isOldFormat, version);
482 
483  ASSERT_(existingObj && strClassName != "nullptr");
484  ASSERT_(strClassName != "nullptr");
485 
486  const TRuntimeClassId* id = existingObj->GetRuntimeClass();
487  const TRuntimeClassId* id2 = mrpt::rtti::findRegisteredClass(strClassName);
488 
489  if (!id2)
491  "Stored object has class '%s' which is not registered!",
492  strClassName.c_str());
493  if (id != id2)
495  "Stored class does not match with existing object!!:\n Stored: "
496  "%s\n Expected: %s",
497  id2->className, id->className));
498 
499  internal_ReadObject(existingObj, strClassName, isOldFormat, version);
500 }
501 
503  CArchive& s, const std::vector<std::string>& vec)
504 {
505  auto N = static_cast<uint32_t>(vec.size());
506  s << N;
507  for (size_t i = 0; i < N; i++) s << vec[i];
508  return s;
509 }
510 
512  CArchive& s, std::vector<std::string>& vec)
513 {
514  uint32_t N;
515  s >> N;
516  vec.resize(N);
517  for (size_t i = 0; i < N; i++) s >> vec[i];
518  return s;
519 }
520 
522 {
523  MRPT_START
524 
525  std::array<uint8_t, 0x10100> buf;
526  unsigned int nBytesTx = 0;
527 
528  const bool msg_format_is_tiny = msg.content.size() < 256;
529 
530  // Build frame -------------------------------------
531  buf[nBytesTx++] = msg_format_is_tiny ? 0x69 : 0x79;
532  buf[nBytesTx++] = (unsigned char)(msg.type);
533 
534  if (msg_format_is_tiny)
535  {
536  buf[nBytesTx++] = (unsigned char)msg.content.size();
537  }
538  else
539  {
540  buf[nBytesTx++] = msg.content.size() & 0xff; // lo
541  buf[nBytesTx++] = (msg.content.size() >> 8) & 0xff; // hi
542  }
543 
544  if (!msg.content.empty())
545  memcpy(&buf.at(nBytesTx), &msg.content[0], msg.content.size());
546  nBytesTx += (unsigned char)msg.content.size();
547  buf[nBytesTx++] = 0x96;
548 
549  // Send buffer -------------------------------------
550  WriteBuffer(&buf[0], nBytesTx); // Exceptions will be raised on errors here
551 
552  MRPT_END
553 }
554 
556 {
557  MRPT_START
558  std::vector<unsigned char> buf(66000);
559  unsigned int nBytesInFrame = 0;
560  unsigned long nBytesToRx = 0;
561  unsigned char tries = 2;
562  unsigned int payload_len = 0;
563  unsigned int expectedLen = 0;
564 
565  for (;;)
566  {
567  if (nBytesInFrame < 4)
568  nBytesToRx = 1;
569  else
570  {
571  if (buf[0] == 0x69)
572  {
573  payload_len = buf[2];
574  expectedLen = payload_len + 4;
575  }
576  else if (buf[0] == 0x79)
577  {
578  payload_len = MAKEWORD16B(
579  buf[3] /*low*/, buf[2] /*hi*/); // Length of the content
580  expectedLen = payload_len + 5;
581  }
582  nBytesToRx = expectedLen - nBytesInFrame;
583  } // end else
584 
585  unsigned long nBytesRx = 0;
586  try
587  {
588  nBytesRx = ReadBuffer(&buf[nBytesInFrame], nBytesToRx);
589  }
590  catch (...)
591  {
592  }
593 
594  // No more data! (read timeout is already included in the call to
595  // "Read")
596  if (!nBytesRx) return false;
597 
598  if (!nBytesInFrame && buf[0] != 0x69 && buf[0] != 0x79)
599  {
600  // Start flag is invalid:
601  if (!tries--) return false;
602  }
603  else
604  {
605  // Is a new byte for the frame:
606  nBytesInFrame += nBytesRx;
607 
608  if (nBytesInFrame == expectedLen)
609  {
610  // Frame complete
611  // check for frame be ok:
612 
613  // End flag?
614  if (buf[nBytesInFrame - 1] != 0x96)
615  {
616  // Error in frame!
617  return false;
618  }
619  else
620  {
621  // copy out data:
622  msg.type = buf[1];
623  if (buf[0] == 0x69)
624  {
625  msg.content.resize(payload_len);
626  if (!msg.content.empty())
627  memcpy(&msg.content[0], &buf[3], payload_len);
628  } // end if
629  if (buf[0] == 0x79)
630  {
631  msg.content.resize(payload_len);
632  if (!msg.content.empty())
633  memcpy(&msg.content[0], &buf[4], payload_len);
634  } // end if
635  return true;
636  }
637  }
638  }
639  }
640  MRPT_END
641 }
const uint8_t SERIALIZATION_END_FLAG
Definition: CArchive.cpp:23
std::chrono::duration< rep, period > duration
Definition: Clock.h:24
virtual void serializeTo(CArchive &out) const =0
Pure virtual method for writing (serializing) to an abstract archive.
CArchive & readStdVectorToStream(CArchive &s, VEC &v)
Definition: CArchive.cpp:181
#define MRPT_START
Definition: exceptions.h:241
#define THROW_TYPED_EXCEPTION(msg, exceptionClass)
Definition: exceptions.h:58
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
virtual void serializeFrom(CArchive &in, uint8_t serial_version)=0
Pure virtual method for reading (deserializing) from an abstract archive.
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
Definition: CArchive.h:133
A structure that holds runtime class type information.
Definition: CObject.h:31
CArchive & writeStdVectorToStream(CArchive &s, const VEC &v)
Definition: CArchive.cpp:173
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:49
void sendMessage(const CMessage &msg)
Send a message to the device.
Definition: CArchive.cpp:521
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
virtual size_t read(void *buf, size_t len)=0
Reads a block of bytes.
CArchive & operator>>(CSerializable &obj)
Reads a CSerializable object from the stream.
Definition: CArchive.cpp:158
void internal_ReadObjectHeader(std::string &className, bool &isOldFormat, int8_t &version)
Read the object Header.
Definition: CArchive.cpp:329
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
const char * className
Definition: CObject.h:34
CArchive & operator<<(const CSerializable &obj)
Write a CSerializable object to a stream in the binary MRPT format.
Definition: CArchive.cpp:146
const TRuntimeClassId * findRegisteredClass(const std::string &className, const bool allow_ignore_namespace=true)
Return info about a given class by its name, or nullptr if the class is not registered.
bool receiveMessage(CMessage &msg)
Tries to receive a message from the device.
Definition: CArchive.cpp:555
virtual size_t write(const void *buf, size_t len)=0
Writes a block of bytes.
CArchive & operator>>(CArchive &s, mrpt::aligned_std_vector< float > &a)
Definition: CArchive.cpp:250
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
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:27
void internal_ReadObject(CSerializable *newObj, const std::string &className, bool isOldFormat, int8_t version)
Read the object.
Definition: CArchive.cpp:428
#define MAKEWORD16B(__LOBYTE, __HILOBYTE)
Definition: byte_manip.h:16
mrpt::vision::TStereoCalibResults out
#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
CArchive & operator<<(CArchive &s, const mrpt::aligned_std_vector< float > &a)
Definition: CArchive.cpp:199
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
void WriteObject(const CSerializable *o)
Writes an object to the stream.
Definition: CArchive.cpp:101
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:35
virtual uint8_t serializeGetVersion() const =0
Must return the current versioning number of the object.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
#define THROW_STACKED_EXCEPTION_CUSTOM_MSG2(stuff, param1)
Definition: exceptions.h:90
Used in mrpt::serialization::CArchive.
Definition: CArchive.h:36
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020