imu.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

/*---------------------------------------------------------------
        APPLICATION: mrpt_ros bridge
        FILE: imu.cpp
        AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
  ---------------------------------------------------------------*/

#include "mrpt_bridge/imu.h"

using namespace std;

namespace mrpt_bridge
{
/************************************************************************
 *                                              ros2mrpt                                                                *
 ************************************************************************/
bool imu::ros2mrpt(const sensor_msgs::Imu& msg, CObservationIMU obj)
{
        obj.rawMeasurements.at(IMU_ORI_QUAT_X) = msg.orientation.x;
        obj.rawMeasurements.at(IMU_ORI_QUAT_Y) = msg.orientation.y;
        obj.rawMeasurements.at(IMU_ORI_QUAT_Z) = msg.orientation.z;
        obj.rawMeasurements.at(IMU_ORI_QUAT_W) = msg.orientation.w;

        obj.rawMeasurements.at(IMU_X_ACC_GLOBAL) = msg.linear_acceleration.x;
        obj.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = msg.linear_acceleration.y;
        obj.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = msg.linear_acceleration.z;

        obj.rawMeasurements.at(IMU_X_VEL) = msg.angular_velocity.x;
        obj.rawMeasurements.at(IMU_Y_VEL) = msg.angular_velocity.y;
        obj.rawMeasurements.at(IMU_Z_VEL) = msg.angular_velocity.z;

        // NEED TO WRITE CODE FOR COVARIANCE
        return true;
}

/************************************************************************
 *                                              mrpt2ros                                                                *
 ************************************************************************/
bool imu::mrpt2ros(
        const CObservationIMU& obj, const std_msgs::Header& msg_header,
        sensor_msgs::Imu& msg)
{
        msg.header = msg_header;

        vector<double> measurements = obj.rawMeasurements;
        msg.orientation.x = measurements.at(IMU_ORI_QUAT_X);
        msg.orientation.y = measurements.at(IMU_ORI_QUAT_Y);
        msg.orientation.z = measurements.at(IMU_ORI_QUAT_Z);
        msg.orientation.w = measurements.at(IMU_ORI_QUAT_W);

        msg.linear_acceleration.x = measurements.at(IMU_X_ACC_GLOBAL);
        msg.linear_acceleration.y = measurements.at(IMU_Y_ACC_GLOBAL);
        msg.linear_acceleration.z = measurements.at(IMU_Z_ACC_GLOBAL);

        msg.angular_velocity.x = measurements.at(IMU_X_VEL);
        msg.angular_velocity.y = measurements.at(IMU_Y_VEL);
        msg.angular_velocity.z = measurements.at(IMU_Z_VEL);

        // msg.angular_velocity_covariance
        return true;
}

}  // namespace mrpt_bridge

/*
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance
 */