mrpt_bridge: imu.cpp Source File

Go to the documentation of this file.
00001 /* +------------------------------------------------------------------------+
00002    |                     Mobile Robot Programming Toolkit (MRPT)            |
00003    |                          http://www.mrpt.org/                          |
00004    |                                                                        |
00005    | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
00006    | See: http://www.mrpt.org/Authors - All rights reserved.                |
00007    | Released under BSD License. See details in http://www.mrpt.org/License |
00008    +------------------------------------------------------------------------+ */
00009 
00010 /*---------------------------------------------------------------
00011         APPLICATION: mrpt_ros bridge
00012         FILE: imu.cpp
00013         AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
00014   ---------------------------------------------------------------*/
00015 
00016 #include "mrpt_bridge/imu.h"
00017 
00018 using namespace std;
00019 
00020 namespace mrpt_bridge
00021 {
00022     /************************************************************************
00023     *                                           ros2mrpt                                                                *
00024     ************************************************************************/
00025     bool imu::ros2mrpt(const sensor_msgs::Imu &msg, CObservationIMU obj)
00026     {
00027         obj.rawMeasurements.at(IMU_ORI_QUAT_X)  = msg.orientation.x;
00028         obj.rawMeasurements.at(IMU_ORI_QUAT_Y)  = msg.orientation.y;
00029         obj.rawMeasurements.at(IMU_ORI_QUAT_Z)  = msg.orientation.z;
00030         obj.rawMeasurements.at(IMU_ORI_QUAT_W)  = msg.orientation.w;
00031 
00032         obj.rawMeasurements.at(IMU_X_ACC_GLOBAL) = msg.linear_acceleration.x;
00033         obj.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = msg.linear_acceleration.y;
00034         obj.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = msg.linear_acceleration.z;
00035 
00036         obj.rawMeasurements.at(IMU_X_VEL)   = msg.angular_velocity.x;
00037         obj.rawMeasurements.at(IMU_Y_VEL)   = msg.angular_velocity.y;
00038         obj.rawMeasurements.at(IMU_Z_VEL)   = msg.angular_velocity.z;
00039 
00040         // NEED TO WRITE CODE FOR COVARIANCE
00041     }
00042 
00043     /************************************************************************
00044     *                                           mrpt2ros                                                                *
00045     ************************************************************************/
00046     bool imu::mrpt2ros(const CObservationIMU &obj, const std_msgs::Header &msg_header, sensor_msgs::Imu &msg)
00047     {
00048         msg.header = msg_header;
00049 
00050         vector<double> measurements = obj.rawMeasurements;
00051         msg.orientation.x   = measurements.at(IMU_ORI_QUAT_X);
00052         msg.orientation.y   = measurements.at(IMU_ORI_QUAT_Y);
00053         msg.orientation.z   = measurements.at(IMU_ORI_QUAT_Z);
00054         msg.orientation.w   = measurements.at(IMU_ORI_QUAT_W);
00055 
00056 
00058         msg.linear_acceleration.x   = measurements.at(IMU_X_ACC_GLOBAL);
00059         msg.linear_acceleration.y   = measurements.at(IMU_Y_ACC_GLOBAL);
00060         msg.linear_acceleration.z   = measurements.at(IMU_Z_ACC_GLOBAL);
00061 
00062         msg.angular_velocity.x      = measurements.at(IMU_X_VEL);
00063         msg.angular_velocity.y      = measurements.at(IMU_Y_VEL);
00064         msg.angular_velocity.z      = measurements.at(IMU_Z_VEL);
00065 
00067         msg.angular_velocity_covariance[0,0] = measurements.at(IMU_X_VEL);
00068         msg.angular_velocity_covariance[1,1] = measurements.at(IMU_Y_VEL);
00069         msg.angular_velocity_covariance[2,2] = measurements.at(IMU_Z_VEL);
00070 
00071         msg.linear_acceleration_covariance[0,0] = measurements.at(IMU_X_ACC_GLOBAL);
00072         msg.linear_acceleration_covariance[1,1] = measurements.at(IMU_Y_ACC_GLOBAL);
00073         msg.linear_acceleration_covariance[2,2] = measurements.at(IMU_Z_ACC_GLOBAL);
00074 
00075         /*msg.orientation_covariance[0,0]         = measurements.at(IMU_ORI_QUAT_X);
00076         msg.orientation_covariance[1,1]         = measurements.at(IMU_ORI_QUAT_Y);
00077         msg.orientation_covariance[2,2]         = measurements.at(IMU_ORI_QUAT_Z);
00078          */
00079     }
00080 
00081 }
00082 
00083 
00084 /*
00085 std_msgs/Header header
00086 geometry_msgs/Quaternion orientation
00087 float64[9] orientation_covariance
00088 geometry_msgs/Vector3 angular_velocity
00089 float64[9] angular_velocity_covariance
00090 geometry_msgs/Vector3 linear_acceleration
00091 float64[9] linear_acceleration_covariance
00092  */