MRPT  1.9.9
CLogFileRecord.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 "nav-precomp.h" // Precomp header
11 
15 #include <mrpt/poses/CPoint2D.h>
18 
19 using namespace mrpt::nav;
20 
22 
23 /*---------------------------------------------------------------
24  Constructor
25  ---------------------------------------------------------------*/
27  : robotPoseLocalization(0, 0, 0),
28  robotPoseOdometry(0, 0, 0),
29  relPoseSense(0, 0, 0),
30  relPoseVelCmd(0, 0, 0),
31  WS_targets_relative(),
32  cur_vel(0, 0, 0),
33  cur_vel_local(0, 0, 0),
34 
35  rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
36  rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0)
37 {
38  infoPerPTG.clear();
39  WS_Obstacles.clear();
40 }
41 
42 uint8_t CLogFileRecord::serializeGetVersion() const { return 26; }
44 {
45  uint32_t i, n;
46 
47  // Version 0 ---------
48  n = infoPerPTG.size();
49  out << n;
50  for (i = 0; i < n; i++)
51  {
52  out << infoPerPTG[i].PTG_desc.c_str();
53 
54  uint32_t m = infoPerPTG[i].TP_Obstacles.size();
55  out << m;
56  if (m)
57  out.WriteBuffer(
58  (const void*)&(*infoPerPTG[i].TP_Obstacles.begin()),
59  m * sizeof(infoPerPTG[i].TP_Obstacles[0]));
60 
61  out << infoPerPTG[i]
62  .TP_Targets; // v8: CPoint2D -> TPoint2D. v26: vector
63  out << infoPerPTG[i].TP_Robot; // v17
64  out << infoPerPTG[i].timeForTPObsTransformation
65  << infoPerPTG[i].timeForHolonomicMethod; // made double in v12
66  out << infoPerPTG[i].desiredDirection << infoPerPTG[i].desiredSpeed
67  << infoPerPTG[i].evaluation; // made double in v12
68  // removed in v23: out << evaluation_org << evaluation_priority; //
69  // added in v21
70  out << infoPerPTG[i].HLFR;
71 
72  // Version 9: Removed security distances. Added optional field with
73  // PTG info.
74  const bool there_is_ptg_data = infoPerPTG[i].ptg ? true : false;
75  out << there_is_ptg_data;
76  if (there_is_ptg_data) out << infoPerPTG[i].ptg;
77 
78  // Was: out << infoPerPTG[i].clearance.raw_clearances; // v19
79  infoPerPTG[i].clearance.writeToStream(out); // v25
80  }
82  out << WS_Obstacles_original; // v20
83 
84  // Removed v24: out << robotOdometryPose;
86 
87  out << WS_targets_relative; // v8, v26: vector
88  // v16:
91  << rel_pose_PTG_origin_wrt_sense_NOP; // v24: CPose2D->TPose2D
92 
93  // Removed: v24. out << ptg_last_curRobotVelLocal; // v17
95 
96  if (ptg_index_NOP < 0) out << cmd_vel /*v10*/ << cmd_vel_original; // v15
97 
98  // Previous values: REMOVED IN VERSION #6
99  n = robotShape_x.size();
100  out << n;
101  if (n)
102  {
103  out.WriteBuffer(
104  (const void*)&(*robotShape_x.begin()), n * sizeof(robotShape_x[0]));
105  out.WriteBuffer(
106  (const void*)&(*robotShape_y.begin()), n * sizeof(robotShape_y[0]));
107  }
108 
109  // Version 1 ---------
110  out << cur_vel << cur_vel_local; /*v10*/
111  // out << estimatedExecutionPeriod; // removed v13
112 
113  // Version 3 ----------
114  for (i = 0; i < infoPerPTG.size(); i++)
115  {
116  out << infoPerPTG[i]
117  .evalFactors.base; // v22: this is now a TParameters
118  }
119 
120  out << nPTGs; // v4
121  // out << timestamp; // removed v13
122  out << robotShape_radius; // v11
123  // out << cmd_vel_filterings; // added v12: Removed in v15
124 
125  out << values << timestamps; // v13
126 
127  out << relPoseSense << relPoseVelCmd; // v14, v24 changed CPose2D->TPose2D
128 
129  // v15: cmd_vel converted from std::vector<double> into CSerializable
130  out << additional_debug_msgs; // v18
131 
132  navDynState.writeToStream(out); // v24
133 }
134 
136  mrpt::serialization::CArchive& in, uint8_t version)
137 {
138  switch (version)
139  {
140  case 0:
141  case 1:
142  case 2:
143  case 3:
144  case 4:
145  case 5:
146  case 6:
147  case 7:
148  case 8:
149  case 9:
150  case 10:
151  case 11:
152  case 12:
153  case 13:
154  case 14:
155  case 15:
156  case 16:
157  case 17:
158  case 18:
159  case 19:
160  case 20:
161  case 21:
162  case 22:
163  case 23:
164  case 24:
165  case 25:
166  case 26:
167  {
168  // Version 0 --------------
169  uint32_t i, n;
170 
171  infoPerPTG.clear();
172 
173  in >> n;
174  infoPerPTG.resize(n);
175  for (i = 0; i < n; i++)
176  {
177  auto& ipp = infoPerPTG[i];
178  in >> ipp.PTG_desc;
179 
180  int32_t m;
181  in >> m;
182  ipp.TP_Obstacles.resize(m);
183  if (m)
184  in.ReadBufferFixEndianness(&(*ipp.TP_Obstacles.begin()), m);
185 
186  ipp.TP_Targets.clear();
187  if (version >= 8)
188  {
189  if (version >= 26)
190  {
191  in >> ipp.TP_Targets;
192  }
193  else
194  {
196  in >> trg;
197  ipp.TP_Targets.push_back(trg);
198  }
199  }
200  else
201  {
203  in >> pos;
204  ipp.TP_Targets.emplace_back(pos.x(), pos.y());
205  }
206  if (version >= 17)
207  in >> ipp.TP_Robot;
208  else
209  ipp.TP_Robot = mrpt::math::TPoint2D(0, 0);
210 
211  if (version >= 12)
212  {
213  in >> ipp.timeForTPObsTransformation >>
214  ipp.timeForHolonomicMethod;
215  in >> ipp.desiredDirection >> ipp.desiredSpeed >>
216  ipp.evaluation;
217  }
218  else
219  {
220  in.ReadAsAndCastTo<float, double>(
221  ipp.timeForTPObsTransformation);
222  in.ReadAsAndCastTo<float, double>(
223  ipp.timeForHolonomicMethod);
224  in.ReadAsAndCastTo<float, double>(ipp.desiredDirection);
225  in.ReadAsAndCastTo<float, double>(ipp.desiredSpeed);
226  in.ReadAsAndCastTo<float, double>(ipp.evaluation);
227  }
228  if (version >= 21 && version < 23)
229  {
230  double evaluation_org, evaluation_priority;
231  in >> evaluation_org >> evaluation_priority;
232  }
233 
234  in >> ipp.HLFR;
235 
236  if (version >= 9) // Extra PTG info
237  {
238  ipp.ptg.reset();
239 
240  bool there_is_ptg_data;
241  in >> there_is_ptg_data;
242  if (there_is_ptg_data)
243  ipp.ptg = std::dynamic_pointer_cast<
245  }
246 
247  if (version >= 19)
248  {
249  if (version < 25)
250  {
251  std::vector<std::map<double, double>> raw_clearances;
252  in >> raw_clearances;
253  ipp.clearance.resize(
254  raw_clearances.size(), raw_clearances.size());
255  for (size_t k = 0; k < raw_clearances.size(); k++)
256  ipp.clearance.get_path_clearance_decimated(k) =
257  raw_clearances[k];
258  }
259  else
260  {
261  ipp.clearance.readFromStream(in);
262  }
263  }
264  else
265  {
266  ipp.clearance.clear();
267  }
268  }
269 
270  in >> nSelectedPTG >> WS_Obstacles;
271  if (version >= 20)
272  {
273  in >> WS_Obstacles_original; // v20
274  }
275  else
276  {
278  }
279 
280  if (version < 24)
281  {
282  mrpt::poses::CPose2D robotOdometryPose;
283  in >> robotOdometryPose;
284  robotPoseOdometry = robotOdometryPose.asTPose();
285  robotPoseLocalization = robotOdometryPose.asTPose();
286  }
287  else
288  {
290  }
291 
292  WS_targets_relative.clear();
293  if (version >= 8)
294  {
295  if (version >= 26)
296  {
297  in >> WS_targets_relative;
298  }
299  else
300  {
302  in >> trg;
303  WS_targets_relative.emplace_back(trg);
304  }
305  }
306  else
307  {
309  in >> pos;
310  WS_targets_relative.emplace_back(
311  mrpt::math::TPoint2D(pos.x(), pos.y()));
312  }
313 
314  if (version >= 16)
315  {
316  in >> ptg_index_NOP >> ptg_last_k_NOP;
317  if (version >= 24)
318  {
321  }
322  else
323  {
324  mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP,
325  crel_pose_PTG_origin_wrt_sense_NOP;
326  in >> crel_cur_pose_wrt_last_vel_cmd_NOP >>
327  crel_pose_PTG_origin_wrt_sense_NOP;
329  crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose();
331  crel_pose_PTG_origin_wrt_sense_NOP.asTPose();
332  }
333  }
334  else
335  {
336  ptg_index_NOP = -1;
337  }
338  if (version >= 17 && version < 24)
339  {
341  }
342  if (version >= 24)
343  {
345  }
346 
347  if (version >= 10)
348  {
349  if (version >= 15)
350  {
351  if (ptg_index_NOP < 0) in >> cmd_vel;
352  }
353  else
354  {
355  std::vector<double> vel;
356  in >> vel;
357  if (vel.size() == 2)
360  else
363  for (size_t k = 0; k < cmd_vel->getVelCmdLength(); k++)
364  cmd_vel->setVelCmdElement(i, vel[k]);
365  }
366  }
367  else
368  {
369  float v, w;
370  in >> v >> w;
373  cmd_vel->setVelCmdElement(0, v);
374  cmd_vel->setVelCmdElement(0, w);
375  }
376 
377  if (version >= 15 && ptg_index_NOP < 0) in >> cmd_vel_original;
378 
379  if (version < 13)
380  {
381  float old_exec_time;
382  in >> old_exec_time;
383  values["executionTime"] = old_exec_time;
384  }
385 
386  if (version < 6)
387  {
388  mrpt::math::CVectorFloat prevV, prevW, prevSelPTG;
389 
390  // Previous values: (Removed in version 6)
391  in >> n;
392  prevV.resize(n);
393  if (n) in.ReadBufferFixEndianness(&(*prevV.begin()), n);
394 
395  in >> n;
396  prevW.resize(n);
397  if (n) in.ReadBufferFixEndianness(&(*prevW.begin()), n);
398 
399  in >> n;
400  prevSelPTG.resize(n);
401  if (n) in.ReadBufferFixEndianness(&(*prevSelPTG.begin()), n);
402  }
403 
404  in >> n;
405  robotShape_x.resize(n);
406  robotShape_y.resize(n);
407  if (n)
408  {
411  }
412 
413  if (version > 0)
414  { // Version 1 --------------
415  if (version >= 10)
416  {
417  in >> cur_vel >> cur_vel_local;
418  }
419  else
420  {
421  float actual_v, actual_w;
422  in >> actual_v >> actual_w;
423  cur_vel = mrpt::math::TTwist2D(0, 0, 0);
424  cur_vel_local =
425  mrpt::math::TTwist2D(actual_v, .0, actual_w);
426  }
427  }
428  else
429  { // Default values for old versions:
430  cur_vel = mrpt::math::TTwist2D(0, 0, 0);
431  }
432 
433  if (version < 13 && version > 1)
434  {
435  float old_estim_period;
436  in >> old_estim_period;
437  values["estimatedExecutionPeriod"] = old_estim_period;
438  }
439 
440  for (i = 0; i < infoPerPTG.size(); i++)
441  {
442  infoPerPTG[i].evalFactors.clear();
443  }
444  if (version > 2)
445  {
446  // Version 3..22 ----------
447  for (i = 0; i < infoPerPTG.size(); i++)
448  {
449  if (version < 22)
450  {
451  in >> n;
452  for (unsigned int j = 0; j < n; j++)
453  {
454  float f;
455  in >> f;
456  infoPerPTG[i].evalFactors[mrpt::format("f%u", j)] =
457  f;
458  }
459  }
460  else
461  {
462  in >> infoPerPTG[i].evalFactors.base;
463  }
464  }
465  }
466 
467  if (version > 3)
468  {
469  // Version 4 ----------
470  in >> nPTGs;
471  if (version < 9) // Old "securityDistances", now unused.
472  {
473  in >> n;
474  float dummy;
475  for (i = 0; i < n; i++) in >> dummy;
476  }
477  }
478  else
479  {
480  nPTGs = infoPerPTG.size();
481  }
482 
483  if (version > 4)
484  {
485  if (version < 10)
486  {
487  int32_t navigatorBehavior; // removed in v10
488  in >> navigatorBehavior;
489  }
490 
491  if (version < 6) // Removed in version 6:
492  {
493  mrpt::poses::CPoint2D doorCrossing_P1, doorCrossing_P2;
494  in >> doorCrossing_P1 >> doorCrossing_P2;
495  }
496  }
497 
498  if (version > 6 && version < 13)
499  {
501  in >> tt;
502  timestamps["tim_start_iteration"] = tt;
503  }
504 
505  if (version >= 11)
506  {
507  in >> robotShape_radius;
508  }
509  else
510  {
511  robotShape_radius = 0.5;
512  }
513 
514  if (version >= 12 && version < 15)
515  {
516  std::vector<std::vector<double>> dummy_cmd_vel_filterings;
517  in >> dummy_cmd_vel_filterings;
518  }
519 
520  if (version >= 13)
521  {
522  in >> values >> timestamps;
523  }
524  else
525  {
526  values.clear();
527  timestamps.clear();
528  }
529 
530  if (version >= 14)
531  {
532  if (version >= 24)
533  {
534  in >> relPoseSense >> relPoseVelCmd;
535  }
536  else
537  {
538  mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd;
539  in >> crelPoseSense >> crelPoseVelCmd;
540  relPoseSense = crelPoseSense.asTPose();
541  relPoseVelCmd = crelPoseVelCmd.asTPose();
542  }
543  }
544  else
545  {
547  }
548 
549  if (version >= 18)
550  in >> additional_debug_msgs;
551  else
552  additional_debug_msgs.clear();
553 
554  if (version >= 24)
556  else
557  {
558  navDynState =
561  if (!WS_targets_relative.empty())
563  }
564  }
565  break;
566  default:
568  };
569 }
mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
int32_t nSelectedPTG
The selected PTG.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
std::vector< mrpt::math::TPose2D > WS_targets_relative
Relative poses (wrt to robotPoseLocalization) for.
mrpt::math::CVectorFloat robotShape_y
mrpt::math::TTwist2D cur_vel_local
The actual robot velocities in local (robot) coordinates, as read from sensors, in "m/sec" and "rad/s...
Template for column vectors of dynamic size, compatible with Eigen.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
std::map< std::string, mrpt::system::TTimeStamp > timestamps
Known values:
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense_NOP
mrpt::math::TPose2D robotPoseOdometry
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
mrpt::math::TPose2D relPoseSense
uint32_t nPTGs
The number of PTGS:
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
mrpt::math::CVectorFloat robotShape_x
The robot shape in WS.
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
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
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState
This is the base class for any user-defined PTG.
mrpt::math::TPose2D robotPoseLocalization
The robot pose (from odometry and from the localization/SLAM system).
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
void ReadAsAndCastTo(CAST_TO_TYPE &read_here)
Read a value from a stream stored in a type different of the target variable, making the conversion v...
Definition: CArchive.h:147
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
int16_t ptg_index_NOP
Negative means no NOP mode evaluation, so the rest of "NOP variables" should be ignored.
mrpt::math::TPose2D relTarget
Current relative target location.
mrpt::maps::CSimplePointsMap WS_Obstacles_original
std::map< std::string, double > values
Known values:
A class used to store a 2D point.
Definition: CPoint2D.h:32
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState navDynState
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
mrpt::vision::TStereoCalibResults out
std::vector< TInfoPerPTG > infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel_original
Motion command as comes out from the PTG, before scaling speed limit filtering.
mrpt::math::TTwist2D cur_vel
The actual robot velocities in global (map) coordinates, as read from sensors, in "m/sec" and "rad/se...
Lightweight 2D pose.
Definition: TPose2D.h:22
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
void resize(std::size_t N, bool zeroNewElements=false)
mrpt::maps::CSimplePointsMap WS_Obstacles
The WS-Obstacles.
std::shared_ptr< CVehicleVelCmd > Ptr
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
Dynamic state that may affect the PTG path parameterization.
mrpt::math::TPose2D relPoseVelCmd
Kinematic model for Ackermann-like or differential-driven vehicles.
std::map< std::string, std::string > additional_debug_msgs
Additional debug traces.
double robotShape_radius
The circular robot radius.



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