Kaera umschaltbar

ExoMy_Software-master/src/gps_node.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 import errno
+import glob
 import json
 import os
 import select
@@ -12,7 +13,7 @@ from sensor_msgs.msg import NavSatFix, NavSatStatus
 from std_msgs.msg import String
 
 
-PORT = '/dev/ttyUSB0'
+PORT = 'auto'
 BAUD = 4800
 FRAME_ID = 'gps'
 READ_SIZE = 512
@@ -114,20 +115,60 @@ def configure_serial(fd, baud):
     termios.tcflush(fd, termios.TCIFLUSH)
 
 
-def open_device(port, baud):
+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:
-            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)
+            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
 
 
@@ -440,7 +481,7 @@ if __name__ == '__main__':
 
     while not rospy.is_shutdown():
         if serial_fd is None:
-            serial_fd = open_device(port, baud)
+            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)