MRPT  2.0.4
CObservationIMU.h
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 #pragma once
10 
11 #include <mrpt/math/CMatrixD.h>
12 #include <mrpt/obs/CObservation.h>
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
16 
17 namespace mrpt::obs
18 {
19 /** Symbolic names for the indices of IMU data (refer to
20  * mrpt::obs::CObservationIMU)
21  * \ingroup mrpt_obs_grp
22  */
24 {
25  /** x-axis acceleration (local/vehicle frame) (m/sec<sup>2</sup>) */
26  IMU_X_ACC = 0,
27  /** y-axis acceleration (local/vehicle frame) (m/sec<sup>2</sup>) */
29  /** z-axis acceleration (local/vehicle frame) (m/sec<sup>2</sup>) */
31  /** yaw angular velocity (local/vehicle frame) (rad/sec) */
33  /** angular velocity - z (local/vehicle frame) (rad/sec) */
35  /** pitch angular velocity (local/vehicle frame) (rad/sec) */
37  /** angular velocity - y (local/vehicle frame) (rad/sec) */
39  /** roll angular velocity (local/vehicle frame) (rad/sec) */
41  /** angular velocity - x (local/vehicle frame) (rad/sec) */
43  /** x-axis velocity (global/navigation frame) (m/sec) */
45  /** y-axis velocity (global/navigation frame) (m/sec) */
47  /** z-axis velocity (global/navigation frame) (m/sec) */
49  /** orientation yaw absolute value (global/navigation frame) (rad) */
51  /** orientation pitch absolute value (global/navigation frame) (rad) */
53  /** orientation roll absolute value (global/navigation frame) (rad) */
55  /** x absolute value (global/navigation frame) (meters) */
57  /** y absolute value (global/navigation frame) (meters) */
59  /** z absolute value (global/navigation frame) (meters) */
61  /** x magnetic field value (local/vehicle frame) (gauss) */
63  /** y magnetic field value (local/vehicle frame) (gauss) */
65  /** z magnetic field value (local/vehicle frame) (gauss) */
67  /** air pressure (Pascals) */
69  /** altitude from an altimeter (meters) */
71  /** temperature (degrees Celsius) */
73  /** Orientation Quaternion X (global/navigation frame) */
75  /** Orientation Quaternion Y (global/navigation frame) */
77  /** Orientation Quaternion Z (global/navigation frame) */
79  /** Orientation Quaternion W (global/navigation frame) */
81  /** yaw angular velocity (global/navigation frame) (rad/sec) */
83  /** pitch angular velocity (global/navigation frame) (rad/sec) */
85  /** roll angular velocity (global/navigation frame) (rad/sec) */
87  /** x-axis acceleration (global/navigation frame) (m/sec<sup>2</sup>) */
89  /** y-axis acceleration (global/navigation frame) (m/sec<sup>2</sup>) */
91  /** z-axis acceleration (global/navigation frame) (m/sec<sup>2</sup>) */
93 
94  // Always leave this last value to reflect the number of enum values
96 };
97 
98 /** This class stores measurements from an Inertial Measurement Unit (IMU)
99  * (attitude estimation, raw gyroscope and accelerometer values), altimeters or
100  * magnetometers.
101  *
102  * The order of the values in each entry of
103  * mrpt::obs::CObservationIMU::rawMeasurements is defined as symbolic names in
104  * the enum mrpt::obs::TIMUDataIndex.
105  * Check it out also for reference on the unit and the coordinate frame used
106  * for each value.
107  *
108  * \sa CObservation
109  * \ingroup mrpt_obs_grp
110  */
112 {
114 
115  public:
116  CObservationIMU() = default;
117  ~CObservationIMU() override = default;
118 
119  /** The pose of the sensor on the robot. */
121 
122  /** Each entry in this vector is true if the corresponding data index
123  * contains valid data (the IMU unit supplies that kind of data).
124  * See the top of this page for the meaning of the indices.
125  */
126  std::vector<bool> dataIsPresent =
127  std::vector<bool>(mrpt::obs::COUNT_IMU_DATA_FIELDS, false);
128 
129  /** The accelerometer and/or gyroscope measurements taken by the IMU at the
130  * given timestamp.
131  * \sa dataIsPresent, CObservation::timestamp
132  */
133  std::vector<double> rawMeasurements =
134  std::vector<double>(mrpt::obs::COUNT_IMU_DATA_FIELDS, 0);
135 
136  /** Sets a given data type, and mark it as present. \sa has(), set() */
137  void set(TIMUDataIndex idx, double value)
138  {
139  rawMeasurements.at(idx) = value;
140  dataIsPresent.at(idx) = true;
141  }
142 
143  /** Gets a given data type, throws if not set. \sa has(), get() */
144  double get(TIMUDataIndex idx) const
145  {
146  ASSERTMSG_(dataIsPresent.at(idx), "Trying to access non-set value");
147  return rawMeasurements.at(idx);
148  }
149  /** Returns true if the given data type is set. \sa set(), get() */
150  bool has(TIMUDataIndex idx) const { return dataIsPresent.at(idx); }
151 
152  // See base class docs
153  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
154  {
155  out_sensorPose = sensorPose;
156  }
157  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
158  {
159  sensorPose = newSensorPose;
160  }
161  void getDescriptionAsText(std::ostream& o) const override;
162 
163 }; // End of class def.
164 
165 } // namespace mrpt::obs
x-axis acceleration (global/navigation frame) (m/sec2)
z-axis acceleration (global/navigation frame) (m/sec2)
yaw angular velocity (global/navigation frame) (rad/sec)
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
std::vector< bool > dataIsPresent
Each entry in this vector is true if the corresponding data index contains valid data (the IMU unit s...
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
orientation pitch absolute value (global/navigation frame) (rad)
temperature (degrees Celsius)
x magnetic field value (local/vehicle frame) (gauss)
bool has(TIMUDataIndex idx) const
Returns true if the given data type is set.
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)
roll angular velocity (global/navigation frame) (rad/sec)
pitch angular velocity (local/vehicle frame) (rad/sec)
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
angular velocity - x (local/vehicle frame) (rad/sec)
This namespace contains representation of robot actions and observations.
pitch angular velocity (global/navigation frame) (rad/sec)
Orientation Quaternion Y (global/navigation frame)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
angular velocity - z (local/vehicle frame) (rad/sec)
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)
angular velocity - y (local/vehicle frame) (rad/sec)
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
y-axis acceleration (global/navigation frame) (m/sec2)
air pressure (Pascals)
z absolute value (global/navigation frame) (meters)
TIMUDataIndex
Symbolic names for the indices of IMU data (refer to mrpt::obs::CObservationIMU)
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
~CObservationIMU() override=default
orientation yaw absolute value (global/navigation frame) (rad)
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)
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
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)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020