Rückmeldung vibration

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