#!/usr/bin/env python3 import datetime import errno import fcntl import glob import http.server import json import math import os import shutil import socket import socketserver import struct import subprocess import termios import threading import time import warnings from zoneinfo import ZoneInfo PORT = 8082 EXOMY_CONTAINER = 'exomy_autostart' DELAY_FILE = '/tmp/exomy_delay.txt' SPEED_LIMIT_FILE = '/tmp/exomy_speed_limit.txt' DRIVE_ESTIMATOR_CALIBRATION_FILE = os.path.join( os.path.dirname(__file__), '..', 'config', 'drive_estimator_calibration.json' ) ROUTE_LIBRARY_FILE = os.path.join( os.path.dirname(__file__), '..', 'config', 'route_library.json' ) CAMERA_RUNTIME_SETTINGS_FILE = '/tmp/exomy_camera_settings.json' IMU_HEADING_CALIBRATION_FILE = os.path.join( os.path.dirname(__file__), '..', 'config', 'imu_heading_calibration.json' ) IMU_TILT_CALIBRATION_FILE = os.path.join( os.path.dirname(__file__), '..', 'config', 'imu_tilt_calibration.json' ) MOTOR_TEST_LOCK = threading.Lock() GPS_PORT_PATTERNS = [ '/dev/serial/by-id/*', '/dev/serial/by-path/*', '/dev/ttyUSB*', '/dev/ttyACM*', ] GPS_BAUD_RATE = 4800 GPS_COLD_START_COMMAND = '$PAIR006*3C\r\n' CAMERA_PROFILES = [ { 'id': '640x480', 'label': '640 x 480', 'description': '4:3, klein und sparsam', 'width': 640, 'height': 480, 'fps': 10, }, { 'id': '1024x768', 'label': '1024 x 768', 'description': '4:3, großes Sichtfeld mit mehr Details', 'width': 1024, 'height': 768, 'fps': 10, }, { 'id': '1296x972', 'label': '1296 x 972', 'description': '4:3, großes Sichtfeld mit noch mehr Details', 'width': 1296, 'height': 972, 'fps': 10, }, ] CAMERA_PROFILE_MAP = {profile['id']: profile for profile in CAMERA_PROFILES} DEFAULT_CAMERA_PROFILE_ID = '1296x972' CAMERA_FPS_OPTIONS = [1, 2, 3, 4, 5, 7, 10, 20] DEFAULT_CAMERA_FPS = 10 IMU_I2C_ADDRESS = '0x4A' IMU_LOCK = threading.Lock() IMU_SENSOR = None IMU_LAST_ERROR = None IMU_WORKAROUND_APPLIED = False ROSBRIDGE_HOST = '127.0.0.1' ROSBRIDGE_PORT = 9090 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', 'address': IMU_I2C_ADDRESS, 'acceleration': 'unbekannt', 'gyro': 'unbekannt', 'magnetic': 'unbekannt', 'quaternion': 'unbekannt', 'roll_pitch_yaw': 'unbekannt', 'roll_deg': None, 'pitch_deg': None, 'error': 'Noch keine IMU-Daten aus ROS', 'heading_deg': None, 'updated_at': 0.0, } class ReusableThreadingTCPServer(socketserver.ThreadingTCPServer): allow_reuse_address = True def run_command(command): result = subprocess.run(command, capture_output=True, text=True, check=False) return result.stdout.strip(), result.stderr.strip(), result.returncode def _format_vector(values, unit, decimals=2): if not values: return 'unbekannt' return 'X {0:.{d}f} | Y {1:.{d}f} | Z {2:.{d}f} {u}'.format( values[0], values[1], values[2], d=decimals, u=unit ) def _format_quaternion(values): if not values: return 'unbekannt' return 'I {0:.3f} | J {1:.3f} | K {2:.3f} | R {3:.3f}'.format( values[0], values[1], values[2], values[3] ) def _format_euler_deg(values): if not values: return 'unbekannt' return 'Roll {0:.1f}° | Pitch {1:.1f}° | Yaw {2:.1f}°'.format( values[0], values[1], values[2] ) def _quaternion_to_euler_deg(values): if not values: return None x, y, z, w = values sinr_cosp = 2.0 * (w * x + y * z) cosr_cosp = 1.0 - 2.0 * (x * x + y * y) roll = math.atan2(sinr_cosp, cosr_cosp) sinp = 2.0 * (w * y - z * x) if abs(sinp) >= 1.0: pitch = math.copysign(math.pi / 2.0, sinp) else: pitch = math.asin(sinp) siny_cosp = 2.0 * (w * z + x * y) cosy_cosp = 1.0 - 2.0 * (y * y + z * z) yaw = math.atan2(siny_cosp, cosy_cosp) return ( math.degrees(roll), math.degrees(pitch), math.degrees(yaw), ) def _normalize_heading_deg(value): heading = float(value) % 360.0 if heading < 0.0: heading += 360.0 return heading def _imu_ros_cache_update(**kwargs): with IMU_ROS_LOCK: IMU_ROS_CACHE.update(kwargs) IMU_ROS_CACHE['updated_at'] = time.time() def _on_imu_data_message(message): orientation = message.get('orientation') or {} angular_velocity = message.get('angular_velocity') or {} linear_acceleration = message.get('linear_acceleration') or {} quaternion = ( float(orientation.get('x', 0.0)), float(orientation.get('y', 0.0)), float(orientation.get('z', 0.0)), float(orientation.get('w', 1.0)), ) euler_deg = _quaternion_to_euler_deg(quaternion) _imu_ros_cache_update( state='active', acceleration=_format_vector(( float(linear_acceleration.get('x', 0.0)), float(linear_acceleration.get('y', 0.0)), float(linear_acceleration.get('z', 0.0)), ), 'm/s^2'), gyro=_format_vector(( float(angular_velocity.get('x', 0.0)), float(angular_velocity.get('y', 0.0)), float(angular_velocity.get('z', 0.0)), ), 'rad/s', 3), quaternion=_format_quaternion(quaternion), roll_pitch_yaw=_format_euler_deg(euler_deg), roll_deg=None if not euler_deg else euler_deg[0], pitch_deg=None if not euler_deg else euler_deg[1], heading_deg=None if not euler_deg else _normalize_heading_deg(euler_deg[2]), error='', ) def _on_imu_mag_message(message): magnetic_field = message.get('magnetic_field') or {} _imu_ros_cache_update( magnetic=_format_vector(( float(magnetic_field.get('x', 0.0)) * 1000000.0, float(magnetic_field.get('y', 0.0)) * 1000000.0, float(magnetic_field.get('z', 0.0)) * 1000000.0, ), 'uT'), ) def _on_imu_status_message(message): status_text = message.get('data') or 'Verbunden' _imu_ros_cache_update(status=status_text) 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() start_time = time.time() while not client.is_connected: if time.time() - start_time > 10.0: raise RuntimeError('rosbridge timeout') time.sleep(0.1) 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') 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 _imu_ros_worker(): global IMU_ROS_CLIENT, IMU_ROS_TOPICS while True: try: _connect_imu_ros() while IMU_ROS_CLIENT is not None and IMU_ROS_CLIENT.is_connected: time.sleep(1.0) except Exception as exc: _imu_ros_cache_update( state='unknown', status='ROS getrennt', error='ROS IMU nicht erreichbar: {0}'.format(exc), ) time.sleep(2.0) finally: for topic in IMU_ROS_TOPICS: try: topic.unsubscribe() except Exception: pass if IMU_ROS_CLIENT is not None: try: IMU_ROS_CLIENT.terminate() except Exception: pass IMU_ROS_CLIENT = None IMU_ROS_TOPICS = [] def ensure_imu_ros_bridge(): global IMU_ROS_THREAD with IMU_ROS_LOCK: if IMU_ROS_THREAD is not None and IMU_ROS_THREAD.is_alive(): return IMU_ROS_THREAD = threading.Thread(target=_imu_ros_worker, daemon=True) IMU_ROS_THREAD.start() def read_imu_ros_status(): ensure_imu_ros_bridge() with IMU_ROS_LOCK: result = dict(IMU_ROS_CACHE) if time.time() - result.get('updated_at', 0.0) > 5.0: result['state'] = 'unknown' if result.get('status') in ('Verbunden', 'ROS verbunden'): result['status'] = 'Keine frischen Daten' if not result.get('error'): result['error'] = 'IMU-Topic sendet gerade keine neuen Daten' result.pop('updated_at', None) return result def _apply_bno08x_workaround(module): global IMU_WORKAROUND_APPLIED if IMU_WORKAROUND_APPLIED: return original_handle_packet = module.BNO08X._handle_packet def safe_handle_packet(self, packet): try: return original_handle_packet(self, packet) except KeyError as exc: # Some BNO085 firmwares emit undocumented reports that the Adafruit # library cannot decode yet. Ignore those packets and keep known # sensor reports flowing. if len(exc.args) == 1 and isinstance(exc.args[0], int): return raise module.BNO08X._handle_packet = safe_handle_packet IMU_WORKAROUND_APPLIED = True def _create_imu_sensor(): global IMU_LAST_ERROR try: with warnings.catch_warnings(): warnings.filterwarnings( 'ignore', message='I2C frequency is not settable in python, ignoring!', category=RuntimeWarning, ) import adafruit_bno08x import board import busio from adafruit_bno08x import ( BNO_REPORT_ACCELEROMETER, BNO_REPORT_GYROSCOPE, BNO_REPORT_MAGNETOMETER, BNO_REPORT_ROTATION_VECTOR, ) from adafruit_bno08x.i2c import BNO08X_I2C _apply_bno08x_workaround(adafruit_bno08x) i2c = busio.I2C(board.SCL, board.SDA, frequency=400000) sensor = BNO08X_I2C(i2c) for feature in ( BNO_REPORT_ACCELEROMETER, BNO_REPORT_GYROSCOPE, BNO_REPORT_MAGNETOMETER, BNO_REPORT_ROTATION_VECTOR, ): sensor.enable_feature(feature) time.sleep(0.2) IMU_LAST_ERROR = None return sensor except Exception as exc: IMU_LAST_ERROR = str(exc) return None def read_imu_status(): global IMU_SENSOR, IMU_LAST_ERROR with IMU_LOCK: if IMU_SENSOR is None: IMU_SENSOR = _create_imu_sensor() if IMU_SENSOR is None: return { 'state': 'unknown', 'status': 'Nicht bereit', 'address': IMU_I2C_ADDRESS, 'acceleration': 'unbekannt', 'gyro': 'unbekannt', 'magnetic': 'unbekannt', 'quaternion': 'unbekannt', 'roll_pitch_yaw': 'unbekannt', 'error': IMU_LAST_ERROR or 'BNO085 nicht verfugbar', } try: quaternion = IMU_SENSOR.quaternion return { 'state': 'active', 'status': 'Verbunden', 'address': IMU_I2C_ADDRESS, 'acceleration': _format_vector(IMU_SENSOR.acceleration, 'm/s^2'), 'gyro': _format_vector(IMU_SENSOR.gyro, 'rad/s', 3), 'magnetic': _format_vector(IMU_SENSOR.magnetic, 'uT'), 'quaternion': _format_quaternion(quaternion), 'roll_pitch_yaw': _format_euler_deg(_quaternion_to_euler_deg(quaternion)), 'error': '', } except Exception as exc: IMU_SENSOR = None IMU_LAST_ERROR = str(exc) return { 'state': 'unknown', 'status': 'Lesefehler', 'address': IMU_I2C_ADDRESS, 'acceleration': 'unbekannt', 'gyro': 'unbekannt', 'magnetic': 'unbekannt', 'quaternion': 'unbekannt', 'roll_pitch_yaw': 'unbekannt', 'error': IMU_LAST_ERROR, } def read_cpu_temperature(): try: with open('/sys/class/thermal/thermal_zone0/temp', 'r', encoding='utf-8') as handle: raw_value = handle.read().strip() return f'{int(raw_value) / 1000:.1f} °C' except (FileNotFoundError, ValueError): return 'unbekannt' def read_gpu_temperature(): stdout, _, returncode = run_command(['vcgencmd', 'measure_temp']) if returncode != 0 or not stdout or '=' not in stdout: return 'unbekannt' value = stdout.split('=', 1)[1].strip().replace("'C", ' °C') return value or 'unbekannt' def read_cpu_usage(): try: with open('/proc/stat', 'r', encoding='utf-8') as handle: first = handle.readline().split()[1:] first = [int(value) for value in first] idle_first = first[3] + first[4] total_first = sum(first) time.sleep(0.12) with open('/proc/stat', 'r', encoding='utf-8') as handle: second = handle.readline().split()[1:] second = [int(value) for value in second] idle_second = second[3] + second[4] total_second = sum(second) total_delta = total_second - total_first idle_delta = idle_second - idle_first if total_delta <= 0: return 'unbekannt' usage = (1 - (idle_delta / total_delta)) * 100 return f'{usage:.0f}%' except (FileNotFoundError, ValueError, IndexError): return 'unbekannt' def read_per_core_usage(): try: with open('/proc/stat', 'r', encoding='utf-8') as handle: first_lines = [line.split() for line in handle if line.startswith('cpu') and line[3:4].isdigit()] time.sleep(0.12) with open('/proc/stat', 'r', encoding='utf-8') as handle: second_lines = [line.split() for line in handle if line.startswith('cpu') and line[3:4].isdigit()] except (FileNotFoundError, ValueError): return 'unbekannt' if len(first_lines) != len(second_lines) or not first_lines: return 'unbekannt' results = [] for first, second in zip(first_lines, second_lines): try: name = first[0] first_values = [int(value) for value in first[1:]] second_values = [int(value) for value in second[1:]] idle_first = first_values[3] + first_values[4] idle_second = second_values[3] + second_values[4] total_first = sum(first_values) total_second = sum(second_values) total_delta = total_second - total_first idle_delta = idle_second - idle_first if total_delta <= 0: continue usage = (1 - (idle_delta / total_delta)) * 100 results.append(f'{name.upper()}: {usage:.0f}%') except (ValueError, IndexError): continue return ' | '.join(results) if results else 'unbekannt' def read_memory_usage(): try: values = {} with open('/proc/meminfo', 'r', encoding='utf-8') as handle: for line in handle: key, raw_value = line.split(':', 1) values[key] = int(raw_value.strip().split()[0]) total = values.get('MemTotal') available = values.get('MemAvailable') if not total or available is None: return 'unbekannt' used = total - available percent = (used / total) * 100 return f'{percent:.0f}%' except (FileNotFoundError, ValueError): return 'unbekannt' def read_memory_details(): try: values = {} with open('/proc/meminfo', 'r', encoding='utf-8') as handle: for line in handle: key, raw_value = line.split(':', 1) values[key] = int(raw_value.strip().split()[0]) total = values.get('MemTotal') available = values.get('MemAvailable') if not total or available is None: return 'unbekannt' used = total - available total_mb = total / 1024 used_mb = used / 1024 free_mb = available / 1024 percent = (used / total) * 100 return f'{used_mb:.0f} MB belegt / {free_mb:.0f} MB frei / {total_mb:.0f} MB gesamt ({percent:.0f}%)' except (FileNotFoundError, ValueError): return 'unbekannt' def read_undervoltage_status(): stdout, _, returncode = run_command(['vcgencmd', 'get_throttled']) if returncode != 0 or not stdout or '=' not in stdout: return { 'text': 'unbekannt', 'state': 'unknown', } try: throttled_value = int(stdout.split('=', 1)[1].strip(), 16) except ValueError: return { 'text': 'unbekannt', 'state': 'unknown', } undervoltage_now = bool(throttled_value & 0x1) undervoltage_occurred = bool(throttled_value & 0x10000) if undervoltage_now: return { 'text': 'Ja, aktuell', 'state': 'active', } if undervoltage_occurred: return { 'text': 'Früher erkannt', 'state': 'past', } return { 'text': 'Nein', 'state': 'clear', } def read_throttling_details(): stdout, _, returncode = run_command(['vcgencmd', 'get_throttled']) if returncode != 0 or not stdout or '=' not in stdout: return 'unbekannt' try: throttled_value = int(stdout.split('=', 1)[1].strip(), 16) except ValueError: return 'unbekannt' flags = [ (0x1, 'Unterspannung aktuell'), (0x2, 'ARM gedrosselt aktuell'), (0x4, 'aktuell limitiert'), (0x8, 'Soft-Temperaturlimit aktuell'), (0x10000, 'Unterspannung früher'), (0x20000, 'ARM früher gedrosselt'), (0x40000, 'früher limitiert'), (0x80000, 'Soft-Temperaturlimit früher'), ] active = [label for bitmask, label in flags if throttled_value & bitmask] if not active: return 'keine Auffälligkeiten' return ' | '.join(active) def format_uptime(): try: with open('/proc/uptime', 'r', encoding='utf-8') as handle: uptime_seconds = int(float(handle.read().split()[0])) except (FileNotFoundError, ValueError, IndexError): return 'unbekannt' days, remainder = divmod(uptime_seconds, 86400) hours, remainder = divmod(remainder, 3600) minutes, _ = divmod(remainder, 60) parts = [] if days: parts.append(f'{days}d') if days or hours: parts.append(f'{hours}h') parts.append(f'{minutes}m') return ' '.join(parts) def format_disk_free(): usage = shutil.disk_usage('/') free_gb = usage.free / (1024 ** 3) total_gb = usage.total / (1024 ** 3) return f'{free_gb:.1f} GB frei / {total_gb:.1f} GB' def format_path_disk_usage(path): try: usage = shutil.disk_usage(path) free_gb = usage.free / (1024 ** 3) used_gb = usage.used / (1024 ** 3) total_gb = usage.total / (1024 ** 3) return f'{used_gb:.1f} GB belegt / {free_gb:.1f} GB frei / {total_gb:.1f} GB' except FileNotFoundError: return 'unbekannt' def read_inode_free(path='/'): try: stats = os.statvfs(path) total = stats.f_files free = stats.f_ffree used = total - free if total <= 0: return 'unbekannt' percent_used = (used / total) * 100 return f'{free:,} frei / {total:,} gesamt ({percent_used:.0f}% belegt)'.replace(',', '.') except OSError: return 'unbekannt' def read_load_average(): try: load_1, load_5, load_15 = os.getloadavg() return f'{load_1:.2f} / {load_5:.2f} / {load_15:.2f}' except (AttributeError, OSError): return 'unbekannt' def get_ip_addresses(): stdout, _, returncode = run_command(['ip', '-4', '-o', 'addr', 'show', 'dev', 'wlan0', 'scope', 'global']) if returncode == 0 and stdout: for line in stdout.splitlines(): parts = line.split() if 'inet' in parts: inet_index = parts.index('inet') if inet_index + 1 < len(parts): return parts[inet_index + 1].split('/')[0] stdout, _, returncode = run_command(['hostname', '-I']) if returncode != 0 or not stdout: return 'unbekannt' ipv4_addresses = [] for item in stdout.split(): if item.count('.') == 3 and not item.startswith('172.17.'): ipv4_addresses.append(item) return ipv4_addresses[0] if ipv4_addresses else 'unbekannt' def get_wifi_status(): stdout, _, returncode = run_command(['nmcli', '-t', '-f', 'DEVICE,STATE,CONNECTION', 'device']) if returncode != 0 or not stdout: return 'unbekannt' for line in stdout.splitlines(): parts = line.split(':', 2) if len(parts) != 3: continue device, state, connection = parts if device == 'wlan0': if state == 'connected': ssid_stdout, _, ssid_returncode = run_command(['nmcli', '-t', '-f', 'ACTIVE,SSID', 'device', 'wifi', 'list', 'ifname', 'wlan0']) if ssid_returncode == 0 and ssid_stdout: for ssid_line in ssid_stdout.splitlines(): if ssid_line.startswith('yes:'): active_ssid = ssid_line.split(':', 1)[1].strip() if active_ssid: return f'verbunden: {active_ssid}' return f'verbunden: {connection}' return state return 'kein wlan0' def get_wifi_details(): details = { 'ssid': 'unbekannt', 'signal_percent': 'unbekannt', 'signal_dbm': 'unbekannt', 'channel': 'unbekannt', 'frequency': 'unbekannt', } stdout, _, returncode = run_command(['nmcli', '-t', '-f', 'ACTIVE,SSID,SIGNAL,CHAN,FREQ,BARS', 'device', 'wifi', 'list', 'ifname', 'wlan0']) if returncode != 0 or not stdout: return details for line in stdout.splitlines(): if not line.startswith('yes:'): continue parts = line.split(':') if len(parts) >= 6: ssid = parts[1].strip() signal = parts[2].strip() channel = parts[3].strip() frequency = parts[4].strip() details['ssid'] = ssid or 'unbekannt' details['signal_percent'] = f'{signal} %' if signal else 'unbekannt' details['channel'] = channel or 'unbekannt' details['frequency'] = frequency or 'unbekannt' try: dbm = (int(float(signal)) / 2) - 100 details['signal_dbm'] = f'{dbm:.0f} dBm' except ValueError: details['signal_dbm'] = 'unbekannt' break return details def get_wifi_signal(): details = get_wifi_details() return details['signal_percent'] def get_hostname(): try: return socket.gethostname() except Exception: return 'unbekannt' def get_kernel_version(): stdout, _, returncode = run_command(['uname', '-r']) if returncode == 0 and stdout: return stdout return 'unbekannt' def get_pi_model(): try: with open('/proc/device-tree/model', 'r', encoding='utf-8') as handle: return handle.read().strip('\x00').strip() or 'unbekannt' except (FileNotFoundError, ValueError): return 'unbekannt' def get_system_time_details(): local_time = datetime.datetime.now().astimezone() timedatectl_stdout, _, timedatectl_returncode = run_command(['timedatectl', 'show', '-p', 'NTPSynchronized', '-p', 'SystemClockSynchronized', '-p', 'Timezone']) ntp_sync = 'unbekannt' clock_sync = 'unbekannt' timezone_name = local_time.tzname() or 'unbekannt' if timedatectl_returncode == 0 and timedatectl_stdout: for line in timedatectl_stdout.splitlines(): if line.startswith('NTPSynchronized='): ntp_sync = 'ja' if line.split('=', 1)[1].strip().lower() == 'yes' else 'nein' elif line.startswith('SystemClockSynchronized='): clock_sync = 'ja' if line.split('=', 1)[1].strip().lower() == 'yes' else 'nein' elif line.startswith('Timezone='): timezone_name = line.split('=', 1)[1].strip() or timezone_name return { 'local': local_time.strftime('%Y-%m-%d %H:%M:%S'), 'timezone': timezone_name, 'ntp_sync': ntp_sync, 'clock_sync': clock_sync, } def get_systemd_state(service_name): stdout, _, returncode = run_command(['systemctl', 'is-active', service_name]) if returncode == 0 and stdout: return stdout return stdout or 'unknown' def get_container_state(container_name): stdout, _, returncode = run_command(['docker', 'inspect', '-f', '{{.State.Status}}', container_name]) if returncode == 0 and stdout: return stdout return 'unknown' def get_container_details(container_name): stdout, _, returncode = run_command(['docker', 'inspect', container_name]) if returncode != 0 or not stdout: return { 'uptime': 'unbekannt', 'restarts': 'unbekannt', 'docker_disk': 'unbekannt', } try: payload = json.loads(stdout) info = payload[0] if payload else {} state = info.get('State', {}) started_at = state.get('StartedAt') restart_count = info.get('RestartCount', state.get('RestartCount', 'unbekannt')) uptime = 'unbekannt' if started_at and started_at != '0001-01-01T00:00:00Z': started = datetime.datetime.fromisoformat(started_at.replace('Z', '+00:00')) delta = datetime.datetime.now(datetime.timezone.utc) - started uptime = format_duration(int(delta.total_seconds())) return { 'uptime': uptime, 'restarts': str(restart_count), 'docker_disk': get_docker_disk_usage(), } except (ValueError, KeyError, TypeError, IndexError): return { 'uptime': 'unbekannt', 'restarts': 'unbekannt', 'docker_disk': 'unbekannt', } def get_docker_disk_usage(): stdout, _, returncode = run_command(['docker', 'info', '-f', '{{.DockerRootDir}}']) if returncode != 0 or not stdout: return 'unbekannt' return format_path_disk_usage(stdout.strip()) def format_duration(seconds): days, remainder = divmod(max(0, int(seconds)), 86400) hours, remainder = divmod(remainder, 3600) minutes, _ = divmod(remainder, 60) parts = [] if days: parts.append(f'{days}d') if days or hours: parts.append(f'{hours}h') parts.append(f'{minutes}m') return ' '.join(parts) def get_motor_test_container_status(): state = get_container_state(EXOMY_CONTAINER) return { 'container': EXOMY_CONTAINER, 'state': state, 'is_safe_for_motor_test': state in ('exited', 'stopped', 'created'), } def collect_status(): undervoltage = read_undervoltage_status() wifi_details = get_wifi_details() time_details = get_system_time_details() container_details = get_container_details(EXOMY_CONTAINER) imu_details = read_imu_ros_status() imu_heading = get_imu_heading_status() imu_tilt = get_imu_tilt_status() return { 'status': 'Bereit', 'system': { 'wifi': get_wifi_status(), 'wifi_signal': get_wifi_signal(), 'wifi_signal_dbm': wifi_details['signal_dbm'], 'wifi_ssid': wifi_details['ssid'], 'wifi_channel': wifi_details['channel'], 'wifi_frequency': wifi_details['frequency'], 'ips': get_ip_addresses(), 'undervoltage': undervoltage['text'], 'undervoltage_state': undervoltage['state'], 'throttling_details': read_throttling_details(), 'cpu_temperature': read_cpu_temperature(), 'gpu_temperature': read_gpu_temperature(), 'cpu_usage': read_cpu_usage(), 'cpu_usage_per_core': read_per_core_usage(), 'memory_usage': read_memory_usage(), 'memory_details': read_memory_details(), 'uptime': format_uptime(), 'disk_free': format_disk_free(), 'disk_root': format_path_disk_usage('/'), 'disk_docker': container_details['docker_disk'], 'inode_free': read_inode_free('/'), 'load_average': read_load_average(), 'hostname': get_hostname(), 'kernel_version': get_kernel_version(), 'pi_model': get_pi_model(), 'system_time': time_details['local'], 'system_timezone': time_details['timezone'], 'ntp_sync': time_details['ntp_sync'], 'clock_sync': time_details['clock_sync'], }, 'services': { 'camera': get_systemd_state('exomy-camera-stream.service'), 'camera_settings_version': read_camera_settings_version(), 'admin_api': get_systemd_state('exomy-admin-api.service'), 'exomy': get_container_state(EXOMY_CONTAINER), 'exomy_uptime': container_details['uptime'], 'exomy_restarts': container_details['restarts'], }, 'imu': imu_details, 'imu_heading': imu_heading, 'imu_tilt': imu_tilt, } def get_delay(): try: with open(DELAY_FILE) as f: return max(0.0, min(10.0, float(f.read().strip()))) except Exception: return 0.0 def set_delay(seconds): seconds = max(0.0, min(10.0, float(seconds))) data = str(seconds).encode() try: fd = os.open(DELAY_FILE, os.O_WRONLY | os.O_TRUNC) except FileNotFoundError: fd = os.open(DELAY_FILE, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o666) try: os.write(fd, data) finally: os.close(fd) subprocess.run( ['docker', 'exec', EXOMY_CONTAINER, 'bash', '-c', f'source /opt/ros/melodic/setup.bash && rosparam set /delay_seconds {seconds}'], capture_output=True, check=False ) return seconds def get_speed_limit(): try: with open(SPEED_LIMIT_FILE) as f: return max(10, min(100, int(f.read().strip()))) except Exception: return 100 def set_speed_limit(percent): percent = max(10, min(100, int(percent))) data = str(percent).encode() try: fd = os.open(SPEED_LIMIT_FILE, os.O_WRONLY | os.O_TRUNC) except FileNotFoundError: fd = os.open(SPEED_LIMIT_FILE, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o666) try: os.write(fd, data) finally: os.close(fd) subprocess.run( ['docker', 'exec', EXOMY_CONTAINER, 'bash', '-c', f'source /opt/ros/melodic/setup.bash && rosparam set /speed_limit_percent {percent}'], capture_output=True, check=False ) return percent def get_default_camera_profile(): return dict(CAMERA_PROFILE_MAP[DEFAULT_CAMERA_PROFILE_ID]) def normalize_camera_profile(profile_id): profile = CAMERA_PROFILE_MAP.get(str(profile_id or '').strip()) if profile is None: raise ValueError('Unbekanntes Kamera-Profil') return dict(profile) def normalize_camera_fps(value): try: fps = int(value) except (TypeError, ValueError): raise ValueError('Unbekannte Bildrate') if fps not in CAMERA_FPS_OPTIONS: raise ValueError('Unbekannte Bildrate') return fps def read_camera_settings(): default_profile = get_default_camera_profile() try: with open(CAMERA_RUNTIME_SETTINGS_FILE, 'r', encoding='utf-8') as handle: payload = json.load(handle) except (FileNotFoundError, json.JSONDecodeError, OSError, ValueError, TypeError): return default_profile try: width = int(payload.get('width', default_profile['width'])) height = int(payload.get('height', default_profile['height'])) fps = normalize_camera_fps(payload.get('fps', DEFAULT_CAMERA_FPS)) except (TypeError, ValueError): fps = DEFAULT_CAMERA_FPS selected_profile_id = str(payload.get('profile', '')).strip() if selected_profile_id in CAMERA_PROFILE_MAP: profile = dict(CAMERA_PROFILE_MAP[selected_profile_id]) else: profile = None for item in CAMERA_PROFILES: if item['width'] == width and item['height'] == height: profile = dict(item) break if profile is None: profile = default_profile profile['fps'] = fps return profile def write_camera_settings(profile, fps): profile = dict(profile) fps = normalize_camera_fps(fps) payload = { 'profile': profile['id'], 'width': profile['width'], 'height': profile['height'], 'fps': fps, 'updated_at': int(time.time()), } with open(CAMERA_RUNTIME_SETTINGS_FILE, 'w', encoding='utf-8') as handle: json.dump(payload, handle, indent=2, sort_keys=True) handle.write('\n') def read_camera_settings_version(): try: with open(CAMERA_RUNTIME_SETTINGS_FILE, 'r', encoding='utf-8') as handle: payload = json.load(handle) except (FileNotFoundError, json.JSONDecodeError, OSError, ValueError, TypeError): return 0 try: return int(payload.get('updated_at', 0)) except (TypeError, ValueError): return 0 def get_default_imu_heading_calibration(): return { 'offset_deg': 0.0, 'updated_at': None, 'source': 'default', } def read_imu_heading_calibration(): data = get_default_imu_heading_calibration() try: with open(IMU_HEADING_CALIBRATION_FILE, 'r', encoding='utf-8') as handle: payload = json.load(handle) except (FileNotFoundError, json.JSONDecodeError, OSError, ValueError, TypeError): return data try: offset_deg = float(payload.get('offset_deg', 0.0)) except (TypeError, ValueError): return data data['offset_deg'] = offset_deg data['updated_at'] = payload.get('updated_at') data['source'] = 'saved' return data def write_imu_heading_calibration(offset_deg): payload = { 'offset_deg': float(offset_deg), 'updated_at': time.strftime('%Y-%m-%d %H:%M:%S'), } os.makedirs(os.path.dirname(IMU_HEADING_CALIBRATION_FILE), exist_ok=True) with open(IMU_HEADING_CALIBRATION_FILE, 'w', encoding='utf-8') as handle: json.dump(payload, handle, ensure_ascii=False, indent=2) handle.write('\n') payload['source'] = 'saved' return payload def get_corrected_heading_deg(raw_heading_deg, offset_deg): if raw_heading_deg is None: return None return _normalize_heading_deg(float(raw_heading_deg) + float(offset_deg)) def get_camera_config(): profile = read_camera_settings() return { 'status': 'Bereit', 'camera': { 'selected_profile': profile['id'], 'width': profile['width'], 'height': profile['height'], 'fps': profile['fps'], 'fps_options': list(CAMERA_FPS_OPTIONS), 'profiles': [dict(item) for item in CAMERA_PROFILES], } } def get_imu_heading_status(): calibration = read_imu_heading_calibration() with IMU_ROS_LOCK: raw_heading_deg = IMU_ROS_CACHE.get('heading_deg') corrected_heading_deg = get_corrected_heading_deg(raw_heading_deg, calibration['offset_deg']) return { 'offset_deg': calibration['offset_deg'], 'raw_heading_deg': raw_heading_deg, 'corrected_heading_deg': corrected_heading_deg, 'updated_at': calibration['updated_at'], 'source': calibration['source'], } def set_imu_heading_offset(offset_deg): payload = write_imu_heading_calibration(offset_deg) corrected_heading_deg = None with IMU_ROS_LOCK: raw_heading_deg = IMU_ROS_CACHE.get('heading_deg') if raw_heading_deg is not None: corrected_heading_deg = get_corrected_heading_deg(raw_heading_deg, payload['offset_deg']) return { 'offset_deg': payload['offset_deg'], 'raw_heading_deg': raw_heading_deg, 'corrected_heading_deg': corrected_heading_deg, 'updated_at': payload['updated_at'], 'source': payload['source'], } def calibrate_imu_heading_to_north(): with IMU_ROS_LOCK: raw_heading_deg = IMU_ROS_CACHE.get('heading_deg') if raw_heading_deg is None: return 409, {'status': 'Noch keine frischen IMU-Daten für Nord-Kalibrierung'} calibration = set_imu_heading_offset(-float(raw_heading_deg)) return 200, { 'status': 'Nordrichtung gespeichert', 'imu_heading': calibration, } def get_default_imu_tilt_calibration(): return { 'roll_offset_deg': 0.0, 'pitch_offset_deg': 0.0, 'updated_at': None, 'source': 'default', } def read_imu_tilt_calibration(): data = get_default_imu_tilt_calibration() try: with open(IMU_TILT_CALIBRATION_FILE, 'r', encoding='utf-8') as handle: payload = json.load(handle) data['roll_offset_deg'] = float(payload.get('roll_offset_deg', 0.0)) data['pitch_offset_deg'] = float(payload.get('pitch_offset_deg', 0.0)) data['updated_at'] = payload.get('updated_at') data['source'] = payload.get('source', 'file') except (OSError, ValueError, KeyError): pass return data def write_imu_tilt_calibration(roll_offset_deg, pitch_offset_deg): payload = { 'roll_offset_deg': float(roll_offset_deg), 'pitch_offset_deg': float(pitch_offset_deg), 'updated_at': time.strftime('%Y-%m-%dT%H:%M:%S'), 'source': 'admin', } os.makedirs(os.path.dirname(IMU_TILT_CALIBRATION_FILE), exist_ok=True) with open(IMU_TILT_CALIBRATION_FILE, 'w', encoding='utf-8') as handle: json.dump(payload, handle, indent=2) return payload def get_imu_tilt_status(): calibration = read_imu_tilt_calibration() with IMU_ROS_LOCK: raw_roll_deg = IMU_ROS_CACHE.get('roll_deg') raw_pitch_deg = IMU_ROS_CACHE.get('pitch_deg') corrected_roll = None if raw_roll_deg is None else raw_roll_deg + calibration['roll_offset_deg'] corrected_pitch = None if raw_pitch_deg is None else raw_pitch_deg + calibration['pitch_offset_deg'] return { 'roll_offset_deg': calibration['roll_offset_deg'], 'pitch_offset_deg': calibration['pitch_offset_deg'], 'raw_roll_deg': raw_roll_deg, 'raw_pitch_deg': raw_pitch_deg, 'corrected_roll_deg': corrected_roll, 'corrected_pitch_deg': corrected_pitch, 'updated_at': calibration['updated_at'], 'source': calibration['source'], } def zero_imu_tilt(): with IMU_ROS_LOCK: raw_roll_deg = IMU_ROS_CACHE.get('roll_deg') raw_pitch_deg = IMU_ROS_CACHE.get('pitch_deg') if raw_roll_deg is None or raw_pitch_deg is None: return 409, {'status': 'Noch keine frischen IMU-Daten für Neigungskalibrierung'} payload = write_imu_tilt_calibration(-float(raw_roll_deg), -float(raw_pitch_deg)) return 200, { 'status': 'Neigung genullt', 'imu_tilt': { 'roll_offset_deg': payload['roll_offset_deg'], 'pitch_offset_deg': payload['pitch_offset_deg'], 'raw_roll_deg': raw_roll_deg, 'raw_pitch_deg': raw_pitch_deg, 'corrected_roll_deg': 0.0, 'corrected_pitch_deg': 0.0, 'updated_at': payload['updated_at'], 'source': payload['source'], }, } def set_camera_config(profile_id=None, fps=None): current = read_camera_settings() if profile_id is None: profile = normalize_camera_profile(current['id']) else: profile = normalize_camera_profile(profile_id) if fps is None: fps = current.get('fps', DEFAULT_CAMERA_FPS) fps = normalize_camera_fps(fps) profile['fps'] = fps write_camera_settings(profile, fps) restart_camera_service() return profile def rumble_controller(duration_ms=400): # Linux FF constants EV_FF = 0x15 FF_RUMBLE = 0x50 # EVIOCSFF = _IOW('E', 0x80, struct ff_effect) — ff_effect is 48 bytes on ARM64 EVIOCSFF = (0x40000000 | (48 << 16) | (ord('E') << 8) | 0x80) device_path = None for path in sorted(glob.glob('/dev/input/event*')): try: sys_name = f'/sys/class/input/{os.path.basename(path)}/device/name' with open(sys_name, encoding='utf-8') as f: name = f.read().strip() if any(k in name for k in ('Gamepad', 'F710', 'GAMEPAD')): device_path = path break except OSError: pass if not device_path: raise RuntimeError('Kein Gamepad gefunden') fd = os.open(device_path, os.O_RDWR) try: # struct ff_effect layout on ARM64 (48 bytes): # offset 0: type (u16) # offset 2: id (s16) — set to -1, kernel assigns # offset 4: direction (u16) # offset 6: trigger.button (u16) # offset 8: trigger.interval (u16) # offset 10: replay.length (u16, ms) # offset 12: replay.delay (u16) # offset 14: padding (2 bytes) # offset 16: union.rumble.strong_magnitude (u16) # offset 18: union.rumble.weak_magnitude (u16) # offset 20–47: zeros buf = bytearray(48) struct.pack_into(' /tmp/gps_node.log 2>&1 &" ]) def resolve_gps_device(): candidates = [] seen = set() for pattern in GPS_PORT_PATTERNS: for path in sorted(glob.glob(pattern)): if path in seen: continue seen.add(path) candidates.append(path) return candidates[0] if candidates else None def configure_gps_serial(fd, baud_rate): baud_map = { 4800: termios.B4800, 9600: termios.B9600, 19200: termios.B19200, 38400: termios.B38400, 57600: termios.B57600, 115200: termios.B115200, } baud = baud_map.get(int(baud_rate), termios.B4800) attrs = termios.tcgetattr(fd) attrs[0] = 0 attrs[1] = 0 attrs[2] = termios.CS8 | termios.CREAD | termios.CLOCAL attrs[3] = 0 attrs[4] = baud attrs[5] = baud attrs[6][termios.VMIN] = 0 attrs[6][termios.VTIME] = 0 termios.tcsetattr(fd, termios.TCSANOW, attrs) termios.tcflush(fd, termios.TCIOFLUSH) def send_gps_serial_command(command_text, baud_rate=GPS_BAUD_RATE): device_path = resolve_gps_device() if not device_path: raise RuntimeError('Kein GPS-Empfänger gefunden') payload = command_text.encode('ascii') fd = None try: fd = os.open(device_path, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK) configure_gps_serial(fd, baud_rate) os.write(fd, payload) termios.tcdrain(fd) except OSError as exc: if exc.errno == errno.ENOENT: raise RuntimeError('GPS-Gerät nicht erreichbar') raise RuntimeError(f'GPS-Befehl fehlgeschlagen: {exc}') except termios.error as exc: raise RuntimeError(f'GPS-Port konnte nicht konfiguriert werden: {exc}') finally: if fd is not None: try: os.close(fd) except OSError: pass return device_path def cold_start_gps_receiver(): device_path = send_gps_serial_command(GPS_COLD_START_COMMAND) time.sleep(1.5) restart_gps_node() return device_path def read_gps_diagnostics_from_ros(): command = [ 'docker', 'exec', EXOMY_CONTAINER, 'bash', '-lc', "source /opt/ros/melodic/setup.bash && " "source /root/exomy_ws/devel/setup.bash && " "timeout 8s rostopic echo -n 1 /gps/diagnostics_json" ] stdout, stderr, returncode = run_command(command) if returncode != 0 or not stdout: raise RuntimeError('Keine GPS-Diagnosedaten verfügbar') raw_text = stdout.strip() lines = [line.strip() for line in raw_text.splitlines() if line.strip()] if not lines: raise RuntimeError('GPS-Diagnosedaten sind leer') if lines[0].startswith('data:'): payload_text = lines[0][5:].strip() if payload_text.startswith("'") and payload_text.endswith("'"): payload_text = payload_text[1:-1] payload_text = payload_text.encode('utf-8').decode('unicode_escape') else: payload_text = lines[0] try: return json.loads(payload_text) except json.JSONDecodeError as exc: raise RuntimeError(f'GPS-Diagnosedaten konnten nicht gelesen werden: {exc}') def parse_gps_datetime(): diagnostics = read_gps_diagnostics_from_ros() utc_date = str(diagnostics.get('utc_date') or '').strip() utc_time = str(diagnostics.get('utc_time') or '').strip() if not utc_date or not utc_time: raise RuntimeError('GPS liefert aktuell keine UTC-Zeit') combined = f'{utc_date} {utc_time}' for pattern in ('%Y-%m-%d %H:%M:%S.%f', '%Y-%m-%d %H:%M:%S'): try: return datetime.datetime.strptime(combined, pattern) except ValueError: continue raise RuntimeError(f'GPS-Zeitformat unbekannt: {combined}') def get_local_timezone(): try: with open('/etc/timezone', 'r', encoding='utf-8') as handle: timezone_name = handle.read().strip() if timezone_name: return ZoneInfo(timezone_name) except (FileNotFoundError, OSError, ValueError): pass return datetime.datetime.now().astimezone().tzinfo or datetime.timezone.utc def set_system_time_from_gps(): gps_dt = parse_gps_datetime() gps_utc = gps_dt.replace(tzinfo=datetime.timezone.utc) local_time = gps_utc.astimezone(get_local_timezone()) time_text = local_time.strftime('%Y-%m-%d %H:%M:%S') stdout, stderr, returncode = run_command(['timedatectl', 'set-time', time_text]) if returncode != 0: raise RuntimeError(stderr or stdout or 'Systemzeit konnte nicht gesetzt werden') return time_text def stop_exomy_container(): stdout, _, returncode = run_command(['docker', 'stop', '-t', '2', EXOMY_CONTAINER]) return returncode == 0, stdout def start_exomy_container(): stdout, _, returncode = run_command(['docker', 'start', EXOMY_CONTAINER]) return returncode == 0, stdout def reboot_raspberry_pi(): subprocess.Popen(['systemctl', 'reboot']) def shutdown_raspberry_pi(): subprocess.Popen(['systemctl', 'poweroff']) def run_motor_test_action(wheel, action): from admin_motor_test import run_motor_test acquired = MOTOR_TEST_LOCK.acquire(blocking=False) if not acquired: return 409, {'status': 'Motortest läuft bereits'} try: container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } result = run_motor_test(wheel, action) return 200, { 'status': f"{result['wheel_label']}: {result['action_label']}", 'result': result, 'container_status': get_motor_test_container_status(), } except FileNotFoundError: return 500, {'status': 'Motor-Konfiguration fehlt'} except RuntimeError as exc: return 500, {'status': str(exc)} except ValueError as exc: return 400, {'status': str(exc)} except Exception: return 500, {'status': 'Motortest fehlgeschlagen'} finally: MOTOR_TEST_LOCK.release() def get_steering_neutral_status(): from admin_motor_test import get_steering_neutral_values container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } try: result = get_steering_neutral_values() return 200, { 'status': 'Servo-Mitten geladen', 'neutral_values': result, 'container_status': container_status, } except FileNotFoundError: return 500, {'status': 'Motor-Konfiguration fehlt'} except Exception: return 500, {'status': 'Servo-Mitten konnten nicht geladen werden'} def preview_steering_neutral_action(wheel, value): from admin_motor_test import preview_steering_neutral_value container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } try: result = preview_steering_neutral_value(str(wheel).lower(), int(value)) return 200, { 'status': f"{result['wheel_label']}: PWM {result['value']}", 'preview': result, 'container_status': container_status, } except ValueError as exc: return 400, {'status': str(exc)} except RuntimeError as exc: return 500, {'status': str(exc)} except Exception: return 500, {'status': 'Servo-Vorschau fehlgeschlagen'} def save_steering_neutral_action(values): from admin_motor_test import save_steering_neutral_values container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } try: result = save_steering_neutral_values(values) return 200, { 'status': 'Servo-Mitten gespeichert', 'neutral_values': result, 'container_status': container_status, } except ValueError as exc: return 400, {'status': str(exc)} except RuntimeError as exc: return 500, {'status': str(exc)} except FileNotFoundError: return 500, {'status': 'Motor-Konfiguration fehlt'} except Exception: return 500, {'status': 'Servo-Mitten konnten nicht gespeichert werden'} def get_drive_neutral_status(): from admin_motor_test import get_drive_neutral_value container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } try: result = get_drive_neutral_value() return 200, { 'status': 'Fahr-Neutralwerte geladen', 'drive_neutral': result, 'container_status': container_status, } except FileNotFoundError: return 500, {'status': 'Motor-Konfiguration fehlt'} except Exception: return 500, {'status': 'Fahr-Neutralwerte konnten nicht geladen werden'} def get_drive_estimator_config(): from admin_motor_test import WHEELS, _load_config try: config_dir = os.path.join(os.path.dirname(__file__), '..', 'config') config_candidates = [ os.path.join(config_dir, 'exomy.yaml'), os.path.join(config_dir, 'exomy.remote.yaml'), os.path.join(config_dir, 'exomy.yaml.template'), ] config_path = next((path for path in config_candidates if os.path.exists(path)), None) if config_path is None: raise FileNotFoundError('Konfigurationsdatei nicht gefunden') config = _load_config(config_path) neutral_values = [ int(config[f'drive_pwm_neutral_{wheel}']) for wheel in WHEELS ] average_neutral = sum(neutral_values) / len(neutral_values) return 200, { 'status': 'Bereit', 'estimator': { 'config_path': os.path.abspath(config_path), 'drive_pwm_neutral_average': average_neutral, 'drive_pwm_neutral_values': neutral_values, 'drive_pwm_range': int(config['drive_pwm_range']), 'pwm_frequency_hz': 50, 'servo_max_rpm': 50.0, 'servo_full_scale_pulse_delta_ms': 0.2, 'wheel_diameter_m_default': 0.1, }, } except FileNotFoundError: return 500, {'status': 'Motor-Konfiguration fehlt'} except Exception: return 500, {'status': 'Estimator-Konfiguration konnte nicht geladen werden'} def get_drive_estimator_calibration(): default_points = [0, 20, 30, 40, 50, 60, 80, 100] data = { 'points': [ {'percent': point, 'speed_mps': 0.0} for point in default_points ], 'updated_at': None, 'source': 'default', } try: with open(DRIVE_ESTIMATOR_CALIBRATION_FILE, 'r', encoding='utf-8') as handle: payload = json.load(handle) points = payload.get('points') if isinstance(points, list): cleaned_points = [] for item in points: if not isinstance(item, dict): continue percent = int(item.get('percent', 0)) speed_mps = float(item.get('speed_mps', 0.0)) cleaned_points.append({ 'percent': max(0, min(100, percent)), 'speed_mps': max(0.0, speed_mps), }) if cleaned_points: cleaned_points.sort(key=lambda item: item['percent']) data = { 'points': cleaned_points, 'updated_at': payload.get('updated_at'), 'source': 'saved', } except FileNotFoundError: pass except (OSError, ValueError, TypeError, json.JSONDecodeError): return 500, {'status': 'Kalibrierung konnte nicht geladen werden'} return 200, { 'status': 'Bereit', 'calibration': data, } def save_drive_estimator_calibration(points): if not isinstance(points, list) or not points: return 400, {'status': 'Kalibrierpunkte fehlen'} cleaned_points = [] seen_percents = set() for item in points: if not isinstance(item, dict): return 400, {'status': 'Kalibrierpunkte sind ungültig'} try: percent = int(item.get('percent', 0)) speed_mps = float(item.get('speed_mps', 0.0)) except (TypeError, ValueError): return 400, {'status': 'Kalibrierwerte sind ungültig'} percent = max(0, min(100, percent)) speed_mps = max(0.0, speed_mps) if percent in seen_percents: return 400, {'status': 'Jede Stufe darf nur einmal vorkommen'} seen_percents.add(percent) cleaned_points.append({ 'percent': percent, 'speed_mps': speed_mps, }) cleaned_points.sort(key=lambda item: item['percent']) os.makedirs(os.path.dirname(DRIVE_ESTIMATOR_CALIBRATION_FILE), exist_ok=True) payload = { 'updated_at': time.strftime('%Y-%m-%d %H:%M:%S'), 'points': cleaned_points, } try: with open(DRIVE_ESTIMATOR_CALIBRATION_FILE, 'w', encoding='utf-8') as handle: json.dump(payload, handle, ensure_ascii=False, indent=2) handle.write('\n') except OSError: return 500, {'status': 'Kalibrierung konnte nicht gespeichert werden'} return 200, { 'status': 'Kalibrierung gespeichert', 'calibration': { 'points': cleaned_points, 'updated_at': payload['updated_at'], 'source': 'saved', }, } def get_route_library(): data = { 'routes': [], 'updated_at': None, } try: with open(ROUTE_LIBRARY_FILE, 'r', encoding='utf-8') as handle: payload = json.load(handle) routes = payload.get('routes') if isinstance(routes, list): cleaned_routes = [] for route in routes: if not isinstance(route, dict): continue name = str(route.get('name', '')).strip() waypoints = route.get('waypoints', []) if not name or not isinstance(waypoints, list): continue cleaned_waypoints = [] for point in waypoints: if not isinstance(point, dict): continue try: lat = float(point.get('lat')) lon = float(point.get('lon')) except (TypeError, ValueError): continue cleaned_waypoints.append({ 'lat': lat, 'lon': lon, }) cleaned_routes.append({ 'name': name, 'waypoints': cleaned_waypoints, 'updated_at': route.get('updated_at'), }) data = { 'routes': cleaned_routes, 'updated_at': payload.get('updated_at'), } except FileNotFoundError: pass except (OSError, ValueError, TypeError, json.JSONDecodeError): return 500, {'status': 'Routen konnten nicht geladen werden'} return 200, { 'status': 'Bereit', 'library': data, } def save_named_route(name, waypoints): route_name = str(name or '').strip() if not route_name: return 400, {'status': 'Routenname fehlt'} if not isinstance(waypoints, list) or not waypoints: return 400, {'status': 'Route hat keine Wegpunkte'} cleaned_waypoints = [] for point in waypoints: if not isinstance(point, dict): return 400, {'status': 'Wegpunkte sind ungültig'} try: lat = float(point.get('lat')) lon = float(point.get('lon')) except (TypeError, ValueError): return 400, {'status': 'Wegpunkte sind ungültig'} cleaned_waypoints.append({ 'lat': lat, 'lon': lon, }) existing_status, existing_response = get_route_library() if existing_status != 200: return existing_status, existing_response routes = existing_response['library']['routes'] now_text = time.strftime('%Y-%m-%d %H:%M:%S') replaced = False for route in routes: if route['name'].lower() == route_name.lower(): route['name'] = route_name route['waypoints'] = cleaned_waypoints route['updated_at'] = now_text replaced = True break if not replaced: routes.append({ 'name': route_name, 'waypoints': cleaned_waypoints, 'updated_at': now_text, }) routes.sort(key=lambda item: item['name'].lower()) os.makedirs(os.path.dirname(ROUTE_LIBRARY_FILE), exist_ok=True) payload = { 'updated_at': now_text, 'routes': routes, } try: with open(ROUTE_LIBRARY_FILE, 'w', encoding='utf-8') as handle: json.dump(payload, handle, ensure_ascii=False, indent=2) handle.write('\n') except OSError: return 500, {'status': 'Route konnte nicht gespeichert werden'} return 200, { 'status': 'Route gespeichert', 'route': { 'name': route_name, 'waypoints': cleaned_waypoints, 'updated_at': now_text, }, 'library': payload, } def delete_named_route(name): route_name = str(name or '').strip() if not route_name: return 400, {'status': 'Routenname fehlt'} existing_status, existing_response = get_route_library() if existing_status != 200: return existing_status, existing_response routes = existing_response['library']['routes'] filtered_routes = [route for route in routes if route['name'].lower() != route_name.lower()] if len(filtered_routes) == len(routes): return 404, {'status': 'Route nicht gefunden'} now_text = time.strftime('%Y-%m-%d %H:%M:%S') payload = { 'updated_at': now_text, 'routes': filtered_routes, } try: with open(ROUTE_LIBRARY_FILE, 'w', encoding='utf-8') as handle: json.dump(payload, handle, ensure_ascii=False, indent=2) handle.write('\n') except OSError: return 500, {'status': 'Route konnte nicht gelöscht werden'} return 200, { 'status': 'Route gelöscht', 'library': payload, } def preview_drive_neutral_action(values): from admin_motor_test import preview_drive_neutral_value container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } try: result = preview_drive_neutral_value(values) return 200, { 'status': 'Fahr-Neutralwerte angefahren', 'drive_neutral': result, 'container_status': container_status, } except ValueError as exc: return 400, {'status': str(exc)} except RuntimeError as exc: return 500, {'status': str(exc)} except Exception: return 500, {'status': 'Fahr-Vorschau fehlgeschlagen'} def save_drive_neutral_action(values): from admin_motor_test import save_drive_neutral_value container_status = get_motor_test_container_status() if not container_status['is_safe_for_motor_test']: return 409, { 'status': 'ExoMy-Container läuft noch. Bitte zuerst auf der Motortest-Seite stoppen.', 'container_status': container_status, } try: result = save_drive_neutral_value(values) return 200, { 'status': 'Fahr-Neutralwerte gespeichert', 'drive_neutral': result, 'container_status': container_status, } except ValueError as exc: return 400, {'status': str(exc)} except RuntimeError as exc: return 500, {'status': str(exc)} except FileNotFoundError: return 500, {'status': 'Motor-Konfiguration fehlt'} except Exception: return 500, {'status': 'Fahr-Neutralwerte konnten nicht gespeichert werden'} class Handler(http.server.BaseHTTPRequestHandler): def end_headers(self): self.send_header('Access-Control-Allow-Origin', '*') self.send_header('Access-Control-Allow-Methods', 'GET, POST, OPTIONS') self.send_header('Access-Control-Allow-Headers', 'Content-Type') super().end_headers() def write_json(self, status_code, payload): body = json.dumps(payload).encode('utf-8') self.send_response(status_code) self.send_header('Content-Type', 'application/json; charset=utf-8') self.send_header('Content-Length', str(len(body))) self.end_headers() self.wfile.write(body) def do_OPTIONS(self): self.send_response(204) self.end_headers() def do_GET(self): if self.path == '/api/imu/rotate-status': import imu_rotate 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()}) return if self.path == '/api/speed-limit': self.write_json(200, {'speed_limit_percent': get_speed_limit()}) return if self.path == '/api/camera-config': self.write_json(200, get_camera_config()) return if self.path == '/api/status': self.write_json(200, collect_status()) return if self.path == '/api/motor-test/status': self.write_json(200, { 'status': 'Bereit', 'container_status': get_motor_test_container_status(), }) return if self.path == '/api/motor-test/steering-neutral': status_code, response = get_steering_neutral_status() self.write_json(status_code, response) return if self.path == '/api/motor-test/drive-neutral': status_code, response = get_drive_neutral_status() self.write_json(status_code, response) return if self.path == '/api/drive-estimator/config': status_code, response = get_drive_estimator_config() self.write_json(status_code, response) return if self.path == '/api/drive-estimator/calibration': status_code, response = get_drive_estimator_calibration() self.write_json(status_code, response) return if self.path == '/api/routes': status_code, response = get_route_library() self.write_json(status_code, response) return self.write_json(404, {'status': 'Unbekannt'}) def do_POST(self): raw_body = b'{}' 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: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' if self.path == '/api/delay': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return try: seconds = set_delay(payload.get('delay_seconds', 0)) self.write_json(200, {'status': 'Delay gesetzt', 'delay_seconds': seconds}) except Exception as exc: self.write_json(500, {'status': str(exc)}) return if self.path == '/api/speed-limit': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return try: percent = set_speed_limit(payload.get('speed_limit_percent', 100)) self.write_json(200, {'status': 'Geschwindigkeitslimit gesetzt', 'speed_limit_percent': percent}) except Exception as exc: self.write_json(500, {'status': str(exc)}) return if self.path == '/api/camera-config': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return try: profile = set_camera_config(payload.get('profile'), payload.get('fps')) self.write_json(200, { 'status': 'Kameraprofil gespeichert, Kameradienst startet neu', 'camera': { 'selected_profile': profile['id'], 'width': profile['width'], 'height': profile['height'], 'fps': profile['fps'], 'fps_options': list(CAMERA_FPS_OPTIONS), 'profiles': [dict(item) for item in CAMERA_PROFILES], } }) except ValueError as exc: self.write_json(400, {'status': str(exc)}) except Exception as exc: self.write_json(500, {'status': str(exc)}) return if self.path == '/api/drive-estimator/calibration': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return status_code, response = save_drive_estimator_calibration(payload.get('points')) self.write_json(status_code, response) return if self.path == '/api/routes/save': try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return status_code, response = save_named_route( payload.get('name'), payload.get('waypoints'), ) self.write_json(status_code, response) return if self.path == '/api/routes/delete': try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return status_code, response = delete_named_route(payload.get('name')) self.write_json(status_code, response) return if self.path == '/api/imu/heading-offset': try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return try: offset_deg = float(payload.get('offset_deg', 0.0)) except (TypeError, ValueError): self.write_json(400, {'status': 'Ungültiger Heading-Offset'}) return self.write_json(200, { 'status': 'Heading-Offset gespeichert', 'imu_heading': set_imu_heading_offset(offset_deg), }) return if self.path == '/api/motor-test/drive-neutral/preview': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return values = payload.get('values') if not isinstance(values, dict): self.write_json(400, {'status': 'Fahr-Neutralwerte fehlen'}) return status_code, response = preview_drive_neutral_action(values) self.write_json(status_code, response) return if self.path == '/api/motor-test/drive-neutral/save': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return values = payload.get('values') if not isinstance(values, dict): self.write_json(400, {'status': 'Fahr-Neutralwerte fehlen'}) return status_code, response = save_drive_neutral_action(values) self.write_json(status_code, response) return if self.path == '/api/motor-test/steering-neutral/preview': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return status_code, response = preview_steering_neutral_action( payload.get('wheel', ''), payload.get('value', 0) ) self.write_json(status_code, response) return if self.path == '/api/motor-test/steering-neutral/save': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return values = payload.get('values') if not isinstance(values, dict): self.write_json(400, {'status': 'Neutralwerte fehlen'}) return status_code, response = save_steering_neutral_action(values) self.write_json(status_code, response) return if self.path == '/api/motor-test/stop-container': stopped, _ = stop_exomy_container() container_status = get_motor_test_container_status() if not stopped and not container_status['is_safe_for_motor_test']: self.write_json(500, { 'status': 'ExoMy-Container konnte nicht gestoppt werden', 'container_status': container_status, }) return self.write_json(200, { 'status': 'ExoMy-Container ist gestoppt', 'container_status': container_status, }) return if self.path == '/api/motor-test/start-container': started, _ = start_exomy_container() container_status = get_motor_test_container_status() if not started and container_status['state'] != 'running': self.write_json(500, { 'status': 'ExoMy-Container konnte nicht gestartet werden', 'container_status': container_status, }) return self.write_json(200, { 'status': 'ExoMy-Container läuft wieder', 'container_status': container_status, }) return if self.path == '/api/motor-test': try: content_length = int(self.headers.get('Content-Length', '0')) except ValueError: content_length = 0 raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}' try: payload = json.loads(raw_body.decode('utf-8')) except (UnicodeDecodeError, json.JSONDecodeError): self.write_json(400, {'status': 'Ungültige Anfrage'}) return status_code, response = run_motor_test_action( str(payload.get('wheel', '')).lower(), str(payload.get('action', '')) ) self.write_json(status_code, response) return if self.path == '/api/rumble': try: rumble_controller(400) self.write_json(200, {'status': 'Vibration ausgelöst'}) except RuntimeError as exc: self.write_json(500, {'status': str(exc)}) return if self.path == '/api/imu/set-north': status_code, response = calibrate_imu_heading_to_north() self.write_json(status_code, response) return if self.path == '/api/imu/set-level': status_code, response = zero_imu_tilt() 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)) speed_pct = max(20, min(50, int(payload.get('speed_pct', 20)))) 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'] vel = max(1, int(100.0 * (speed_pct / 100.0) ** 2.0)) status = imu_rotate.start_rotate( target_deg=target_deg, heading_offset=heading_offset, restore_mode=LAST_LOCOMOTION_MODE, vel=vel, ) 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, 'status': 'GPS-Kaltstart wird ausgelöst' }, '/api/restart-gps': { 'handler': restart_gps_node, 'status': 'GPS-Node wird neu gestartet' }, '/api/set-time-from-gps': { 'handler': set_system_time_from_gps, 'status': 'Pi-Zeit wird aus GPS gesetzt' }, '/api/restart-camera': { 'handler': restart_camera_service, 'status': 'Kameradienst wird neu gestartet' }, '/api/restart-container': { 'handler': restart_exomy_container, 'status': 'ExoMy-Container wird neu gestartet' }, '/api/reboot': { 'handler': reboot_raspberry_pi, 'status': 'Raspberry Pi wird neu gestartet' }, '/api/shutdown': { 'handler': shutdown_raspberry_pi, 'status': 'Raspberry Pi wird heruntergefahren' } } action = actions.get(self.path) if action is None: self.write_json(404, {'status': 'Unbekannt'}) return self.write_json(202, {'status': action['status']}) self.wfile.flush() time.sleep(0.1) action['handler']() def log_message(self, fmt, *args): return if __name__ == '__main__': with ReusableThreadingTCPServer(('0.0.0.0', PORT), Handler) as server: server.daemon_threads = True server.serve_forever()