Rückmeldung vibration

ExoMy_Software-master/gui/admin.html

@@ -40,6 +40,7 @@
                 <button class="button buttonDanger" onclick="runAdminAction('restart-container')">ExoMy-Container neu starten</button>
                 <button class="button buttonWarm" onclick="runAdminAction('reboot')">Raspberry Pi neu starten</button>
                 <button class="button buttonDanger" onclick="runAdminAction('shutdown')">Raspberry Pi herunterfahren</button>
+                <button class="button rumble_btn" onclick="runRumble()">Controller vibrieren</button>
               </div>
 
               <div class="service_compact">
@@ -330,6 +331,17 @@
       }
     }
 
+    async function runRumble() {
+      try {
+        var response = await fetch(adminApiBase + '/api/rumble', { method: 'POST' });
+        var data = await response.json();
+        setAdminStatus(data.status || 'Vibration ausgelöst');
+        window.setTimeout(function() { setAdminStatus('Bereit'); }, 2000);
+      } catch (e) {
+        setAdminStatus('Fehler');
+      }
+    }
+
     window.addEventListener('load', function () {
       var input = document.getElementById('auth_input');
       input.focus();

ExoMy_Software-master/gui/style.css

@@ -632,6 +632,22 @@ h2 {
     0 8px 18px rgba(109, 126, 132, 0.08);
 }
 
+.rumble_btn {
+  grid-column: span 2;
+  min-height: auto !important;
+  padding: 0.45rem 0.9rem;
+  font-size: 0.8rem;
+  opacity: 0.75;
+  color: #fff;
+  background: rgba(255,255,255,0.12);
+  border: 1px solid rgba(255,255,255,0.2);
+}
+
+.rumble_btn:hover {
+  opacity: 1;
+  background: rgba(255,255,255,0.2);
+}
+
 .delay_control {
   display: flex;
   flex-direction: column;

ExoMy_Software-master/scripts/exomy_admin_api.py

@@ -1,9 +1,12 @@
 #!/usr/bin/env python3
+import fcntl
+import glob
 import http.server
 import json
 import os
 import shutil
 import socketserver
+import struct
 import subprocess
 import threading
 import time
@@ -248,6 +251,62 @@ def set_delay(seconds):
     return seconds
 
 
+def rumble_controller(duration_ms=400):
+    # Linux FF constants
+    EV_FF = 0x15
+    FF_RUMBLE = 0x50
+    # EVIOCSFF = _IOW('E', 0x80, struct ff_effect) — ff_effect is 48 bytes on ARM64
+    EVIOCSFF = (0x40000000 | (48 << 16) | (ord('E') << 8) | 0x80)
+
+    device_path = None
+    for path in sorted(glob.glob('/dev/input/event*')):
+        try:
+            sys_name = f'/sys/class/input/{os.path.basename(path)}/device/name'
+            with open(sys_name, encoding='utf-8') as f:
+                name = f.read().strip()
+            if any(k in name for k in ('Gamepad', 'F710', 'GAMEPAD')):
+                device_path = path
+                break
+        except OSError:
+            pass
+
+    if not device_path:
+        raise RuntimeError('Kein Gamepad gefunden')
+
+    fd = os.open(device_path, os.O_RDWR)
+    try:
+        # struct ff_effect layout on ARM64 (48 bytes):
+        # offset  0: type (u16)
+        # offset  2: id   (s16) — set to -1, kernel assigns
+        # offset  4: direction (u16)
+        # offset  6: trigger.button (u16)
+        # offset  8: trigger.interval (u16)
+        # offset 10: replay.length (u16, ms)
+        # offset 12: replay.delay (u16)
+        # offset 14: padding (2 bytes)
+        # offset 16: union.rumble.strong_magnitude (u16)
+        # offset 18: union.rumble.weak_magnitude (u16)
+        # offset 20–47: zeros
+        buf = bytearray(48)
+        struct.pack_into('<H', buf,  0, FF_RUMBLE)
+        struct.pack_into('<h', buf,  2, -1)
+        struct.pack_into('<H', buf, 10, duration_ms)
+        struct.pack_into('<H', buf, 16, 0xFFFF)
+        struct.pack_into('<H', buf, 18, 0x8000)
+
+        fcntl.ioctl(fd, EVIOCSFF, buf)
+        effect_id = struct.unpack_from('<h', buf, 2)[0]
+
+        # input_event on ARM64: sec(u64) + usec(u64) + type(u16) + code(u16) + value(s32) = 24 bytes
+        os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 1))
+        time.sleep(duration_ms / 1000.0 + 0.1)
+        os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 0))
+    except OSError as exc:
+        raise RuntimeError(f'FF ioctl fehlgeschlagen: {exc}') from exc
+    finally:
+        os.close(fd)
+
+
 def restart_camera_service():
     subprocess.Popen(['systemctl', 'restart', 'exomy-camera-stream.service'])
 
