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-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 /*---------------------------------------------------------------
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  ************************************************************************/
47  bool imu::mrpt2ros(const CObservationIMU &obj, const std_msgs::Header &msg_header, sensor_msgs::Imu &msg)
48  {
49  msg.header = msg_header;
50 
51  vector<double> measurements = obj.rawMeasurements;
52  msg.orientation.x = measurements.at(IMU_ORI_QUAT_X);
53  msg.orientation.y = measurements.at(IMU_ORI_QUAT_Y);
54  msg.orientation.z = measurements.at(IMU_ORI_QUAT_Z);
55  msg.orientation.w = measurements.at(IMU_ORI_QUAT_W);
56 
57 
59  msg.linear_acceleration.x = measurements.at(IMU_X_ACC_GLOBAL);
60  msg.linear_acceleration.y = measurements.at(IMU_Y_ACC_GLOBAL);
61  msg.linear_acceleration.z = measurements.at(IMU_Z_ACC_GLOBAL);
62 
63  msg.angular_velocity.x = measurements.at(IMU_X_VEL);
64  msg.angular_velocity.y = measurements.at(IMU_Y_VEL);
65  msg.angular_velocity.z = measurements.at(IMU_Z_VEL);
66 
67  // msg.angular_velocity_covariance
68  return true;
69  }
70 
71 }
72 
73 
74 /*
75 std_msgs/Header header
76 geometry_msgs/Quaternion orientation
77 float64[9] orientation_covariance
78 geometry_msgs/Vector3 angular_velocity
79 float64[9] angular_velocity_covariance
80 geometry_msgs/Vector3 linear_acceleration
81 float64[9] linear_acceleration_covariance
82  */
bool mrpt2ros(const CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Definition: GPS.cpp:57
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types...
bool ros2mrpt(const sensor_msgs::NavSatFix &msg, CObservationGPS &obj)
Definition: GPS.cpp:24


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sat Apr 28 2018 02:49:17