Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
14 #include <mrpt/utils/CStream.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 
46 /*---------------------------------------------------------------
47  writeToStream
48  ---------------------------------------------------------------*/
50 {
51  if (version)
52  *version = 26;
53  else
54  {
55  uint32_t i,n;
56 
57  // Version 0 ---------
58  n = infoPerPTG.size();
59  out << n;
60  for (i=0;i<n;i++)
61  {
62  out << infoPerPTG[i].PTG_desc.c_str();
63 
64  uint32_t m = infoPerPTG[i].TP_Obstacles.size();
65  out << m;
66  if (m) out.WriteBuffer((const void*)&(*infoPerPTG[i].TP_Obstacles.begin()), m * sizeof(infoPerPTG[i].TP_Obstacles[0]));
67 
68  out << infoPerPTG[i].TP_Targets; // v8: CPoint2D -> TPoint2D. v26: vector
69  out << infoPerPTG[i].TP_Robot; // v17
70  out << infoPerPTG[i].timeForTPObsTransformation << infoPerPTG[i].timeForHolonomicMethod; // made double in v12
71  out << infoPerPTG[i].desiredDirection << infoPerPTG[i].desiredSpeed << infoPerPTG[i].evaluation; // made double in v12
72  // removed in v23: out << evaluation_org << evaluation_priority; // added in v21
73  out << infoPerPTG[i].HLFR;
74 
75  // Version 9: Removed security distances. Added optional field with PTG info.
76  const bool there_is_ptg_data = infoPerPTG[i].ptg.present();
77  out << there_is_ptg_data;
78  if (there_is_ptg_data)
79  out << infoPerPTG[i].ptg;
80 
81  // Was: out << infoPerPTG[i].clearance.raw_clearances; // v19
82  infoPerPTG[i].clearance.writeToStream(out); // v25
83  }
84  out << nSelectedPTG << WS_Obstacles;
85  out << WS_Obstacles_original; // v20
86 
87  // Removed v24: out << robotOdometryPose;
89 
90  out << WS_targets_relative; //v8, v26: vector
91  // v16:
92  out << ptg_index_NOP << ptg_last_k_NOP;
94 
95  // Removed: v24. out << ptg_last_curRobotVelLocal; // v17
97 
98  if (ptg_index_NOP<0)
99  out << cmd_vel /*v10*/ << cmd_vel_original; // v15
100 
101  // Previous values: REMOVED IN VERSION #6
102  n = robotShape_x.size();
103  out << n;
104  if (n) {
105  out.WriteBuffer((const void*)&(*robotShape_x.begin()), n*sizeof(robotShape_x[0]));
106  out.WriteBuffer((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  out << infoPerPTG[i].evalFactors; // v22: this is now a TParameters
116  }
117 
118  out << nPTGs; // v4
119  // out << timestamp; // removed v13
120  out << robotShape_radius; // v11
121  //out << cmd_vel_filterings; // added v12: Removed in v15
122 
123  out << values << timestamps; // v13
124 
125  out << relPoseSense << relPoseVelCmd; // v14, v24 changed CPose2D->TPose2D
126 
127  // v15: cmd_vel converted from std::vector<double> into CSerializable
128  out << additional_debug_msgs; // v18
129 
130  navDynState.writeToStream(out); // v24
131  }
132 }
133 
134 /*---------------------------------------------------------------
135  readFromStream
136  ---------------------------------------------------------------*/
138 {
139  switch(version)
140  {
141  case 0:
142  case 1:
143  case 2:
144  case 3:
145  case 4:
146  case 5:
147  case 6:
148  case 7:
149  case 8:
150  case 9:
151  case 10:
152  case 11:
153  case 12:
154  case 13:
155  case 14:
156  case 15:
157  case 16:
158  case 17:
159  case 18:
160  case 19:
161  case 20:
162  case 21:
163  case 22:
164  case 23:
165  case 24:
166  case 25:
167  case 26:
168  {
169  // Version 0 --------------
170  uint32_t i,n;
171 
172  infoPerPTG.clear();
173 
174  in >> n;
175  infoPerPTG.resize(n);
176  for (i=0;i<n;i++)
177  {
178  auto &ipp = infoPerPTG[i];
179  char str[256];
180  in >> str;
181  ipp.PTG_desc = std::string(str);
182 
183  int32_t m;
184  in >> m;
185  ipp.TP_Obstacles.resize(m);
186  if (m) in.ReadBufferFixEndianness( &(*ipp.TP_Obstacles.begin()), m );
187 
188  ipp.TP_Targets.clear();
189  if (version >= 8)
190  {
191  if (version >= 26) {
192  in >> ipp.TP_Targets;
193  }
194  else {
196  in >> trg;
197  ipp.TP_Targets.push_back(trg);
198  }
199  }
200  else
201  {
203  in >> pos;
204  ipp.TP_Targets.push_back(mrpt::math::TPoint2D(pos));
205  }
206  if (version >= 17)
207  in >> ipp.TP_Robot;
208  else ipp.TP_Robot = mrpt::math::TPoint2D(0, 0);
209 
210  if (version>=12) {
211  in >> ipp.timeForTPObsTransformation >> ipp.timeForHolonomicMethod;
212  in >> ipp.desiredDirection >> ipp.desiredSpeed >> ipp.evaluation;
213  } else {
214  in.ReadAsAndCastTo<float,double>(ipp.timeForTPObsTransformation);
215  in.ReadAsAndCastTo<float,double>(ipp.timeForHolonomicMethod);
216  in.ReadAsAndCastTo<float,double>(ipp.desiredDirection);
217  in.ReadAsAndCastTo<float,double>(ipp.desiredSpeed);
218  in.ReadAsAndCastTo<float,double>(ipp.evaluation);
219  }
220  if (version >= 21 && version <23) {
221  double evaluation_org, evaluation_priority;
222  in >> evaluation_org >> evaluation_priority;
223  }
224 
225  in >> ipp.HLFR;
226 
227  if (version>=9) // Extra PTG info
228  {
229  ipp.ptg.clear();
230 
231  bool there_is_ptg_data;
232  in >> there_is_ptg_data;
233  if (there_is_ptg_data)
234  ipp.ptg = mrpt::nav::CParameterizedTrajectoryGeneratorPtr( in.ReadObject() );
235  }
236 
237  if (version >= 19)
238  {
239  if (version < 25) {
240  std::vector<std::map<double,double> > raw_clearances;
241  in >> raw_clearances;
242  ipp.clearance.resize(raw_clearances.size(), raw_clearances.size());
243  for (size_t i = 0; i < raw_clearances.size(); i++)
244  ipp.clearance.get_path_clearance_decimated(i) = raw_clearances[i];
245  }
246  else {
247  ipp.clearance.readFromStream(in);
248  }
249  }
250  else {
251  ipp.clearance.clear();
252  }
253  }
254 
256  if (version >= 20) {
257  in >> WS_Obstacles_original; // v20
258  }
259  else {
261  }
262 
263  if (version < 24) {
264  mrpt::poses::CPose2D robotOdometryPose;
265  in >> robotOdometryPose;
266  robotPoseOdometry = robotOdometryPose;
267  robotPoseLocalization = robotOdometryPose;
268  }
269  else
270  {
272  }
273 
274  WS_targets_relative.clear();
275  if (version >= 8)
276  {
277  if (version >= 26)
278  {
280  }
281  else
282  {
284  in >> trg;
285  WS_targets_relative.push_back(trg);
286  }
287  }
288  else
289  {
291  in >> pos;
293  }
294 
295  if (version >= 16) {
297  if (version >= 24) {
299  }
300  else
301  {
302  mrpt::poses::CPose2D crel_cur_pose_wrt_last_vel_cmd_NOP,crel_pose_PTG_origin_wrt_sense_NOP;
303  in >> crel_cur_pose_wrt_last_vel_cmd_NOP >> crel_pose_PTG_origin_wrt_sense_NOP;
304  rel_cur_pose_wrt_last_vel_cmd_NOP = crel_cur_pose_wrt_last_vel_cmd_NOP;
305  rel_pose_PTG_origin_wrt_sense_NOP = crel_pose_PTG_origin_wrt_sense_NOP;
306  }
307  }
308  else {
309  ptg_index_NOP = -1;
310  }
311  if (version >= 17 && version < 24) {
313  }
314  if (version >= 24) {
316  }
317 
318  if (version >= 10) {
319  if (version >= 15) {
320  if (ptg_index_NOP<0)
321  in >> cmd_vel;
322  }
323  else {
324  std::vector<double> vel;
325  in >> vel;
326  if (vel.size() == 2)
327  cmd_vel = mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_DiffDriven);
328  else cmd_vel = mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_Holo);
329  for (size_t i = 0; i < cmd_vel->getVelCmdLength(); i++)
330  cmd_vel->setVelCmdElement(i, vel[i]);
331  }
332  }
333  else {
334  float v, w;
335  in >> v >> w;
336  cmd_vel = mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_DiffDriven);
337  cmd_vel->setVelCmdElement(0, v);
338  cmd_vel->setVelCmdElement(0, w);
339  }
340 
341  if (version>=15 && ptg_index_NOP<0)
342  in >> cmd_vel_original;
343 
344  if (version < 13) {
345  float old_exec_time; in >> old_exec_time;
346  values["executionTime"] = old_exec_time;
347  }
348 
349  if (version<6)
350  {
351  mrpt::math::CVectorFloat prevV,prevW,prevSelPTG;
352 
353  // Previous values: (Removed in version 6)
354  in >> n;
355  prevV.resize(n);
356  if (n) in.ReadBufferFixEndianness( &(*prevV.begin()),n);
357 
358  in >> n;
359  prevW.resize(n);
360  if (n) in.ReadBufferFixEndianness( &(*prevW.begin()),n);
361 
362  in >> n;
363  prevSelPTG.resize(n);
364  if (n) in.ReadBufferFixEndianness( &(*prevSelPTG.begin()),n);
365  }
366 
367  in >> n;
368  robotShape_x.resize(n);
369  robotShape_y.resize(n);
370  if (n) {
371  in.ReadBufferFixEndianness( &(*robotShape_x.begin()), n);
372  in.ReadBufferFixEndianness( &(*robotShape_y.begin()), n);
373  }
374 
375  if (version > 0)
376  { // Version 1 --------------
377  if (version >= 10) {
378  in >> cur_vel >> cur_vel_local;
379  }
380  else {
381  float actual_v, actual_w;
382  in >> actual_v >> actual_w;
383  cur_vel = mrpt::math::TTwist2D(0,0,0);
384  cur_vel_local= mrpt::math::TTwist2D(actual_v, .0, actual_w );
385  }
386  }
387  else
388  { // Default values for old versions:
389  cur_vel = mrpt::math::TTwist2D(0,0,0);
390  }
391 
392  if (version < 13 && version>1) {
393  float old_estim_period; in >> old_estim_period;
394  values["estimatedExecutionPeriod"] = old_estim_period;
395  }
396 
397  for (i = 0; i < infoPerPTG.size(); i++) {
398  infoPerPTG[i].evalFactors.clear();
399  }
400  if (version > 2)
401  {
402  // Version 3..22 ----------
403  for (i=0;i<infoPerPTG.size();i++)
404  {
405  if (version < 22) {
406  in >> n;
407  for (unsigned int j = 0; j < n; j++) {
408  float f;
409  in >> f;
410  infoPerPTG[i].evalFactors[mrpt::format("f%u", j)] = f;
411  }
412  }
413  else {
414  in >> infoPerPTG[i].evalFactors;
415  }
416  }
417 
418  }
419 
420  if (version > 3)
421  {
422  // Version 4 ----------
423  in >> nPTGs;
424  if (version <9) // Old "securityDistances", now unused.
425  {
426  in >> n;
427  float dummy;
428  for (i=0;i<n;i++)
429  in >> dummy;
430  }
431  }
432  else
433  {
434  nPTGs = infoPerPTG.size();
435  }
436 
437  if (version > 4)
438  {
439  if (version < 10)
440  {
441  int32_t navigatorBehavior; // removed in v10
442  in >> navigatorBehavior;
443  }
444 
445  if (version<6) // Removed in version 6:
446  {
447  mrpt::poses::CPoint2D doorCrossing_P1,doorCrossing_P2;
448  in >> doorCrossing_P1 >> doorCrossing_P2;
449  }
450  }
451 
452  if (version>6 && version<13) {
453  mrpt::system::TTimeStamp tt; in >> tt;
454  timestamps["tim_start_iteration"] = tt;
455  }
456 
457  if (version>=11) {
459  } else {
460  robotShape_radius = 0.5;
461  }
462 
463  if (version >= 12 && version<15) {
464  std::vector<std::vector<double> > dummy_cmd_vel_filterings;
465  in >> dummy_cmd_vel_filterings;
466  }
467 
468  if (version >= 13) {
469  in >> values >> timestamps;
470  }
471  else {
472  values.clear();
473  timestamps.clear();
474  }
475 
476  if (version >= 14) {
477  if (version >= 24) {
479  }
480  else {
481  mrpt::poses::CPose2D crelPoseSense, crelPoseVelCmd;
482  in >> crelPoseSense >> crelPoseVelCmd;
483  relPoseSense = crelPoseSense;
484  relPoseVelCmd = crelPoseVelCmd;
485  }
486  }
487  else {
489  }
490 
491  if (version>=18)
493  else additional_debug_msgs.clear();
494 
495  if (version>=24)
497  else {
500  if (!WS_targets_relative.empty())
502  }
503 
504 
505  } break;
506  default:
508 
509  };
510 
511 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP
int32_t nSelectedPTG
The selected PTG.
std::vector< mrpt::math::TPose2D > WS_targets_relative
Relative poses (wrt to robotPoseLocalization) for extrapolated paths at two instants: time of obstacl...
mrpt::math::CVectorFloat robotShape_y
The robot shape in WS. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Polygonal.
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 readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3535
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
The robot pose (from odometry and from the localization/SLAM system).
GLenum GLsizei n
Definition: glext.h:4618
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
mrpt::math::TPose2D relPoseSense
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
uint32_t nPTGs
The number of PTGS:
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
A class for storing, saving and loading a reactive navigation log record for the CReactiveNavigationS...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
mrpt::math::CVectorFloat robotShape_x
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState ptg_last_navDynState
mrpt::math::TPose2D robotPoseLocalization
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
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::kinematics::CVehicleVelCmdPtr cmd_vel_original
Motion command as comes out from the PTG, before scaling speed limit filtering.
mrpt::kinematics::CVehicleVelCmdPtr cmd_vel
The final motion command sent to robot, in "m/sec" and "rad/sec".
mrpt::aligned_containers< TInfoPerPTG >::vector_t infoPerPTG
The info for each applied PTG: must contain "nPTGs * nSecDistances" elements.
int version
Definition: mrpt_jpeglib.h:898
mrpt::maps::CSimplePointsMap WS_Obstacles_original
The WS-Obstacles.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
A class used to store a 2D point.
Definition: CPoint2D.h:36
__int32 int32_t
Definition: rptypes.h:48
const GLdouble * v
Definition: glext.h:3603
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:36
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:6301
Lightweight 2D pose.
mrpt::maps::CSimplePointsMap WS_Obstacles
unsigned __int32 uint32_t
Definition: rptypes.h:49
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. Used by PTGs derived from mrpt::nav::CPTG_RobotShape_Circular.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019