From b24996e67f40d32e296d758bd06a147856bfc410 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20M=C3=BCller?= Date: Sat, 23 May 2026 19:44:12 +0200 Subject: [PATCH] GPS eingebaut --- ExoMy_Software-master/docker/entrypoint.sh | 6 +- ExoMy_Software-master/gui/index.html | 48 ++- ExoMy_Software-master/launch/exomy.launch | 5 + ExoMy_Software-master/src/gps_node.py | 327 +++++++++++++++++++++ 4 files changed, 380 insertions(+), 6 deletions(-) create mode 100644 ExoMy_Software-master/src/gps_node.py diff --git a/ExoMy_Software-master/docker/entrypoint.sh b/ExoMy_Software-master/docker/entrypoint.sh index 9d52866..fc442d1 100644 --- a/ExoMy_Software-master/docker/entrypoint.sh +++ b/ExoMy_Software-master/docker/entrypoint.sh @@ -2,7 +2,7 @@ set -eo pipefail cleanup() { - for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID DELAY_PID; do + for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID DELAY_PID GPS_PID; do if [[ -n "${!pid_var:-}" ]]; then kill "${!pid_var}" 2>/dev/null || true fi @@ -53,8 +53,10 @@ then MOTOR_PID=$! python /root/exomy_ws/src/exomy/src/joystick_parser_node.py > /tmp/joystick_parser.log 2>&1 & JOYSTICK_PID=$! + python /root/exomy_ws/src/exomy/src/gps_node.py > /tmp/gps_node.log 2>&1 & + GPS_PID=$! - wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID" "$DELAY_PID" + wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID" "$DELAY_PID" "$GPS_PID" exit 1 elif [[ $1 == "devel" ]] then diff --git a/ExoMy_Software-master/gui/index.html b/ExoMy_Software-master/gui/index.html index 628ec47..8d4c542 100644 --- a/ExoMy_Software-master/gui/index.html +++ b/ExoMy_Software-master/gui/index.html @@ -64,8 +64,10 @@ CTRL: Joystick
- X: +0.00
- Y: +0.00 + GPS: Offline
+ SAT: --
+ LAT: --
+ LON: --
HOST: --
@@ -280,6 +282,8 @@ var ros = null; var joyListener = null; var roverCommandListener = null; var motorCommandsListener = null; +var gpsFixListener = null; +var gpsStatusListener = null; var publishTimer = null; var statusTimer = null; var axes = [0, 0, 0, 0, 0, 0]; @@ -584,8 +588,28 @@ function updateAxesDisplay(x, y) { setText("jy", formatAxis(y)); setText("dpx", formatAxis(x)); setText("dpy", formatAxis(y)); - setText("cam-x", formatAxis(x)); - setText("cam-y", formatAxis(y)); +} + +function updateGpsOverlayFromFix(message) { + if (!message) return; + + var hasFix = message.status && message.status.status >= 0; + if (!hasFix) { + setText("gps-lat", "--"); + setText("gps-lon", "--"); + return; + } + + setText("gps-lat", formatNumber(Number(message.latitude || 0), 5)); + setText("gps-lon", formatNumber(Number(message.longitude || 0), 5)); +} + +function updateGpsOverlayFromStatus(text) { + var value = String(text || "Offline"); + setText("gps-state", value); + + var satMatch = value.match(/(\d+)\s*Sat/i); + setText("gps-sats", satMatch ? satMatch[1] : "--"); } function updateModeDisplay() { @@ -1028,6 +1052,22 @@ window.addEventListener("load", function () { renderTrajectoryOverlay(); }); + gpsFixListener = new ROSLIB.Topic({ + ros: ros, + name: "/fix", + messageType: "sensor_msgs/NavSatFix" + }); + gpsFixListener.subscribe(updateGpsOverlayFromFix); + + gpsStatusListener = new ROSLIB.Topic({ + ros: ros, + name: "/gps/status_text", + messageType: "std_msgs/String" + }); + gpsStatusListener.subscribe(function (message) { + updateGpsOverlayFromStatus(message.data); + }); + var joySubscriber = new ROSLIB.Topic({ ros: ros, name: "/joy", diff --git a/ExoMy_Software-master/launch/exomy.launch b/ExoMy_Software-master/launch/exomy.launch index d10428f..e384fe9 100644 --- a/ExoMy_Software-master/launch/exomy.launch +++ b/ExoMy_Software-master/launch/exomy.launch @@ -2,6 +2,11 @@ + + + + + diff --git a/ExoMy_Software-master/src/gps_node.py b/ExoMy_Software-master/src/gps_node.py new file mode 100644 index 0000000..d534a87 --- /dev/null +++ b/ExoMy_Software-master/src/gps_node.py @@ -0,0 +1,327 @@ +#!/usr/bin/env python +import errno +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.valid = False + self.last_sentence_time = None + self.last_fix_time = None + + 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() + + # Optional date/time parse for debugging consistency. + if fields[1] and fields[9]: + try: + datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S') + except ValueError: + pass + 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) + return False + + +def publish_fix(fix_publisher, overlay_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())) + + +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) + + 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, 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, 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, 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)