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():