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