Turn on Point kann jetzt schon im Admin aufgerufen werden um den Rover in eine Richtug zu drehen

2026-05-29 09:36:53 +02:00
parent 5e39a41f49
commit 4c24558516
3 changed files with 285 additions and 2 deletions
@@ -0,0 +1,133 @@
+"""
+Dreht den Rover auf der Stelle zu einem Ziel-Heading.
+
+Öffentliche API:
+    set_publish_fn(fn)   wird von exomy_admin_api nach ROS-Connect gesetzt
+    start_rotate(target_deg, get_heading_fn) -> dict
+    stop_rotate()
+    get_rotate_status() -> dict
+"""
+
+import threading
+import time
+
+_POINT_TURN = 2    # LocomotionMode.POINT_TURN
+_ACKERMANN  = 1    # LocomotionMode.ACKERMANN (Standard-Rückkehrmodus)
+_DONE_DEG   = 3.0  # Toleranz in Grad
+_VEL        = 20   # konstante Drehgeschwindigkeit (0–100), direkte Motoransteuerung
+_INTERVAL   = 0.02 # Sekunden zwischen Regelschritten (50 Hz)
+
+_lock       = threading.Lock()
+_thread     = None
+_stop_event = threading.Event()
+_publish_fn = None
+
+_status = {
+    'running':     False,
+    'target_deg':  None,
+    'current_deg': None,
+    'error_deg':   None,
+    'result':      'idle',
+    'message':     '',
+}
+
+
+def set_publish_fn(fn):
+    global _publish_fn
+    _publish_fn = fn
+
+
+def _publish(locomotion_mode, vel, steering):
+    fn = _publish_fn
+    if fn is not None:
+        fn(locomotion_mode=locomotion_mode, vel=vel, steering=steering)
+
+
+def _shortest_error(current, target):
+    """Vorzeichen: positiv = rechtsdrehung nötig, negativ = linksdrehung."""
+    return (target - current + 540) % 360 - 180
+
+
+def _restore(restore_mode):
+    _publish(restore_mode, vel=0, steering=0)
+
+
+def _control_loop(target_deg, get_heading_fn, restore_mode):
+    global _status
+
+    try:
+        while not _stop_event.is_set():
+            heading = get_heading_fn()
+            if heading is None:
+                time.sleep(0.05)
+                continue
+
+            error = _shortest_error(heading, target_deg)
+
+            with _lock:
+                _status['current_deg'] = round(heading, 1)
+                _status['error_deg']   = round(error, 1)
+
+            if abs(error) <= _DONE_DEG:
+                _restore(restore_mode)
+                with _lock:
+                    _status['running'] = False
+                    _status['result']  = 'done'
+                    _status['message'] = 'Ziel erreicht'
+                return
+
+            vel = _VEL if error > 0 else -_VEL
+            _publish(_POINT_TURN, vel=vel, steering=0)
+            time.sleep(_INTERVAL)
+
+        _restore(restore_mode)
+        with _lock:
+            _status['running'] = False
+            _status['result']  = 'aborted'
+            _status['message'] = 'Abgebrochen'
+
+    except Exception as exc:
+        _restore(restore_mode)
+        with _lock:
+            _status['running'] = False
+            _status['result']  = 'error'
+            _status['message'] = str(exc)
+
+
+def start_rotate(target_deg, get_heading_fn):
+    global _thread, _status
+
+    target_deg = float(target_deg) % 360
+    stop_rotate()
+
+    _stop_event.clear()
+    with _lock:
+        _status = {
+            'running':     True,
+            'target_deg':  round(target_deg, 1),
+            'current_deg': None,
+            'error_deg':   None,
+            'result':      'running',
+            'message':     'Drehe zu {0:.0f}°'.format(target_deg),
+        }
+
+    _thread = threading.Thread(
+        target=_control_loop,
+        args=(target_deg, get_heading_fn),
+        daemon=True,
+    )
+    _thread.start()
+    return get_rotate_status()
+
+
+def stop_rotate():
+    global _thread
+    if _thread is not None and _thread.is_alive():
+        _stop_event.set()
+        _thread.join(timeout=2.0)
+    _thread = None
+
+
+def get_rotate_status():
+    with _lock:
+        return dict(_status)