gps_message_converter.py

Go to the documentation of this file.

from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import NavSatStatus
from gps_common.msg import GPSFix
from gps_common.msg import GPSStatus
 
 
def navsatfix_to_gpsfix(navsat_msg):
    # Convert sensor_msgs/NavSatFix messages to gps_common/GPSFix messages
    gpsfix_msg = GPSFix()
    gpsfix_msg.header = navsat_msg.header
    gpsfix_msg.status.status = navsat_msg.status.status
 
    gpsfix_msg.status.motion_source = GPSStatus.SOURCE_NONE
    gpsfix_msg.status.orientation_source = GPSStatus.SOURCE_NONE
    gpsfix_msg.status.position_source = GPSStatus.SOURCE_NONE
    if ((navsat_msg.status.service & NavSatStatus.SERVICE_GPS) or
            (navsat_msg.status.service & NavSatStatus.SERVICE_GLONASS) or
            (navsat_msg.status.service & NavSatStatus.SERVICE_GALILEO)):
        gpsfix_msg.status.motion_source = \
            gpsfix_msg.status.motion_source | GPSStatus.SOURCE_GPS
        gpsfix_msg.status.orientation_source = \
            gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_GPS
        gpsfix_msg.status.position_source = \
            gpsfix_msg.status.position_source | GPSStatus.SOURCE_GPS
 
    if navsat_msg.status.service & NavSatStatus.SERVICE_COMPASS:
        gpsfix_msg.status.orientation_source = \
            gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_MAGNETIC
 
    gpsfix_msg.latitude = navsat_msg.latitude
    gpsfix_msg.longitude = navsat_msg.longitude
    gpsfix_msg.altitude = navsat_msg.altitude
    gpsfix_msg.position_covariance = navsat_msg.position_covariance
    gpsfix_msg.position_covariance_type = navsat_msg.position_covariance_type
 
    return gpsfix_msg
 
 
def gpsfix_to_navsatfix(gpsfix_msg):
    # Convert gps_common/GPSFix messages to sensor_msgs/NavSatFix messages
    navsat_msg = NavSatFix()
    navsat_msg.header = gpsfix_msg.header
 
    # Caution: GPSFix has defined some additional status constants, which are
    # not defined in NavSatFix.
    navsat_msg.status.status = gpsfix_msg.status.status
 
    navsat_msg.status.service = 0
    if gpsfix_msg.status.position_source & GPSStatus.SOURCE_GPS:
        navsat_msg.status.service = \
            navsat_msg.status.service | NavSatStatus.SERVICE_GPS
    if gpsfix_msg.status.orientation_source & GPSStatus.SOURCE_MAGNETIC:
        navsat_msg.status.service = \
            navsat_msg.status.service | NavSatStatus.SERVICE_COMPASS
 
    navsat_msg.latitude = gpsfix_msg.latitude
    navsat_msg.longitude = gpsfix_msg.longitude
    navsat_msg.altitude = gpsfix_msg.altitude
    navsat_msg.position_covariance = gpsfix_msg.position_covariance
    navsat_msg.position_covariance_type = gpsfix_msg.position_covariance_type
 
    return navsat_msg