From 93772ec3b7edf6ae1c26ec991a218ff03a0c7c4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20M=C3=BCller?= Date: Fri, 29 May 2026 10:26:11 +0200 Subject: [PATCH] HUD-Farbe konfigurierbar, Neigungskalibrierung, Auto-Rotate verbessert MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - HUD-Farbe per Farbwähler im Admin einstellbar (localStorage) - Schwarze Kontur auf HUD-Elementen via CSS drop-shadow und canvas shadowBlur - Himmelsrichtungen auf Deutsch (O statt E, etc.) - Roll/Pitch-Neigung kann im Admin genullt werden (imu_tilt_calibration.json) - Neigungsoffset wird im Kamera-Overlay berücksichtigt - Auto-Rotate läuft jetzt nativ als rospy-Script im Docker-Container - Auto-Rotate erkennt aktuellen Locomotion-Mode und stellt ihn danach wieder her - Auto-Rotate pausiert Web-GUI-Joystick via localStorage-Flag - Feste 20%-Geschwindigkeit für Auto-Rotate (vel=4, Expo-äquivalent) - Vignette entfernt Co-Authored-By: Claude Sonnet 4.6 --- gui/admin.html | 3 + gui/index.html | 8 +- scripts/auto_rotate_ros.py | 117 ++++++++++++++++++++++++++ scripts/exomy_admin_api.py | 44 +++++----- scripts/imu_rotate.py | 163 ++++++++++++++++--------------------- 5 files changed, 221 insertions(+), 114 deletions(-) create mode 100644 scripts/auto_rotate_ros.py diff --git a/gui/admin.html b/gui/admin.html index 7e92933..5b3d3ec 100644 --- a/gui/admin.html +++ b/gui/admin.html @@ -2938,6 +2938,7 @@ if (!running && rotatePoller) { clearInterval(rotatePoller); rotatePoller = null; + try { localStorage.removeItem('autoRotateActive'); } catch(e) {} } } @@ -2954,6 +2955,7 @@ }); var data = await response.json(); if (!response.ok) throw new Error(data.status || ('status ' + response.status)); + try { localStorage.setItem('autoRotateActive', '1'); } catch(e) {} updateRotateUi(data.rotate); setAdminStatus(data.status || 'Drehung gestartet'); if (rotatePoller) clearInterval(rotatePoller); @@ -2969,6 +2971,7 @@ try { var response = await fetch(adminApiBase + '/api/imu/rotate-stop', { method: 'POST' }); var data = await response.json(); + try { localStorage.removeItem('autoRotateActive'); } catch(e) {} updateRotateUi(data.rotate); setAdminStatus(data.status || 'Drehung gestoppt'); } catch (e) { diff --git a/gui/index.html b/gui/index.html index 4c6ee82..6e2953f 100644 --- a/gui/index.html +++ b/gui/index.html @@ -827,8 +827,10 @@ function setRosStatus(text) { setText("ros-state", text); } +var autoRotateActive = false; + function controlsAreBlocked() { - return pageConnection.state === "offline"; + return pageConnection.state === "offline" || autoRotateActive; } function updateControlAvailability() { @@ -1572,6 +1574,10 @@ function toggleStatPanel() { window.addEventListener("storage", function (e) { if (e.key === "hudHexColor" && e.newValue) applyHudColor(e.newValue); + if (e.key === "autoRotateActive") { + autoRotateActive = e.newValue === "1"; + if (!autoRotateActive) { setAxes(0, 0); } + } }); window.addEventListener("load", function () { diff --git a/scripts/auto_rotate_ros.py b/scripts/auto_rotate_ros.py new file mode 100644 index 0000000..b98a31f --- /dev/null +++ b/scripts/auto_rotate_ros.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +Laeuft INNERHALB des Docker-Containers mit nativem rospy. +Dreht den Rover auf der Stelle zu einem Ziel-Heading und beendet sich. + +Aufruf: + python3 auto_rotate_ros.py + +Exit-Codes: + 0 = Ziel erreicht + 1 = Fehler +""" + +import math +import signal +import sys +import time + +import rospy +from exomy.msg import RoverCommand +from sensor_msgs.msg import Imu + +_POINT_TURN = 2 +_DONE_DEG = float(sys.argv[4]) if len(sys.argv) > 4 else 1.0 +_VEL = int(sys.argv[5]) if len(sys.argv) > 5 else 20 + +target_deg = float(sys.argv[1]) % 360 +restore_mode = int(sys.argv[2]) +heading_offset = float(sys.argv[3]) + +_running = True +_publisher = None +_heading = None + + +def _normalize(deg): + return (float(deg) % 360 + 360) % 360 + + +def _shortest_error(current, target): + return (target - current + 540) % 360 - 180 + + +def _on_imu(msg): + global _heading + o = msg.orientation + x, y, z, w = o.x, o.y, o.z, o.w + siny = 2.0 * (w * z + x * y) + cosy = 1.0 - 2.0 * (y * y + z * z) + yaw_deg = math.degrees(math.atan2(siny, cosy)) + _heading = _normalize(yaw_deg + heading_offset) + + +def _publish(locomotion_mode, vel, steering=0): + if _publisher is None: + return + cmd = RoverCommand() + cmd.connected = True + cmd.motors_enabled = True + cmd.locomotion_mode = locomotion_mode + cmd.vel = vel + cmd.steering = steering + _publisher.publish(cmd) + + +def _shutdown(signum, frame): + global _running + _running = False + + +def main(): + global _publisher, _running + + signal.signal(signal.SIGTERM, _shutdown) + signal.signal(signal.SIGINT, _shutdown) + + rospy.init_node('auto_rotate', anonymous=True, disable_signals=True) + + _publisher = rospy.Publisher('/rover_command', RoverCommand, queue_size=1) + rospy.Subscriber('/imu/data', Imu, _on_imu, queue_size=1) + + # Kurz warten bis IMU-Daten ankommen + deadline = time.time() + 5.0 + while _heading is None and time.time() < deadline and _running: + time.sleep(0.05) + + if _heading is None: + rospy.logerr('auto_rotate: keine IMU-Daten') + _publish(restore_mode, vel=0) + sys.exit(1) + + rate = rospy.Rate(50) # 50 Hz, passend zum PWM-Chip + + while _running and not rospy.is_shutdown(): + error = _shortest_error(_heading, target_deg) + + if abs(error) <= _DONE_DEG: + _publish(restore_mode, vel=0) + sys.exit(0) + + # Richtung wie Joystick: immer positiver vel, steering ±180 für Richtung + # steering=0 → Linksdrehung (deg < 85) + # steering=180 → Rechtsdrehung (deg >= 85) + if error > 0: + _publish(_POINT_TURN, vel=_VEL, steering=0) + else: + _publish(_POINT_TURN, vel=_VEL, steering=180) + rate.sleep() + + # Abbruch per Signal + _publish(restore_mode, vel=0) + sys.exit(0) + + +if __name__ == '__main__': + main() diff --git a/scripts/exomy_admin_api.py b/scripts/exomy_admin_api.py index ae5f2a8..96abdae 100644 --- a/scripts/exomy_admin_api.py +++ b/scripts/exomy_admin_api.py @@ -85,6 +85,7 @@ IMU_ROS_LOCK = threading.Lock() IMU_ROS_THREAD = None IMU_ROS_CLIENT = None IMU_ROS_TOPICS = [] +LAST_LOCOMOTION_MODE = 1 # ACKERMANN als Fallback IMU_ROS_CACHE = { 'state': 'unknown', 'status': 'Warte auf ROS', @@ -241,27 +242,23 @@ def _connect_imu_ros(): mag_topic = roslibpy.Topic(client, '/imu/mag', 'sensor_msgs/MagneticField') status_topic = roslibpy.Topic(client, '/imu/status', 'std_msgs/String') rover_cmd_topic = roslibpy.Topic(client, '/rover_command', 'exomy/RoverCommand') + def _on_rover_cmd_message(message): + global LAST_LOCOMOTION_MODE + import imu_rotate + if not imu_rotate.get_rotate_status()['running']: + mode = message.get('locomotion_mode') + if mode is not None: + LAST_LOCOMOTION_MODE = int(mode) + imu_topic.subscribe(_on_imu_data_message) mag_topic.subscribe(_on_imu_mag_message) status_topic.subscribe(_on_imu_status_message) + rover_cmd_topic.subscribe(_on_rover_cmd_message) rover_cmd_topic.advertise() IMU_ROS_CLIENT = client IMU_ROS_TOPICS = [imu_topic, mag_topic, status_topic, rover_cmd_topic] _imu_ros_cache_update(status='ROS verbunden', error='') - def _publish_rover_cmd(locomotion_mode, vel, steering): - try: - rover_cmd_topic.publish(roslibpy.Message({ - 'connected': True, - 'motors_enabled': True, - 'locomotion_mode': int(locomotion_mode), - 'vel': int(vel), - 'steering': int(steering), - })) - except Exception: - pass - - imu_rotate.set_publish_fn(_publish_rover_cmd) def _imu_ros_worker(): @@ -1976,7 +1973,17 @@ class Handler(http.server.BaseHTTPRequestHandler): def do_GET(self): if self.path == '/api/imu/rotate-status': import imu_rotate - self.write_json(200, {'rotate': imu_rotate.get_rotate_status()}) + status = imu_rotate.get_rotate_status() + if status.get('running') and status.get('target_deg') is not None: + calibration = read_imu_heading_calibration() + with IMU_ROS_LOCK: + raw = IMU_ROS_CACHE.get('heading_deg') + corrected = get_corrected_heading_deg(raw, calibration['offset_deg']) if raw is not None else None + status['current_deg'] = round(corrected, 1) if corrected is not None else None + if corrected is not None: + err = (status['target_deg'] - corrected + 540) % 360 - 180 + status['error_deg'] = round(err, 1) + self.write_json(200, {'rotate': status}) return if self.path == '/api/delay': self.write_json(200, {'delay_seconds': get_delay()}) @@ -2326,15 +2333,10 @@ class Handler(http.server.BaseHTTPRequestHandler): return import imu_rotate heading_offset = read_imu_heading_calibration()['offset_deg'] - - def _get_corrected_heading(): - with IMU_ROS_LOCK: - raw = IMU_ROS_CACHE.get('heading_deg') - return get_corrected_heading_deg(raw, heading_offset) if raw is not None else None - status = imu_rotate.start_rotate( target_deg=target_deg, - get_heading_fn=_get_corrected_heading, + heading_offset=heading_offset, + restore_mode=LAST_LOCOMOTION_MODE, ) self.write_json(200, {'status': 'Drehung gestartet', 'rotate': status}) return diff --git a/scripts/imu_rotate.py b/scripts/imu_rotate.py index 7e65460..2f4d510 100644 --- a/scripts/imu_rotate.py +++ b/scripts/imu_rotate.py @@ -1,131 +1,110 @@ """ -Dreht den Rover auf der Stelle zu einem Ziel-Heading. +Startet auto_rotate_ros.py via docker exec im ExoMy-Container. +Das Script läuft dort nativ mit rospy — kein WebSocket-Overhead. Öffentliche API: - set_publish_fn(fn) wird von exomy_admin_api nach ROS-Connect gesetzt - start_rotate(target_deg, get_heading_fn) -> dict + start_rotate(target_deg, heading_offset, restore_mode) -> dict stop_rotate() get_rotate_status() -> dict """ +import subprocess 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) +_CONTAINER = 'exomy_autostart' +_SCRIPT_PATH = '/root/exomy_ws/src/exomy/scripts/auto_rotate_ros.py' +_ROS_SETUP = 'source /opt/ros/melodic/setup.bash && source /root/exomy_ws/devel/setup.bash' -_lock = threading.Lock() -_thread = None -_stop_event = threading.Event() -_publish_fn = None +_lock = threading.Lock() +_proc = None # subprocess.Popen (docker exec) +_monitor_thread = 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: +def _monitor(proc, target_deg, restore_mode): + """Wartet auf Prozessende und aktualisiert Status.""" + global _proc + returncode = proc.wait() + with _lock: + if _proc is proc: # nicht überschrieben durch neuen Start + _proc = None _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) + if returncode == 0: + _status['result'] = 'done' + _status['message'] = 'Ziel erreicht' + elif returncode == -15 or returncode == 143: + _status['result'] = 'aborted' + _status['message'] = 'Abgebrochen' + else: + _status['result'] = 'error' + _status['message'] = 'Fehler (exit {})'.format(returncode) -def start_rotate(target_deg, get_heading_fn): - global _thread, _status +def start_rotate(target_deg, heading_offset, restore_mode=1, done_deg=1.0, vel=None): + global _proc, _monitor_thread, _status - target_deg = float(target_deg) % 360 stop_rotate() - _stop_event.clear() + target_deg = float(target_deg) % 360 + + if vel is None: + # Fest 20% Geschwindigkeit, unabhängig vom Admin-Speed-Limit + # Entspricht dem Joystick-Expo: 100 * (20/100)^2 = 4 + vel = 4 + + cmd = [ + 'docker', 'exec', _CONTAINER, 'bash', '-c', + '{setup} && python {script} {target} {mode} {offset} {done} {vel}'.format( + setup=_ROS_SETUP, + script=_SCRIPT_PATH, + target=target_deg, + mode=int(restore_mode), + offset=float(heading_offset), + done=float(done_deg), + vel=int(vel), + ) + ] + + proc = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + with _lock: + _proc = proc _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), + 'running': True, + 'target_deg': round(target_deg, 1), + 'result': 'running', + 'message': 'Drehe zu {:.0f}°'.format(target_deg), } - _thread = threading.Thread( - target=_control_loop, - args=(target_deg, get_heading_fn), + _monitor_thread = threading.Thread( + target=_monitor, + args=(proc, target_deg, restore_mode), daemon=True, ) - _thread.start() + _monitor_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 + global _proc + with _lock: + proc = _proc + _proc = None + + if proc is not None and proc.poll() is None: + # SIGTERM an den docker-exec-Prozess → Container-Prozess beendet sich sauber + proc.terminate() + try: + proc.wait(timeout=3.0) + except subprocess.TimeoutExpired: + proc.kill() def get_rotate_status():