imu.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 /*---------------------------------------------------------------
11  APPLICATION: mrpt_ros bridge
12  FILE: imu.cpp
13  AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
14  ---------------------------------------------------------------*/
15 
16 #include "mrpt_bridge/imu.h"
17 
18 using namespace std;
19 
20 namespace mrpt_bridge
21 {
22 /************************************************************************
23  * ros2mrpt *
24  ************************************************************************/
25 bool imu::ros2mrpt(const sensor_msgs::Imu& msg, CObservationIMU obj)
26 {
27  obj.rawMeasurements.at(IMU_ORI_QUAT_X) = msg.orientation.x;
28  obj.rawMeasurements.at(IMU_ORI_QUAT_Y) = msg.orientation.y;
29  obj.rawMeasurements.at(IMU_ORI_QUAT_Z) = msg.orientation.z;
30  obj.rawMeasurements.at(IMU_ORI_QUAT_W) = msg.orientation.w;
31 
32  obj.rawMeasurements.at(IMU_X_ACC_GLOBAL) = msg.linear_acceleration.x;
33  obj.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = msg.linear_acceleration.y;
34  obj.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = msg.linear_acceleration.z;
35 
36  obj.rawMeasurements.at(IMU_X_VEL) = msg.angular_velocity.x;
37  obj.rawMeasurements.at(IMU_Y_VEL) = msg.angular_velocity.y;
38  obj.rawMeasurements.at(IMU_Z_VEL) = msg.angular_velocity.z;
39 
40  // NEED TO WRITE CODE FOR COVARIANCE
41  return true;
42 }
43 
44 /************************************************************************
45  * mrpt2ros *
46  ************************************************************************/
48  const CObservationIMU& obj, const std_msgs::Header& msg_header,
49  sensor_msgs::Imu& msg)
50 {
51  msg.header = msg_header;
52 
53  vector<double> measurements = obj.rawMeasurements;
54  msg.orientation.x = measurements.at(IMU_ORI_QUAT_X);
55  msg.orientation.y = measurements.at(IMU_ORI_QUAT_Y);
56  msg.orientation.z = measurements.at(IMU_ORI_QUAT_Z);
57  msg.orientation.w = measurements.at(IMU_ORI_QUAT_W);
58 
61  msg.linear_acceleration.x = measurements.at(IMU_X_ACC_GLOBAL);
62  msg.linear_acceleration.y = measurements.at(IMU_Y_ACC_GLOBAL);
63  msg.linear_acceleration.z = measurements.at(IMU_Z_ACC_GLOBAL);
64 
65  msg.angular_velocity.x = measurements.at(IMU_X_VEL);
66  msg.angular_velocity.y = measurements.at(IMU_Y_VEL);
67  msg.angular_velocity.z = measurements.at(IMU_Z_VEL);
68 
69  // msg.angular_velocity_covariance
70  return true;
71 }
72 
73 } // namespace mrpt_bridge
74 
75 /*
76 std_msgs/Header header
77 geometry_msgs/Quaternion orientation
78 float64[9] orientation_covariance
79 geometry_msgs/Vector3 angular_velocity
80 float64[9] angular_velocity_covariance
81 geometry_msgs/Vector3 linear_acceleration
82 float64[9] linear_acceleration_covariance
83  */
mrpt_bridge::GPS::mrpt2ros
bool mrpt2ros(const CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Definition: GPS.cpp:54
imu.h
std_msgs::Header_
Definition: map.h:24
std
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types.
Definition: include/mrpt_bridge/beacon.h:52
mrpt_bridge::GPS::ros2mrpt
bool ros2mrpt(const sensor_msgs::NavSatFix &msg, CObservationGPS &obj)
Definition: GPS.cpp:23


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10