HUD-Farbe konfigurierbar, Neigungskalibrierung, Auto-Rotate verbessert
- 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 <noreply@anthropic.com>
This commit is contained in:
Claude Sonnet 4.6
@@ -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 <target_deg> <restore_mode> <heading_offset_deg>
|
||||
|
||||
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()
|
||||
+23
-21
@@ -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
|
||||
|
||||
+71
-92
@@ -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():
|
||||
|
||||
Reference in New Issue
Block a user