GPS eingebaut

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

ExoMy_Software-master/gui/index.html

@@ -64,8 +64,10 @@
           CTRL: <span id="input-state">Joystick</span>
         </div>
         <div class="c-bl">
-          X: <span id="cam-x">+0.00</span><br>
+          GPS: <span id="gps-state">Offline</span><br>
-          Y: <span id="cam-y">+0.00</span>
+          SAT: <span id="gps-sats">--</span><br>
+          LAT: <span id="gps-lat">--</span><br>
+          LON: <span id="gps-lon">--</span>
         </div>
         <div class="c-br">
           HOST: <span id="cam-host">--</span><br>
@@ -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",

ExoMy_Software-master/launch/exomy.launch

@@ -2,6 +2,11 @@
     <node name="robot" pkg="exomy" type="robot_node.py" respawn="true" output="screen"/>
     <node name="motors" pkg="exomy" type="motor_node.py" respawn="true" output="screen" />
     <node name="joystick" pkg="exomy" type="joystick_parser_node.py" respawn="true" output="screen" />
+    <node name="gps" pkg="exomy" type="gps_node.py" respawn="true" output="screen">
+        <param name="port" value="/dev/ttyUSB0"/>
+        <param name="baud" value="4800"/>
+        <param name="frame_id" value="gps"/>
+    </node>
 
     <node respawn="true" pkg="joy" type="joy_node" name="joy_node">
         <param name="coalesce_interval" value="0.05"/>

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)