@@ -659,6 +718,14 @@ class Handler(http.server.BaseHTTPRequestHandler):
             self.write_json(status_code, response)
             return
 
+        if self.path == '/api/rumble':
+            try:
+                rumble_controller(400)
+                self.write_json(200, {'status': 'Vibration ausgelöst'})
+            except RuntimeError as exc:
+                self.write_json(500, {'status': str(exc)})
+            return
+
         actions = {
             '/api/restart-camera': {
                 'handler': restart_camera_service,

ExoMy_Software-master/src/joystick_parser_node.py

@@ -1,16 +1,25 @@
 #!/usr/bin/env python
+import ctypes
+import fcntl
+import glob
+import math
+import os
+import struct
+import threading
+
 import rospy
 from sensor_msgs.msg import Joy
 from exomy.msg import RoverCommand
 from locomotion_modes import LocomotionMode
-import math
 
 # Define locomotion modes
 global locomotion_mode
 global motors_enabled
 global last_start_button_pressed
+global last_locomotion_mode
 
 locomotion_mode = LocomotionMode.ACKERMANN.value
+last_locomotion_mode = locomotion_mode
 motors_enabled = True
 AXIS_DEADZONE = 0.1
 last_start_button_pressed = False
@@ -63,6 +72,51 @@ CONTROLLER_FUNCTION_MAPS = {
 }
 
 
+def _rumble_thread(duration_ms):
+    EV_FF = 0x15
+    FF_RUMBLE = 0x50
+    EVIOCSFF = (0x40000000 | (48 << 16) | (ord('E') << 8) | 0x80)
+
+    device_path = None
+    for path in sorted(glob.glob('/dev/input/event*')):
+        try:
+            sys_name = '/sys/class/input/{0}/device/name'.format(os.path.basename(path))
+            with open(sys_name) as f:
+                name = f.read().strip()
+            if any(k in name for k in ('Gamepad', 'F710', 'GAMEPAD')):
+                device_path = path
+                break
+        except OSError:
+            pass
+
+    if not device_path:
+        return
+
+    try:
+        fd = os.open(device_path, os.O_RDWR)
+        try:
+            buf = ctypes.create_string_buffer(48)
+            struct.pack_into('<H', buf,  0, FF_RUMBLE)
+            struct.pack_into('<h', buf,  2, -1)
+            struct.pack_into('<H', buf, 10, duration_ms)
+            struct.pack_into('<H', buf, 16, 0xFFFF)
+            struct.pack_into('<H', buf, 18, 0x8000)
+            fcntl.ioctl(fd, EVIOCSFF, buf)
+            effect_id = struct.unpack_from('<h', buf, 2)[0]
+            import time as _t
+            os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 1))
+            _t.sleep(duration_ms / 1000.0 + 0.05)
+            os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 0))
+        finally:
+            os.close(fd)
+    except Exception as exc:
+        rospy.logwarn('rumble fehler: %s', exc)
+
+
+def trigger_rumble(duration_ms=150):
+    threading.Thread(target=_rumble_thread, args=(duration_ms,)).start()
+
+
 def apply_deadzone(value, deadzone):
     if abs(value) <= deadzone:
         return 0.0
@@ -100,6 +154,7 @@ def callback(data):
     global motors_enabled
     global last_start_button_pressed
     global last_webgui_time
+    global last_locomotion_mode
 
     is_webgui = data.header.frame_id == "webgui"
     now = rospy.Time.now()
@@ -149,6 +204,10 @@ def callback(data):
     if is_button_pressed(data.buttons, controller_function_map["Y_button"]):
         locomotion_mode = LocomotionMode.CRABBING.value
 
+    if not is_webgui and locomotion_mode != last_locomotion_mode:
+        trigger_rumble(150)
+    last_locomotion_mode = locomotion_mode
+
     rover_cmd.locomotion_mode = locomotion_mode
 
     # Enable and disable motors