MRPT  1.9.9
CLogFileRecord.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 "nav-precomp.h" // Precomp header
11 
13 #include <mrpt/poses/CPoint2D.h>
18 
19 using namespace mrpt::nav;
20 
22 
23 /*---------------------------------------------------------------
24  Constructor
25  ---------------------------------------------------------------*/
27  : nPTGs(0),
28  nSelectedPTG(-1),
29  robotPoseLocalization(0, 0, 0),
30  robotPoseOdometry(0, 0, 0),
31  relPoseSense(0, 0, 0),
32  relPoseVelCmd(0, 0, 0),
33  WS_targets_relative(),
34  cur_vel(0, 0, 0),
35  cur_vel_local(0, 0, 0),
36  robotShape_radius(.0),
37  ptg_index_NOP(-1),
38  ptg_last_k_NOP(0),
39  rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
40  rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0)
41 {
42  infoPerPTG.clear();
43  WS_Obstacles.clear();
44 }
45 
48 {
49  uint32_t i, n;
50 
51  // Version 0 ---------
52  n = infoPerPTG.size();
53  out << n;
54  for (i = 0; i < n; i++)
55  {
56  out << infoPerPTG[i].PTG_desc.c_str();
57 
58  uint32_t m = infoPerPTG[i].TP_Obstacles.size();
59  out << m;
60  if (m)
61  out.WriteBuffer(
62  (const void*)&(*infoPerPTG[i].TP_Obstacles.begin()),
63  m * sizeof(infoPerPTG[i].TP_Obstacles[0]));
64 
65  out << infoPerPTG[i]
66  .TP_Targets; // v8: CPoint2D -> TPoint2D. v26: vector
67  out << infoPerPTG[i].TP_Robot; // v17
68  out << infoPerPTG[i].timeForTPObsTransformation
69  << infoPerPTG[i].timeForHolonomicMethod; // made double in v12
70  out << infoPerPTG[i].desiredDirection << infoPerPTG[i].desiredSpeed
71  << infoPerPTG[i].evaluation; // made double in v12
72  // removed in v23: out << evaluation_org << evaluation_priority; //
73  // added in v21
74  out << infoPerPTG[i].HLFR;
75 
76  // Version 9: Removed security distances. Added optional field with
77  // PTG info.
78  const bool there_is_ptg_data = infoPerPTG[i].ptg ? true : false;
79  out << there_is_ptg_data;
80  if (there_is_ptg_data) out << infoPerPTG[i].ptg;
81 
82  // Was: out << infoPerPTG[i].clearance.raw_clearances; // v19
83  infoPerPTG[i].clearance.writeToStream(out); // v25
84  }
85  out << nSelectedPTG << WS_Obstacles;
86  out << WS_Obstacles_original; // v20
87 
88  // Removed v24: out << robotOdometryPose;
90 
91  out << WS_targets_relative; // v8, v26: vector
92  // v16:
93  out << ptg_index_NOP << ptg_last_k_NOP;
95  << rel_pose_PTG_origin_wrt_sense_NOP; // v24: CPose2D->TPose2D
96 
97  // Removed: v24. out << ptg_last_curRobotVelLocal; // v17
99 
100  if (ptg_index_NOP < 0) out << cmd_vel /*v10*/ << cmd_vel_original; // v15
101 
102  // Previous values: REMOVED IN VERSION #6
103  n = robotShape_x.size();
104  out << n;
105  if (n)
106  {
107  out.WriteBuffer(
108  (const void*)&(*robotShape_x.begin()), n * sizeof(robotShape_x[0]));
109  out.WriteBuffer(
110  (const void*)&(*robotShape_y.begin()), n * sizeof(robotShape_y[0]));
111  }
112 
113  // Version 1 ---------
114  out << cur_vel << cur_vel_local; /*v10*/
115  // out << estimatedExecutionPeriod; // removed v13
116 
117  // Version 3 ----------
118  for (i = 0; i < infoPerPTG.size(); i++)
119  {
120  out << infoPerPTG[i].evalFactors; // v22: this is now a TParameters
121  }
122 
123  out << nPTGs; // v4
124  // out << timestamp; // removed v13
125  out << robotShape_radius; // v11
126  // out << cmd_vel_filterings; // added v12: Removed in v15
127 
128  out << values << timestamps; // v13
129 
130  out << relPoseSense << relPoseVelCmd; // v14, v24 changed CPose2D->TPose2D
131 
132  // v15: cmd_vel converted from std::vector<double> into CSerializable
133  out << additional_debug_msgs; // v18
134 
135  navDynState.writeToStream(out); // v24
136 }
137 
140 {
141  switch (version)
142  {
143  case 0:
144  case 1:
145  case 2:
146  case 3:
147  case 4:
148  case 5:
149  case 6:
150  case 7:
151  case 8:
152  case 9:
153  case 10:
154  case 11:
155  case 12:
156  case 13:
157  case 14:
158  case 15:
159  case 16:
160  case 17:
161  case 18:
162  case 19:
163  case 20:
164  case 21:
165  case 22:
166  case 23:
167  case 24:
168  case 25:
169  case 26:
170  {
171  // Version 0 --------------
172  uint32_t i, n;
173 
174  infoPerPTG.clear();
175 
176  in >> n;
177  infoPerPTG.resize(n);
178  for (i = 0; i < n; i++)
179  {
180  auto& ipp = infoPerPTG[i];
181  char str[256];
182  in >> str;
183  ipp.PTG_desc = std::string(str);
184 
185  int32_t m;
186  in >> m;
187  ipp.TP_Obstacles.resize(m);
188  if (m)
189  in.ReadBufferFixEndianness(&(*ipp.TP_Obstacles.begin()), m);
190 
191  ipp.TP_Targets.clear();
192  if (version >= 8)
193  {
194  if (version >= 26)
195  {
196  in >> ipp.TP_Targets;
197  }
198  else
199  {
201  in >> trg;
202  ipp.TP_Targets.push_back(trg);
203  }
204  }
205  else
206  {
208  in >> pos;
209  ipp.TP_Targets.push_back(
210  mrpt::math::TPoint2D(pos.x(), pos.y()));
211  }
212  if (version >= 17)
213  in >> ipp.TP_Robot;
214  else
215  ipp.TP_Robot = mrpt::math::TPoint2D(0, 0);
216 
217  if (version >= 12)
218  {
219  in >> ipp.timeForTPObsTransformation >>
220  ipp.timeForHolonomicMethod;
221  in >> ipp.desiredDirection >> ipp.desiredSpeed >>
222  ipp.evaluation;
223  }
224  else
225  {
226  in.ReadAsAndCastTo<float, double>(
227  ipp.timeForTPObsTransformation);
228  in.ReadAsAndCastTo<float, double>(
229  ipp.timeForHolonomicMethod);
230  in.ReadAsAndCastTo<float, double>(ipp.desiredDirection);
231  in.ReadAsAndCastTo<float, double>(ipp.desiredSpeed);
232  in.ReadAsAndCastTo<float, double>(ipp.evaluation);
233  }
234  if (version >= 21 && version < 23)
235  {
236  double evaluation_org, evaluation_priority;
237  in >> evaluation_org >> evaluation_priority;
238  }
239 
240  in >> ipp.HLFR;
241 
242  if (version >= 9) // Extra PTG info
243  {
244  ipp.ptg.reset();
245 
246  bool there_is_ptg_data;
247  in >> there_is_ptg_data;
248  if (there_is_ptg_data)
249  ipp.ptg = std::dynamic_pointer_cast<
250  CParameterizedTrajectoryGenerator>(in.ReadObject());
251  }
252 
253  if (version >= 19)
254  {
255  if (version < 25)
256  {
257  std::vector<std::map<double, double>> raw_clearances;
258  in >> raw_clearances;
259  ipp.clearance.resize(
260  raw_clearances.size(), raw_clearances.size());
261  for (size_t k = 0; k < raw_clearances.size(); k++)
262  ipp.clearance.get_path_clearance_decimated(k) =
263  raw_clearances[k];
264  }
265  else
266  {
267  ipp.clearance.readFromStream(in);
268  }
269  }
270  else
271  {
272  ipp.clearance.clear();
273  }
274  }
275 
277  if (version >= 20)
278  {
279  in >> WS_Obstacles_original; // v20
280  }
281  else
282  {
284  }
285 
286  if (version < 24)
287  {
288  mrpt::poses::CPose2D robotOdometryPose;
289  in >> robotOdometryPose;
290  robotPoseOdometry = robotOdometryPose.asTPose();
291  robotPoseLocalization = robotOdometryPose.asTPose();
292  }
293  else
294  {
296  }
297 
298  WS_targets_relative.clear();
299  if (version >= 8)
300  {
301  if (version >= 26)
302  {
304  }
305  else
306  {
308  in >> trg;
309  WS_targets_relative.push_back(trg);
310  }
311  }
312  else
313  {
315  in >> pos;
316  WS_targets_relative.push_back(
317  mrpt::math::TPoint2D(pos.x(), pos.y()));
318  }
319 
320  if (version >= 16)
321  {
323  if (version >= 24)
324  {
327  }
328  else
329  {
330  mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP,
331  crel_pose_PTG_origin_wrt_sense_NOP;
332  in >> crel_cur_pose_wrt_last_vel_cmd_NOP >>
333  crel_pose_PTG_origin_wrt_sense_NOP;
335  crel_cur_pose_wrt_last_vel_cmd_NOP.asTPose();
337  crel_pose_PTG_origin_wrt_sense_NOP.asTPose();
338  }
339  }
340  else
341  {
342  ptg_index_NOP = -1;
343  }
344  if (version >= 17 && version < 24)
345  {
347  }
348  if (version >= 24)
349  {
351  }
352 
353  if (version >= 10)
354  {
355  if (version >= 15)
356  {
357  if (ptg_index_NOP < 0) in >> cmd_vel;
358  }
359  else
360  {
361  std::vector<double> vel;
362  in >> vel;
363  if (vel.size() == 2)
366  else
369  for (size_t k = 0; k < cmd_vel->getVelCmdLength(); k++)
370  cmd_vel->setVelCmdElement(i, vel[k]);
371  }
372  }
373  else
374  {
375  float v, w;
376  in >> v >> w;
379  cmd_vel->setVelCmdElement(0, v);
380  cmd_vel->setVelCmdElement(0, w);
381  }
382 
383  if (version >= 15 && ptg_index_NOP < 0) in >> cmd_vel_original;
384 
385  if (version < 13)
386  {
387  float old_exec_time;
388  in >> old_exec_time;
389  values["executionTime"] = old_exec_time;
390  }
391 
392  if (version < 6)
393  {
394  mrpt::math::CVectorFloat prevV, prevW, prevSelPTG;
395 
396  // Previous values: (Removed in version 6)
397  in >> n;
398  prevV.resize(n);
399  if (n) in.ReadBufferFixEndianness(&(*prevV.begin()), n);
400 
401  in >> n;
402  prevW.resize(n);
403  if (n) in.ReadBufferFixEndianness(&(*prevW.begin()), n);
404 
405  in >> n;
406  prevSelPTG.resize(n);
407  if (n) in.ReadBufferFixEndianness(&(*prevSelPTG.begin()), n);
408  }
409 
410  in >> n;
411  robotShape_x.resize(n);
412  robotShape_y.resize(n);
413  if (n)
414  {
415  in.ReadBufferFixEndianness(&(*robotShape_x.begin()), n);
416  in.ReadBufferFixEndianness(&(*robotShape_y.begin()), n);
417  }
418 
419  if (version > 0)
420  { // Version 1 --------------
421  if (version >= 10)
422  {
423  in >> cur_vel >> cur_vel_local;
424  }
425  else
426  {
427  float actual_v, actual_w;
428  in >> actual_v >> actual_w;
429  cur_vel = mrpt::math::TTwist2D(0, 0, 0);
430  cur_vel_local =
431  mrpt::math::TTwist2D(actual_v, .0, actual_w);
432  }
433  }
434  else
435  { // Default values for old versions:
436  cur_vel = mrpt::math::TTwist2D(0, 0, 0);
437  }
438 
439  if (version < 13 && version > 1)
440  {
441  float old_estim_period;
442  in >> old_estim_period;
443  values["estimatedExecutionPeriod"] = old_estim_period;
444  }
445 
446  for (i = 0; i < infoPerPTG.size(); i++)
447  {
448  infoPerPTG[i].evalFactors.clear();
449  }
450  if (version > 2)
451  {
452  // Version 3..22 ----------
453  for (i = 0; i < infoPerPTG.size(); i++)
454  {
455  if (version < 22)
456  {
457  in >> n;
458  for (unsigned int j = 0; j < n; j++)
459  {
460  float f;
461  in >> f;
462  infoPerPTG[i].evalFactors[mrpt::format("f%u", j)] =
463  f;
464  }
465  }
466  else
467  {
468  in >> infoPerPTG[i].evalFactors;
469  }
470  }
471  }
472 
473  if (version > 3)
474  {
475  // Version 4 ----------
476  in >> nPTGs;
477  if (version < 9) // Old "securityDistances", now unused.
478  {
479  in >> n;
480  float dummy;
481  for (i = 0; i < n; i++) in >> dummy;
482  }
483  }
484  else
485  {
486  nPTGs = infoPerPTG.size();
487  }
488 
489  if (version > 4)
490  {
491  if (version < 10)
492  {
493  int32_t navigatorBehavior; // removed in v10
494  in >> navigatorBehavior;
495  }
496 
497  if (version < 6) // Removed in version 6:
498  {
499  mrpt::poses::CPoint2D doorCrossing_P1, doorCrossing_P2;
500  in >> doorCrossing_P1 >> doorCrossing_P2;
501  }
502  }
503 
504  if (version > 6 && version < 13)
505  {
507  in >> tt;
508  timestamps["tim_start_iteration"] = tt;
509  }
510 
511  if (version >= 11)
512  {
514  }
515  else
516  {
517  robotShape_radius = 0.5;
518  }
519 
520  if (version >= 12 && version < 15)
521  {
522  std::vector<std::vector<double>> dummy_cmd_vel_filterings;
523  in >> dummy_cmd_vel_filterings;
524  }
525 
526  if (version >= 13)
527  {
528  in >> values >> timestamps;
529  }
530  else
531  {
532  values.clear();
533  timestamps.clear();
534  }
535 
536  if (version >= 14)
537  {
538  if (version >= 24)
539  {
541  }
542  else
543  {
544  mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd;
545  in >> crelPoseSense >> crelPoseVelCmd;
546  relPoseSense = crelPoseSense.asTPose();
547  relPoseVelCmd = crelPoseVelCmd.asTPose();
548  }
549  }
550  else
551  {
553  }
554 
555  if (version >= 18)
557  else
558  additional_debug_msgs.clear();
559 
560  if (version >= 24)
562  else
563  {
564  navDynState =
567  if (!WS_targets_relative.empty())
569  }
570  }
571  break;
572  default:
574  };
575 }
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.
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...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3582
std::map< std::string, mrpt::system::TTimeStamp > timestamps
Known values:
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense_NOP
mrpt::math::TPose2D robotPoseOdometry
GLenum GLsizei n
Definition: glext.h:5074
mrpt::math::TPose2D relPoseSense
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
uint32_t nPTGs
The number of PTGS:
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:48
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
mrpt::math::CVectorFloat robotShape_x
The robot shape in WS.
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:441
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
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".
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::aligned_std_vector< TInfoPerPTG > infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
mrpt::math::TPose2D relTarget
Current relative target location.
mrpt::maps::CSimplePointsMap WS_Obstacles_original
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 2D point.
Definition: CPoint2D.h:33
__int32 int32_t
Definition: rptypes.h:46
const GLdouble * v
Definition: glext.h:3678
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
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:38
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel_original
Motion command as comes out from the PTG, before scaling speed limit filtering.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::math::TTwist2D cur_vel
The actual robot velocities in global (map) coordinates, as read from sensors, in "m/sec" and "rad/se...
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
mrpt::maps::CSimplePointsMap WS_Obstacles
The WS-Obstacles.
std::shared_ptr< CVehicleVelCmd > Ptr
unsigned __int32 uint32_t
Definition: rptypes.h:47
Lightweight 2D point.
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020