486 lines
14 KiB
Python
486 lines
14 KiB
Python
#!/usr/bin/env python
|
|
import errno
|
|
import json
|
|
import os
|
|
import select
|
|
import termios
|
|
from datetime import datetime
|
|
|
|
import rospy
|
|
from sensor_msgs.msg import NavSatFix, NavSatStatus
|
|
from std_msgs.msg import String
|
|
|
|
|
|
PORT = '/dev/ttyUSB0'
|
|
BAUD = 4800
|
|
FRAME_ID = 'gps'
|
|
READ_SIZE = 512
|
|
|
|
BAUD_MAP = {
|
|
4800: termios.B4800,
|
|
9600: termios.B9600,
|
|
19200: termios.B19200,
|
|
38400: termios.B38400,
|
|
57600: termios.B57600,
|
|
115200: termios.B115200,
|
|
}
|
|
|
|
|
|
def nmea_to_decimal(raw_value, hemisphere):
|
|
if not raw_value or not hemisphere:
|
|
return None
|
|
|
|
try:
|
|
value = float(raw_value)
|
|
except ValueError:
|
|
return None
|
|
|
|
degrees = int(value / 100)
|
|
minutes = value - degrees * 100
|
|
decimal = degrees + minutes / 60.0
|
|
|
|
if hemisphere in ('S', 'W'):
|
|
decimal *= -1.0
|
|
|
|
return decimal
|
|
|
|
|
|
def verify_checksum(sentence):
|
|
if not sentence.startswith('$'):
|
|
return False
|
|
|
|
if '*' not in sentence:
|
|
return True
|
|
|
|
payload, checksum_text = sentence[1:].split('*', 1)
|
|
checksum_text = checksum_text.strip()
|
|
checksum = 0
|
|
for char in payload:
|
|
checksum ^= ord(char)
|
|
|
|
try:
|
|
expected = int(checksum_text[:2], 16)
|
|
except ValueError:
|
|
return False
|
|
|
|
return checksum == expected
|
|
|
|
|
|
class GpsState(object):
|
|
def __init__(self):
|
|
self.connected = False
|
|
self.latitude = None
|
|
self.longitude = None
|
|
self.altitude = None
|
|
self.hdop = None
|
|
self.speed_mps = None
|
|
self.track_deg = None
|
|
self.fix_quality = 0
|
|
self.satellites = 0
|
|
self.pdop = None
|
|
self.vdop = None
|
|
self.valid = False
|
|
self.last_sentence_time = None
|
|
self.last_fix_time = None
|
|
self.utc_datetime = None
|
|
self.visible_satellites = {}
|
|
self.visible_satellite_count = 0
|
|
self._gsv_groups = {}
|
|
|
|
def summary_text(self):
|
|
if not self.connected:
|
|
return 'Offline'
|
|
if self.valid and self.latitude is not None and self.longitude is not None:
|
|
speed_kmh = (self.speed_mps or 0.0) * 3.6
|
|
return 'Fix | {0} Sat | {1:.1f} km/h'.format(max(0, int(self.satellites or 0)), speed_kmh)
|
|
if self.last_sentence_time is not None:
|
|
return 'Suche Satelliten'
|
|
return 'Initialisiere'
|
|
|
|
|
|
def configure_serial(fd, baud):
|
|
attrs = termios.tcgetattr(fd)
|
|
attrs[0] = 0
|
|
attrs[1] = 0
|
|
attrs[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
|
|
attrs[3] = 0
|
|
attrs[4] = baud
|
|
attrs[5] = baud
|
|
attrs[6][termios.VMIN] = 0
|
|
attrs[6][termios.VTIME] = 0
|
|
termios.tcsetattr(fd, termios.TCSANOW, attrs)
|
|
termios.tcflush(fd, termios.TCIFLUSH)
|
|
|
|
|
|
def open_device(port, baud):
|
|
while not rospy.is_shutdown():
|
|
try:
|
|
fd = os.open(port, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
|
|
configure_serial(fd, baud)
|
|
rospy.loginfo('GPS verbunden: %s @ %s Baud', port, baud_rate)
|
|
return fd
|
|
except OSError as exc:
|
|
if exc.errno != errno.ENOENT:
|
|
rospy.logwarn('GPS kann nicht geoeffnet werden: %s', exc)
|
|
rospy.sleep(1.0)
|
|
except termios.error as exc:
|
|
rospy.logwarn('GPS-Port kann nicht konfiguriert werden: %s', exc)
|
|
rospy.sleep(1.0)
|
|
return None
|
|
|
|
|
|
def close_device(fd):
|
|
if fd is None:
|
|
return
|
|
try:
|
|
os.close(fd)
|
|
except OSError:
|
|
pass
|
|
|
|
|
|
def parse_gga(fields, state):
|
|
if len(fields) < 10:
|
|
return False
|
|
|
|
latitude = nmea_to_decimal(fields[2], fields[3])
|
|
longitude = nmea_to_decimal(fields[4], fields[5])
|
|
|
|
try:
|
|
fix_quality = int(fields[6] or 0)
|
|
except ValueError:
|
|
fix_quality = 0
|
|
|
|
try:
|
|
satellites = int(fields[7] or 0)
|
|
except ValueError:
|
|
satellites = 0
|
|
|
|
try:
|
|
hdop = float(fields[8]) if fields[8] else None
|
|
except ValueError:
|
|
hdop = None
|
|
|
|
try:
|
|
altitude = float(fields[9]) if fields[9] else None
|
|
except ValueError:
|
|
altitude = None
|
|
|
|
if latitude is not None:
|
|
state.latitude = latitude
|
|
if longitude is not None:
|
|
state.longitude = longitude
|
|
state.fix_quality = fix_quality
|
|
state.satellites = satellites
|
|
state.hdop = hdop
|
|
state.altitude = altitude
|
|
state.valid = fix_quality > 0 and latitude is not None and longitude is not None
|
|
if state.valid:
|
|
state.last_fix_time = rospy.Time.now()
|
|
return True
|
|
|
|
|
|
def parse_rmc(fields, state):
|
|
if len(fields) < 10:
|
|
return False
|
|
|
|
status = fields[2] if len(fields) > 2 else ''
|
|
latitude = nmea_to_decimal(fields[3], fields[4])
|
|
longitude = nmea_to_decimal(fields[5], fields[6])
|
|
|
|
try:
|
|
speed_knots = float(fields[7]) if fields[7] else 0.0
|
|
except ValueError:
|
|
speed_knots = 0.0
|
|
|
|
try:
|
|
track_deg = float(fields[8]) if fields[8] else 0.0
|
|
except ValueError:
|
|
track_deg = 0.0
|
|
|
|
if latitude is not None:
|
|
state.latitude = latitude
|
|
if longitude is not None:
|
|
state.longitude = longitude
|
|
state.speed_mps = speed_knots * 0.514444
|
|
state.track_deg = track_deg
|
|
if status == 'A' and state.latitude is not None and state.longitude is not None:
|
|
state.valid = True
|
|
state.last_fix_time = rospy.Time.now()
|
|
|
|
if fields[1] and fields[9]:
|
|
try:
|
|
state.utc_datetime = datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
|
|
except ValueError:
|
|
pass
|
|
return True
|
|
|
|
|
|
def parse_gsa(fields, state):
|
|
if len(fields) < 18:
|
|
return False
|
|
|
|
try:
|
|
state.pdop = float(fields[15]) if fields[15] else None
|
|
except ValueError:
|
|
state.pdop = None
|
|
|
|
try:
|
|
state.hdop = float(fields[16]) if fields[16] else state.hdop
|
|
except ValueError:
|
|
pass
|
|
|
|
try:
|
|
state.vdop = float(fields[17]) if fields[17] else None
|
|
except ValueError:
|
|
state.vdop = None
|
|
return True
|
|
|
|
|
|
def merge_visible_satellites(state):
|
|
now = rospy.Time.now()
|
|
merged = {}
|
|
|
|
stale_talkers = []
|
|
for talker, group in state._gsv_groups.items():
|
|
completed_at = group.get('completed_at')
|
|
if completed_at is None:
|
|
continue
|
|
try:
|
|
if (now - completed_at).to_sec() > 8.0:
|
|
stale_talkers.append(talker)
|
|
continue
|
|
except rospy.ROSException:
|
|
continue
|
|
|
|
constellation = group.get('constellation', talker)
|
|
for sat_key, satellite in group.get('satellites', {}).items():
|
|
merged[sat_key] = dict(satellite, constellation=constellation)
|
|
|
|
for talker in stale_talkers:
|
|
state._gsv_groups.pop(talker, None)
|
|
|
|
state.visible_satellites = dict(sorted(merged.items()))
|
|
state.visible_satellite_count = len(state.visible_satellites)
|
|
|
|
|
|
def parse_gsv(fields, state, sentence_type):
|
|
if len(fields) < 4:
|
|
return False
|
|
|
|
try:
|
|
total_sentences = int(fields[1] or 0)
|
|
sentence_index = int(fields[2] or 0)
|
|
total_satellites = int(fields[3] or 0)
|
|
except ValueError:
|
|
return False
|
|
|
|
if sentence_index <= 0:
|
|
return False
|
|
|
|
talker = sentence_type[1:3] if len(sentence_type) >= 3 else 'GN'
|
|
group = state._gsv_groups.get(talker)
|
|
if group is None:
|
|
group = {
|
|
'constellation': talker,
|
|
'total_sentences': 0,
|
|
'working_set': {},
|
|
'satellites': {},
|
|
'completed_at': None,
|
|
'total_satellites_reported': 0,
|
|
}
|
|
state._gsv_groups[talker] = group
|
|
|
|
if sentence_index == 1 or total_sentences != group.get('total_sentences'):
|
|
group['working_set'] = {}
|
|
|
|
group['total_sentences'] = total_sentences
|
|
group['total_satellites_reported'] = total_satellites
|
|
|
|
for index in range(4, len(fields), 4):
|
|
if index + 3 >= len(fields):
|
|
break
|
|
prn = fields[index].strip()
|
|
if not prn:
|
|
continue
|
|
try:
|
|
elevation = int(fields[index + 1]) if fields[index + 1] else None
|
|
except ValueError:
|
|
elevation = None
|
|
try:
|
|
azimuth = int(fields[index + 2]) if fields[index + 2] else None
|
|
except ValueError:
|
|
azimuth = None
|
|
try:
|
|
snr = int(fields[index + 3]) if fields[index + 3] else None
|
|
except ValueError:
|
|
snr = None
|
|
|
|
satellite_key = '{0}:{1}'.format(talker, prn)
|
|
group['working_set'][satellite_key] = {
|
|
'prn': '{0}-{1}'.format(talker, prn),
|
|
'elevation': elevation,
|
|
'azimuth': azimuth,
|
|
'snr': snr
|
|
}
|
|
|
|
if sentence_index >= total_sentences:
|
|
group['satellites'] = dict(group['working_set'])
|
|
group['completed_at'] = rospy.Time.now()
|
|
merge_visible_satellites(state)
|
|
return True
|
|
|
|
|
|
def parse_sentence(sentence, state):
|
|
if not verify_checksum(sentence):
|
|
return False
|
|
|
|
fields = sentence.split('*', 1)[0].split(',')
|
|
if not fields:
|
|
return False
|
|
|
|
sentence_type = fields[0]
|
|
state.last_sentence_time = rospy.Time.now()
|
|
|
|
if sentence_type.endswith('GGA'):
|
|
return parse_gga(fields, state)
|
|
if sentence_type.endswith('RMC'):
|
|
return parse_rmc(fields, state)
|
|
if sentence_type.endswith('GSA'):
|
|
return parse_gsa(fields, state)
|
|
if sentence_type.endswith('GSV'):
|
|
return parse_gsv(fields, state, sentence_type)
|
|
return False
|
|
|
|
|
|
def build_diagnostics_payload(state, frame_id):
|
|
last_fix_age = None
|
|
if state.last_fix_time is not None:
|
|
try:
|
|
last_fix_age = max(0.0, (rospy.Time.now() - state.last_fix_time).to_sec())
|
|
except rospy.ROSException:
|
|
last_fix_age = None
|
|
|
|
return {
|
|
'connected': state.connected,
|
|
'frame_id': frame_id,
|
|
'valid': state.valid,
|
|
'fix_quality': state.fix_quality,
|
|
'satellites_used': max(0, int(state.satellites or 0)),
|
|
'satellites_visible': max(0, int(state.visible_satellite_count or 0)),
|
|
'latitude': state.latitude,
|
|
'longitude': state.longitude,
|
|
'altitude_m': state.altitude,
|
|
'hdop': state.hdop,
|
|
'pdop': state.pdop,
|
|
'vdop': state.vdop,
|
|
'speed_mps': state.speed_mps,
|
|
'speed_kmh': (state.speed_mps or 0.0) * 3.6 if state.speed_mps is not None else None,
|
|
'track_deg': state.track_deg,
|
|
'utc_time': state.utc_datetime.strftime('%H:%M:%S') if state.utc_datetime else None,
|
|
'utc_date': state.utc_datetime.strftime('%Y-%m-%d') if state.utc_datetime else None,
|
|
'last_fix_age_s': last_fix_age,
|
|
'satellites': list(state.visible_satellites.values())
|
|
}
|
|
|
|
|
|
def publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id):
|
|
message = NavSatFix()
|
|
message.header.stamp = rospy.Time.now()
|
|
message.header.frame_id = frame_id
|
|
message.status.service = NavSatStatus.SERVICE_GPS
|
|
|
|
if state.valid and state.latitude is not None and state.longitude is not None:
|
|
message.status.status = NavSatStatus.STATUS_FIX
|
|
message.latitude = state.latitude
|
|
message.longitude = state.longitude
|
|
message.altitude = state.altitude if state.altitude is not None else 0.0
|
|
if state.hdop is not None:
|
|
horizontal_variance = max(0.25, state.hdop * state.hdop)
|
|
vertical_variance = max(1.0, (state.hdop * 2.0) * (state.hdop * 2.0))
|
|
message.position_covariance = [
|
|
horizontal_variance, 0.0, 0.0,
|
|
0.0, horizontal_variance, 0.0,
|
|
0.0, 0.0, vertical_variance
|
|
]
|
|
message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
|
|
else:
|
|
message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
|
|
else:
|
|
message.status.status = NavSatStatus.STATUS_NO_FIX
|
|
message.latitude = 0.0
|
|
message.longitude = 0.0
|
|
message.altitude = 0.0
|
|
message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
|
|
|
|
fix_publisher.publish(message)
|
|
overlay_publisher.publish(String(data=state.summary_text()))
|
|
diagnostics_publisher.publish(String(data=json.dumps(build_diagnostics_payload(state, frame_id), sort_keys=True)))
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('gps_node')
|
|
|
|
port = rospy.get_param('~port', PORT)
|
|
baud_rate = int(rospy.get_param('~baud', BAUD))
|
|
frame_id = rospy.get_param('~frame_id', FRAME_ID)
|
|
baud = BAUD_MAP.get(baud_rate, termios.B4800)
|
|
|
|
fix_publisher = rospy.Publisher('/fix', NavSatFix, queue_size=5)
|
|
overlay_publisher = rospy.Publisher('/gps/status_text', String, queue_size=5)
|
|
diagnostics_publisher = rospy.Publisher('/gps/diagnostics_json', String, queue_size=5)
|
|
|
|
state = GpsState()
|
|
serial_fd = None
|
|
buffer = bytearray()
|
|
|
|
while not rospy.is_shutdown():
|
|
if serial_fd is None:
|
|
serial_fd = open_device(port, baud)
|
|
buffer = bytearray()
|
|
state.connected = serial_fd is not None
|
|
publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
|
|
if serial_fd is None:
|
|
break
|
|
|
|
try:
|
|
readable, _, _ = select.select([serial_fd], [], [], 0.5)
|
|
if serial_fd not in readable:
|
|
publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
|
|
continue
|
|
|
|
chunk = os.read(serial_fd, READ_SIZE)
|
|
if not chunk:
|
|
raise OSError(errno.ENODEV, 'GPS getrennt')
|
|
|
|
buffer.extend(chunk)
|
|
|
|
while b'\n' in buffer:
|
|
raw_line, _, buffer = buffer.partition(b'\n')
|
|
line = raw_line.decode('ascii', 'ignore').strip()
|
|
if not line or not line.startswith('$'):
|
|
continue
|
|
if parse_sentence(line, state):
|
|
publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
|
|
|
|
except OSError as exc:
|
|
if exc.errno not in (errno.EAGAIN, errno.EWOULDBLOCK):
|
|
rospy.logwarn('GPS-Lesefehler: %s', exc)
|
|
close_device(serial_fd)
|
|
serial_fd = None
|
|
state.connected = False
|
|
state.valid = False
|
|
state.last_sentence_time = None
|
|
state.satellites = 0
|
|
rospy.sleep(1.0)
|
|
except termios.error as exc:
|
|
rospy.logwarn('GPS-Fehler: %s', exc)
|
|
close_device(serial_fd)
|
|
serial_fd = None
|
|
state.connected = False
|
|
state.valid = False
|
|
state.last_sentence_time = None
|
|
state.satellites = 0
|
|
rospy.sleep(1.0)
|
|
|
|
close_device(serial_fd)
|