gps_message_converter.py
Go to the documentation of this file.
1 from sensor_msgs.msg import NavSatFix
2 from sensor_msgs.msg import NavSatStatus
3 from gps_common.msg import GPSFix
4 from gps_common.msg import GPSStatus
5 
6 
7 def navsatfix_to_gpsfix(navsat_msg):
8  # Convert sensor_msgs/NavSatFix messages to gps_common/GPSFix messages
9  gpsfix_msg = GPSFix()
10  gpsfix_msg.header = navsat_msg.header
11  gpsfix_msg.status.status = navsat_msg.status.status
12 
13  gpsfix_msg.status.motion_source = GPSStatus.SOURCE_NONE
14  gpsfix_msg.status.orientation_source = GPSStatus.SOURCE_NONE
15  gpsfix_msg.status.position_source = GPSStatus.SOURCE_NONE
16  if ((navsat_msg.status.service & NavSatStatus.SERVICE_GPS) or
17  (navsat_msg.status.service & NavSatStatus.SERVICE_GLONASS) or
18  (navsat_msg.status.service & NavSatStatus.SERVICE_GALILEO)):
19  gpsfix_msg.status.motion_source = \
20  gpsfix_msg.status.motion_source | GPSStatus.SOURCE_GPS
21  gpsfix_msg.status.orientation_source = \
22  gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_GPS
23  gpsfix_msg.status.position_source = \
24  gpsfix_msg.status.position_source | GPSStatus.SOURCE_GPS
25 
26  if navsat_msg.status.service & NavSatStatus.SERVICE_COMPASS:
27  gpsfix_msg.status.orientation_source = \
28  gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_MAGNETIC
29 
30  gpsfix_msg.latitude = navsat_msg.latitude
31  gpsfix_msg.longitude = navsat_msg.longitude
32  gpsfix_msg.altitude = navsat_msg.altitude
33  gpsfix_msg.position_covariance = navsat_msg.position_covariance
34  gpsfix_msg.position_covariance_type = navsat_msg.position_covariance_type
35 
36  return gpsfix_msg
37 
38 
39 def gpsfix_to_navsatfix(gpsfix_msg):
40  # Convert gps_common/GPSFix messages to sensor_msgs/NavSatFix messages
41  navsat_msg = NavSatFix()
42  navsat_msg.header = gpsfix_msg.header
43 
44  # Caution: GPSFix has defined some additional status constants, which are
45  # not defined in NavSatFix.
46  navsat_msg.status.status = gpsfix_msg.status.status
47 
48  navsat_msg.status.service = 0
49  if gpsfix_msg.status.position_source & GPSStatus.SOURCE_GPS:
50  navsat_msg.status.service = \
51  navsat_msg.status.service | NavSatStatus.SERVICE_GPS
52  if gpsfix_msg.status.orientation_source & GPSStatus.SOURCE_MAGNETIC:
53  navsat_msg.status.service = \
54  navsat_msg.status.service | NavSatStatus.SERVICE_COMPASS
55 
56  navsat_msg.latitude = gpsfix_msg.latitude
57  navsat_msg.longitude = gpsfix_msg.longitude
58  navsat_msg.altitude = gpsfix_msg.altitude
59  navsat_msg.position_covariance = gpsfix_msg.position_covariance
60  navsat_msg.position_covariance_type = gpsfix_msg.position_covariance_type
61 
62  return navsat_msg


gps_common
Author(s):
autogenerated on Sat Jun 17 2023 02:44:30