Aufgeräumt
This commit is contained in:
+531
@@ -0,0 +1,531 @@
|
||||
#!/usr/bin/env python
|
||||
import errno
|
||||
import glob
|
||||
import json
|
||||
import os
|
||||
import select
|
||||
import termios
|
||||
from collections import deque
|
||||
from datetime import datetime
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import NavSatFix, NavSatStatus
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
PORT = 'auto'
|
||||
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 = {}
|
||||
self.raw_sentences = deque(maxlen=200)
|
||||
|
||||
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 resolve_port_candidates(port_config):
|
||||
if port_config is None:
|
||||
port_config = PORT
|
||||
|
||||
config_text = str(port_config).strip()
|
||||
if not config_text or config_text.lower() == 'auto':
|
||||
raw_candidates = [
|
||||
'/dev/serial/by-id/*',
|
||||
'/dev/serial/by-path/*',
|
||||
'/dev/ttyUSB*',
|
||||
'/dev/ttyACM*',
|
||||
]
|
||||
else:
|
||||
raw_candidates = [item.strip() for item in config_text.split(',') if item.strip()]
|
||||
|
||||
candidates = []
|
||||
seen = set()
|
||||
for candidate in raw_candidates:
|
||||
matches = sorted(glob.glob(candidate)) if any(char in candidate for char in '*?[') else [candidate]
|
||||
for match in matches:
|
||||
if match in seen:
|
||||
continue
|
||||
seen.add(match)
|
||||
candidates.append(match)
|
||||
return candidates
|
||||
|
||||
|
||||
def open_device(port_config, baud, baud_rate):
|
||||
last_signature = None
|
||||
while not rospy.is_shutdown():
|
||||
candidates = resolve_port_candidates(port_config)
|
||||
signature = tuple(candidates)
|
||||
if not candidates:
|
||||
if signature != last_signature:
|
||||
rospy.loginfo('GPS wartet auf Geraet. Suchmuster: %s', port_config)
|
||||
last_signature = signature
|
||||
rospy.sleep(1.0)
|
||||
continue
|
||||
|
||||
try:
|
||||
for port in candidates:
|
||||
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): %s', port, exc)
|
||||
except termios.error as exc:
|
||||
rospy.logwarn('GPS-Port kann nicht konfiguriert werden (%s): %s', port, exc)
|
||||
finally:
|
||||
last_signature = signature
|
||||
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
|
||||
|
||||
state.raw_sentences.append(sentence)
|
||||
|
||||
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()),
|
||||
'raw_sentences': list(state.raw_sentences)
|
||||
}
|
||||
|
||||
|
||||
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, baud_rate)
|
||||
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)
|
||||
Reference in New Issue
Block a user