Main MRPT website > C++ reference for MRPT 1.5.6
CObservationIMU.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 "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/utils/CStream.h>
14 //#include <mrpt/math/CMatrixD.h>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::utils;
18 using namespace mrpt::poses;
19 
20 // This must be added to any CSerializable class implementation file.
22 
23 /*---------------------------------------------------------------
24  Implements the writing to a CStream capability of CSerializable objects
25  ---------------------------------------------------------------*/
26 void CObservationIMU::writeToStream(mrpt::utils::CStream &out, int *version) const
27 {
28  if (version)
29  *version = 3; // v1->v2 was only done to fix a bug in the ordering of YAW/PITCH/ROLL rates.
30  else
31  {
32  out << sensorPose
33  << dataIsPresent
34  << timestamp;
35 
36  out << rawMeasurements;
37  // Version 3: Added 6 new raw measurements (IMU_MAG_X=15 to IMU_TEMPERATURE=20)
38 
39  out << sensorLabel;
40  }
41 }
42 
43 /*---------------------------------------------------------------
44  Implements the reading from a CStream capability of CSerializable objects
45  ---------------------------------------------------------------*/
47 {
48  switch(version)
49  {
50  case 0:
51  case 1:
52  case 2:
53  case 3:
54  in >> sensorPose;
55  in >> dataIsPresent;
56 
57  in >> timestamp;
58 
59  // In version 0 it was a vector of floats:
60  if (version<1)
61  {
63  in >> tmp;
64  rawMeasurements.resize(tmp.size());
65  for (size_t i=0;i<rawMeasurements.size();i++)
66  rawMeasurements[i] = tmp[i];
67  }
68  else
69  {
70  in >> rawMeasurements;
71  }
72 
73  if (version<2)
74  {
75  // A bug in the grabbing from XSens IMU's made /ROLL rates to be stored in the wrong order:
76  std::swap(rawMeasurements[IMU_YAW_VEL],rawMeasurements[IMU_ROLL_VEL]);
77  }
78  else
79  {
80  // v2: nothing to do, data is already in the right order.
81  }
82 
83  in >> sensorLabel;
84 
85  // Fill new entries with default values:
86  if (dataIsPresent.size()<COUNT_IMU_DATA_FIELDS)
87  {
88  const size_t nOld = dataIsPresent.size();
89  ASSERT_(rawMeasurements.size()==dataIsPresent.size());
90 
91  dataIsPresent.resize(COUNT_IMU_DATA_FIELDS);
92  rawMeasurements.resize(COUNT_IMU_DATA_FIELDS);
93  for (size_t i=nOld;i<COUNT_IMU_DATA_FIELDS;i++) {
94  dataIsPresent[i]=false;
95  rawMeasurements[i]=0;
96  }
97  }
98  break;
99  default:
101  };
102 }
103 
104 void CObservationIMU::getDescriptionAsText(std::ostream &o) const
105 {
106  using namespace std;;
108 
109  o << "Sensor pose on the robot: " << sensorPose << endl;
110 
111  o << format("Orientation (degrees): (yaw,pitch,roll)=(%.06f, %.06f, %.06f)\n\n",
112  RAD2DEG( rawMeasurements[IMU_YAW] ),
113  RAD2DEG( rawMeasurements[IMU_PITCH] ),
114  RAD2DEG( rawMeasurements[IMU_ROLL] ) );
115 
116  // Units:
117  // Use "COUNT_IMU_DATA_FIELDS" so a compile error happens if the sizes don't fit ;-)
118  static const char * imu_units[ mrpt::obs::COUNT_IMU_DATA_FIELDS ] =
119  {
120  "m/s^2", // IMU_X_ACC,
121  "m/s^2", // IMU_Y_ACC,
122  "m/s^2", // IMU_Z_ACC,
123  "rad/s", // IMU_YAW_VEL,
124  "rad/s", // IMU_PITCH_VEL,
125  "rad/s", // IMU_ROLL_VEL,
126  "m/s", // IMU_X_VEL,
127  "m/s", // IMU_Y_VEL,
128  "m/s", // IMU_Z_VEL,
129  "rad", // IMU_YAW,
130  "rad", // IMU_PITCH,
131  "rad", // IMU_ROLL,
132  "m", // IMU_X,
133  "m", // IMU_Y,
134  "m", // IMU_Z
135  "gauss", // IMU_MAG_X,
136  "gauss", // IMU_MAG_Y,
137  "gauss", // IMU_MAG_Z,
138  "Pa", // IMU_PRESSURE,
139  "m", // IMU_ALTITUDE,
140  "deg.", // IMU_TEMPERATURE,
141  "qx", // IMU_ORI_QUAT_X,
142  "qy", // IMU_ORI_QUAT_Y,
143  "qz", // IMU_ORI_QUAT_Z,
144  "qw", // IMU_ORI_QUAT_W,
145  "rad/s", // IMU_YAW_VEL_GLOBAL
146  "rad/s", // IMU_PITCH_VEL_GLOBAL
147  "rad/s", // IMU_ROLL_VEL_GLOBAL
148  "m/s^2", // IMU_X_ACC_GLOBAL
149  "m/s^2", // IMU_Y_ACC_GLOBAL
150  "m/s^2" // IMU_Z_ACC_GLOBAL
151  };
152 
153 #define DUMP_IMU_DATA(x) \
154  o << format("%15s = ",#x); \
155  if (dataIsPresent[x]) \
156  o << format("%10f %s\n", rawMeasurements[x], imu_units[x]); \
157  else o << "(not present)\n";
158 
168  DUMP_IMU_DATA(IMU_YAW)
169  DUMP_IMU_DATA(IMU_PITCH)
170  DUMP_IMU_DATA(IMU_ROLL)
190 
191 
192 }
x-axis acceleration (global/navigation frame) (m/sec2)
z-axis acceleration (global/navigation frame) (m/sec2)
yaw angular velocity (global/navigation frame) (rad/sec)
GLvoid *typedef void(GLAPIENTRY *PFNGLGETVERTEXATTRIBDVPROC)(GLuint
Definition: glew.h:1745
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
orientation pitch absolute value (global/navigation frame) (rad)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
temperature (degrees Celsius)
x magnetic field value (local/vehicle frame) (gauss)
y-axis acceleration (local/vehicle frame) (m/sec2)
Orientation Quaternion X (global/navigation frame)
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation, raw gyroscope and accelerometer values), altimeters or magnetometers.
z-axis acceleration (local/vehicle frame) (m/sec2)
x-axis velocity (global/navigation frame) (m/sec)
GLuint in
Definition: glew.h:7146
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
roll angular velocity (global/navigation frame) (rad/sec)
pitch angular velocity (local/vehicle frame) (rad/sec)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
pitch angular velocity (global/navigation frame) (rad/sec)
#define DUMP_IMU_DATA(x)
Orientation Quaternion Y (global/navigation frame)
int version
Definition: mrpt_jpeglib.h:898
Orientation Quaternion Z (global/navigation frame)
z magnetic field value (local/vehicle frame) (gauss)
Orientation Quaternion W (global/navigation frame)
y absolute value (global/navigation frame) (meters)
y magnetic field value (local/vehicle frame) (gauss)
#define RAD2DEG
Declares a class that represents any robot's observation.
y-axis acceleration (global/navigation frame) (m/sec2)
#define ASSERT_(f)
air pressure (Pascals)
z absolute value (global/navigation frame) (meters)
orientation yaw absolute value (global/navigation frame) (rad)
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...
y-axis velocity (global/navigation frame) (m/sec)
orientation roll absolute value (global/navigation frame) (rad)
yaw angular velocity (local/vehicle frame) (rad/sec)
x absolute value (global/navigation frame) (meters)
roll angular velocity (local/vehicle frame) (rad/sec)
x-axis acceleration (local/vehicle frame) (m/sec2)
z-axis velocity (global/navigation frame) (m/sec)
altitude from an altimeter (meters)
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018