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)