134 lines
3.4 KiB
Python
134 lines
3.4 KiB
Python
"""
|
||
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)
|