diff --git a/gui/admin.html b/gui/admin.html index cb0fc05..7e92933 100644 --- a/gui/admin.html +++ b/gui/admin.html @@ -180,6 +180,24 @@ +
+ + Ziel-Heading + + + ° (0=N, 90=O, 180=S, 270=W) + +
+
+ + +
+
Hinweis @@ -2890,6 +2908,83 @@ } } + var rotatePoller = null; + + function updateRotateUi(rotate) { + if (!rotate) return; + var running = rotate.result === 'running'; + var startBtn = document.getElementById('rotate_start_btn'); + var stopBtn = document.getElementById('rotate_stop_btn'); + var row = document.getElementById('rotate_status_row'); + var val = document.getElementById('rotate_status_val'); + var led = document.getElementById('rotate_status_led'); + if (startBtn) startBtn.style.display = running ? 'none' : ''; + if (stopBtn) stopBtn.style.display = running ? '' : 'none'; + if (row) row.style.display = rotate.result === 'idle' ? 'none' : ''; + if (led) { + led.className = 'sr-led ' + ( + rotate.result === 'done' ? 'led-ok' : + rotate.result === 'error' ? 'led-warn' : + rotate.result === 'aborted' ? 'led-warn' : 'led-blue' + ); + } + if (val) { + var txt = rotate.message || '-'; + if (running && rotate.current_deg !== null && rotate.error_deg !== null) { + txt += ' | Ist: ' + rotate.current_deg + '° | Rest: ' + rotate.error_deg + '°'; + } + val.textContent = txt; + } + if (!running && rotatePoller) { + clearInterval(rotatePoller); + rotatePoller = null; + } + } + + async function startRotate() { + var input = document.getElementById('rotate_target'); + var target = parseFloat(input ? input.value : 0); + if (isNaN(target)) { alert('Ungültiger Winkel'); return; } + setAdminStatus('Starte Drehung zu ' + target + '°…'); + try { + var response = await fetch(adminApiBase + '/api/imu/rotate-to', { + method: 'POST', + headers: {'Content-Type': 'application/json'}, + body: JSON.stringify({target_deg: target}) + }); + var data = await response.json(); + if (!response.ok) throw new Error(data.status || ('status ' + response.status)); + updateRotateUi(data.rotate); + setAdminStatus(data.status || 'Drehung gestartet'); + if (rotatePoller) clearInterval(rotatePoller); + rotatePoller = setInterval(pollRotateStatus, 300); + } catch (e) { + setAdminStatus('Fehler: ' + (e.message || 'unbekannt')); + alert(e.message || 'Drehung konnte nicht gestartet werden.'); + } + } + + async function stopRotate() { + setAdminStatus('Stoppe Drehung…'); + try { + var response = await fetch(adminApiBase + '/api/imu/rotate-stop', { method: 'POST' }); + var data = await response.json(); + updateRotateUi(data.rotate); + setAdminStatus(data.status || 'Drehung gestoppt'); + } catch (e) { + setAdminStatus('Fehler beim Stoppen'); + } + } + + async function pollRotateStatus() { + try { + var response = await fetch(adminApiBase + '/api/imu/rotate-status'); + if (!response.ok) return; + var data = await response.json(); + updateRotateUi(data.rotate); + } catch (e) {} + } + async function fetchDelay() { try { var response = await fetch(adminApiBase + '/api/delay'); diff --git a/scripts/exomy_admin_api.py b/scripts/exomy_admin_api.py index 2d12ea1..ae5f2a8 100644 --- a/scripts/exomy_admin_api.py +++ b/scripts/exomy_admin_api.py @@ -227,6 +227,7 @@ def _connect_imu_ros(): global IMU_ROS_CLIENT, IMU_ROS_TOPICS import roslibpy + import imu_rotate client = roslibpy.Ros(host=ROSBRIDGE_HOST, port=ROSBRIDGE_PORT) client.run() @@ -239,13 +240,29 @@ def _connect_imu_ros(): imu_topic = roslibpy.Topic(client, '/imu/data', 'sensor_msgs/Imu') 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') imu_topic.subscribe(_on_imu_data_message) mag_topic.subscribe(_on_imu_mag_message) status_topic.subscribe(_on_imu_status_message) + rover_cmd_topic.advertise() IMU_ROS_CLIENT = client - IMU_ROS_TOPICS = [imu_topic, mag_topic, status_topic] + 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(): global IMU_ROS_CLIENT, IMU_ROS_TOPICS @@ -1957,6 +1974,10 @@ class Handler(http.server.BaseHTTPRequestHandler): self.end_headers() def do_GET(self): + if self.path == '/api/imu/rotate-status': + import imu_rotate + self.write_json(200, {'rotate': imu_rotate.get_rotate_status()}) + return if self.path == '/api/delay': self.write_json(200, {'delay_seconds': get_delay()}) return @@ -1999,7 +2020,8 @@ class Handler(http.server.BaseHTTPRequestHandler): def do_POST(self): raw_body = b'{}' - if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset'): + if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset', + '/api/imu/rotate-to'): try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: @@ -2290,6 +2312,39 @@ class Handler(http.server.BaseHTTPRequestHandler): self.write_json(status_code, response) return + if self.path == '/api/imu/rotate-to': + try: + payload = json.loads(raw_body.decode('utf-8')) + target_deg = float(payload.get('target_deg', 0)) + except (UnicodeDecodeError, json.JSONDecodeError, TypeError, ValueError): + self.write_json(400, {'status': 'Ungültige Anfrage'}) + return + with IMU_ROS_LOCK: + heading = IMU_ROS_CACHE.get('heading_deg') + if heading is None: + self.write_json(409, {'status': 'Noch keine frischen IMU-Daten'}) + 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, + ) + self.write_json(200, {'status': 'Drehung gestartet', 'rotate': status}) + return + + if self.path == '/api/imu/rotate-stop': + import imu_rotate + imu_rotate.stop_rotate() + self.write_json(200, {'status': 'Drehung gestoppt', 'rotate': imu_rotate.get_rotate_status()}) + return + actions = { '/api/cold-start-gps': { 'handler': cold_start_gps_receiver, diff --git a/scripts/imu_rotate.py b/scripts/imu_rotate.py new file mode 100644 index 0000000..7e65460 --- /dev/null +++ b/scripts/imu_rotate.py @@ -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)