MRPT  1.9.9
imu.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 /*---------------------------------------------------------------
11  APPLICATION: mrpt_ros bridge
12  FILE: imu.cpp
13  AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
14  ---------------------------------------------------------------*/
15 
16 #include <mrpt/ros1bridge/imu.h>
17 
18 namespace mrpt::ros1bridge
19 {
20 bool fromROS(const sensor_msgs::Imu& msg, mrpt::obs::CObservationIMU& obj)
21 {
22  using namespace mrpt::obs;
23 
24  obj.set(IMU_ORI_QUAT_X, msg.orientation.x);
25  obj.set(IMU_ORI_QUAT_Y, msg.orientation.y);
26  obj.set(IMU_ORI_QUAT_Z, msg.orientation.z);
27  obj.set(IMU_ORI_QUAT_W, msg.orientation.w);
28 
29  obj.set(IMU_X_ACC, msg.linear_acceleration.x);
30  obj.set(IMU_Y_ACC, msg.linear_acceleration.y);
31  obj.set(IMU_Z_ACC, msg.linear_acceleration.z);
32 
33  obj.set(IMU_WX, msg.angular_velocity.x);
34  obj.set(IMU_WY, msg.angular_velocity.y);
35  obj.set(IMU_WZ, msg.angular_velocity.z);
36 
37  // NEED TO WRITE CODE FOR COVARIANCE
38  return true;
39 }
40 
41 bool toROS(
42  const mrpt::obs::CObservationIMU& obj, const std_msgs::Header& msg_header,
43  sensor_msgs::Imu& msg)
44 {
45  using namespace mrpt::obs;
46 
47  msg.header = msg_header;
48 
49  const auto& measurements = obj.rawMeasurements;
50  msg.orientation.x = measurements.at(IMU_ORI_QUAT_X);
51  msg.orientation.y = measurements.at(IMU_ORI_QUAT_Y);
52  msg.orientation.z = measurements.at(IMU_ORI_QUAT_Z);
53  msg.orientation.w = measurements.at(IMU_ORI_QUAT_W);
54 
55  /// computing acceleration in global navigation frame not in local vehicle
56  /// frame, might be the other way round
57  msg.linear_acceleration.x = measurements.at(IMU_X_ACC);
58  msg.linear_acceleration.y = measurements.at(IMU_Y_ACC);
59  msg.linear_acceleration.z = measurements.at(IMU_Z_ACC);
60 
61  msg.angular_velocity.x = measurements.at(IMU_WX);
62  msg.angular_velocity.y = measurements.at(IMU_WY);
63  msg.angular_velocity.z = measurements.at(IMU_WZ);
64 
65  // msg.angular_velocity_covariance
66  return true;
67 }
68 
69 } // namespace mrpt::ros1bridge
70 
71 /*
72 std_msgs/Header header
73 geometry_msgs/Quaternion orientation
74 float64[9] orientation_covariance
75 geometry_msgs/Vector3 angular_velocity
76 float64[9] angular_velocity_covariance
77 geometry_msgs/Vector3 linear_acceleration
78 float64[9] linear_acceleration_covariance
79  */
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
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)
void set(TIMUDataIndex idx, double value)
Sets a given data type, and mark it as present.
angular velocity - x (local/vehicle frame) (rad/sec)
This namespace contains representation of robot actions and observations.
Orientation Quaternion Y (global/navigation frame)
angular velocity - z (local/vehicle frame) (rad/sec)
Orientation Quaternion Z (global/navigation frame)
Orientation Quaternion W (global/navigation frame)
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
angular velocity - y (local/vehicle frame) (rad/sec)
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
x-axis acceleration (local/vehicle frame) (m/sec2)



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