GPS.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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: GPS.cpp
        AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
  ---------------------------------------------------------------*/

#include "mrpt_bridge/GPS.h"


namespace mrpt_bridge
{
    /************************************************************************
    *                                           ros2mrpt                                                                *
    ************************************************************************/
    bool GPS::ros2mrpt(const sensor_msgs::NavSatFix &msg,
                  CObservationGPS &obj)
    {
        mrpt::obs::gnss::Message_NMEA_GGA gga;
        gga.fields.altitude_meters      = msg.altitude;
        gga.fields.latitude_degrees     = msg.latitude;
        gga.fields.longitude_degrees    = msg.longitude;

        switch(msg.status.status)
        {
            case -1:
                gga.fields.fix_quality  = 0;
                break;
            case 0:
                gga.fields.fix_quality  = 1;
                break;
            case 2:
                gga.fields.fix_quality  = 2;
                break;
            case 1:
                gga.fields.fix_quality  = 3;
                break;
            default:
                gga.fields.fix_quality  = 0; // never going to execute default

        }
        obj.setMsg(gga);
    }

    /************************************************************************
    *                                           mrpt2ros                                                                *
    ************************************************************************/
    bool GPS::mrpt2ros(const CObservationGPS &obj,
                  const std_msgs::Header &msg_header,
                  sensor_msgs::NavSatFix &msg)
    {
        // 1) sensor_msgs::NavSatFix:: header
        msg.header = msg_header;

        // 2) other NavSatFix Parameters, the following 3 could be wrong too

        if (obj.hasMsgClass<mrpt::obs::gnss::Message_NMEA_GGA>())
        {
            const mrpt::obs::gnss::Message_NMEA_GGA &gga =
                    obj.getMsgByClass<mrpt::obs::gnss::Message_NMEA_GGA>();
            msg.altitude    = gga.fields.altitude_meters;
            msg.latitude    = gga.fields.latitude_degrees;
            msg.longitude   = gga.fields.longitude_degrees;

            switch(gga.fields.fix_quality)
            {
                case 0:
                    msg.status.status = -1;
                    break;
                case 1:
                    msg.status.status = 0;
                    break;
                case 2:
                    msg.status.status = 2;
                    break;
                case 3:
                    msg.status.status = 1;
                    break;
                default:
                    // this is based on literature available on GPS as the number of types in ROS and MRPT are not same
                    msg.status.status = 0;
            }
            // this might be incorrect as there is not matching field in mrpt message type
            msg.status.service = 1;

        }
    }
}


/*
uint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header header
sensor_msgs/NavSatStatus status
float64 latitude
float64 longitude
float64 altitude
float64[9] position_covariance
uint8 position_covariance_type
*/

/*
int8 STATUS_NO_FIX=-1
int8 STATUS_FIX=0
int8 STATUS_SBAS_FIX=1
int8 STATUS_GBAS_FIX=2
uint16 SERVICE_GPS=1
uint16 SERVICE_GLONASS=2
uint16 SERVICE_COMPASS=4
uint16 SERVICE_GALILEO=8
int8 status
uint16 service
 */