def callback( packet ) :
if rospy.is_shutdown() :
sys.exit()
message = GPSFix()
message.header.stamp = rospy.Time.now()
message.header.frame_id = 'base_link'
message.latitude = packet[ 'latitude' ] * 180 / math.pi
message.longitude = packet[ 'longitude' ] * 180 / math.pi
message.altitude = packet[ 'height' ]
message.time = packet[ 'time' ]
message.pdop = packet[ 'PDOP' ]
message.hdop = packet[ 'HDOP' ]
message.vdop = packet[ 'VDOP' ]
message.tdop = packet[ 'TDOP' ]
message.position_covariance = [ packet[ 'sigma-E' ], packet[ 'cov-EN' ] , 0,
packet[ 'cov-EN' ], packet[ 'sigma-N' ], 0,
0, 0, packet[ 'sigma-Up'] ]
if packet[ 'cov-EN' ] != 0 :
message.position_covariance_type = 3
elif packet[ 'sigma-E' ] != 0 :
message.position_covariance_type = 2
else :
message.position_covariance_type = 0
if debug :
pp.pprint( packet )
pp.pprint( message )
message.status.status = (0 if (message.position_covariance_type > 0) else -1)
message.status.position_source = 1
message.status.header = message.header
publisherGPSFix.publish( message )
if message.status >= 0 :
publisherNavSatFix.publish( NavSatFix( message.header , NavSatStatus( message.status.status, 3 ), message.latitude, message.longitude,
message.altitude, message.position_covariance, message.position_covariance_type ) )
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
