Alles verbessert

2026-05-21 19:30:33 +02:00
parent 5dccc8985e
commit 33b71416ff
19 changed files with 855 additions and 229 deletions
@@ -0,0 +1,99 @@
+#!/usr/bin/env python
+import errno
+import os
+import select
+import struct
+import time
+
+import rospy
+from sensor_msgs.msg import Joy
+
+
+DEVICE_PATH = '/dev/input/js0'
+AXIS_COUNT = 8
+BUTTON_COUNT = 12
+PUBLISH_RATE_HZ = 20.0
+
+JS_EVENT_BUTTON = 0x01
+JS_EVENT_AXIS = 0x02
+JS_EVENT_INIT = 0x80
+EVENT_SIZE = struct.calcsize('IhBB')
+
+
+def open_device():
+    while not rospy.is_shutdown():
+        try:
+            fd = os.open(DEVICE_PATH, os.O_RDONLY | os.O_NONBLOCK)
+            rospy.loginfo('Joystick verbunden: %s', DEVICE_PATH)
+            return fd
+        except OSError as exc:
+            if exc.errno != errno.ENOENT:
+                rospy.logwarn('Joystick kann nicht geoeffnet 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
+
+
+if __name__ == '__main__':
+    rospy.init_node('f710_joy_node')
+
+    publisher = rospy.Publisher('/joy', Joy, queue_size=5)
+    rate = rospy.Rate(PUBLISH_RATE_HZ)
+
+    axes = [0.0] * AXIS_COUNT
+    buttons = [0] * BUTTON_COUNT
+    joystick_fd = None
+
+    while not rospy.is_shutdown():
+        if joystick_fd is None:
+            joystick_fd = open_device()
+            axes = [0.0] * AXIS_COUNT
+            buttons = [0] * BUTTON_COUNT
+            if joystick_fd is None:
+                break
+
+        try:
+            readable, _, _ = select.select([joystick_fd], [], [], 0.0)
+            if readable:
+                event = os.read(joystick_fd, EVENT_SIZE)
+                while event and len(event) == EVENT_SIZE:
+                    _, value, event_type, number = struct.unpack('IhBB', event)
+                    event_type &= ~JS_EVENT_INIT
+
+                    if event_type == JS_EVENT_AXIS and number < len(axes):
+                        axes[number] = max(-1.0, min(1.0, value / 32767.0))
+                    elif event_type == JS_EVENT_BUTTON and number < len(buttons):
+                        buttons[number] = 1 if value else 0
+
+                    try:
+                        event = os.read(joystick_fd, EVENT_SIZE)
+                    except OSError as exc:
+                        if exc.errno in (errno.EAGAIN, errno.EWOULDBLOCK):
+                            break
+                        raise
+
+            message = Joy()
+            message.header.stamp = rospy.Time.now()
+            message.axes = list(axes)
+            message.buttons = list(buttons)
+            publisher.publish(message)
+            rate.sleep()
+
+        except OSError as exc:
+            if exc.errno in (errno.ENODEV, errno.EIO, errno.ENXIO, errno.EBADF):
+                rospy.logwarn('Joystick getrennt, warte auf Neuverbindung.')
+            else:
+                rospy.logwarn('Joystick-Lesefehler: %s', exc)
+            close_device(joystick_fd)
+            joystick_fd = None
+            time.sleep(1.0)
+
+    close_device(joystick_fd)