diff --git a/README.md b/README.md index d99ab37..dbd3e4c 100644 --- a/README.md +++ b/README.md @@ -45,6 +45,8 @@ f710_joy_node ────────────────────── /rover_command ──► robot_node ──► /motor_commands ──► motor_node ──► PCA9685 PWM ──► Motoren ``` +Zusätzlich ist ein BNO085-IMU-Sensor auf I2C-Bus `1` mit Adresse `0x4A` angebunden. Er läuft als eigener Python-3-Service auf dem Pi (`exomy-imu-ros.service`), publiziert über `rosbridge` nach ROS und wird von der Admin-API wieder aus ROS gelesen. + **Software auf dem Pi:** `/home/pi/ExoMy_Software/` **ROS-Workspace im Container:** `/root/exomy_ws/src/exomy/` @@ -56,6 +58,7 @@ f710_joy_node ────────────────────── |---|---| | `exomy-admin-api.service` | Admin-API (`exomy_admin_api.py`) | | `exomy-camera-stream.service` | MJPEG Kamera-Stream | +| `exomy-imu-ros.service` | Liest den BNO085 per Python 3 auf dem Pi und publiziert IMU-Daten nach ROS | | `exomy-wifi-bootstrap.service` | Wählt beim Start `4pi` oder `eskimue.de`, sonst eigener Access Point | ### WLAN-Startlogik @@ -102,6 +105,24 @@ docker exec -it exomy_autostart bash | `rosbridge_websocket` | WebSocket-Bridge für die Web-GUI (Port 9090) | | `rosapi_node` | ROS-API für die Web-GUI | +### IMU: BNO085 + +- Sensor: Adafruit BNO085 Breakout +- Bus: I2C `1` +- Adresse: `0x4A` +- Standardrate: `15 Hz` +- Der IMU-Sensor läuft nicht im ROS-Melodic-Container, sondern als eigener Python-3-Systemd-Service auf dem Pi. +- Grund: Der restliche ROS-Stack im Container nutzt noch überwiegend Python 2, der BNO085-Treiber benötigt aber Python 3. +- Der Service `exomy-imu-ros.service` liest den Sensor und publiziert über `rosbridge` nach ROS. + +### IMU-Topics + +| Topic | Typ | Inhalt | +|---|---|---| +| `/imu/data` | `sensor_msgs/Imu` | Quaternion, Gyro und lineare Beschleunigung | +| `/imu/mag` | `sensor_msgs/MagneticField` | Magnetfeld | +| `/imu/status` | `std_msgs/String` | Statusmeldung des IMU-Dienstes | + ### Admin-Funktionen - Admin-Seite: `http://:8000/admin.html` @@ -114,6 +135,16 @@ docker exec -it exomy_autostart bash - `GPS neu starten` - `GPS-Kaltstart` für den GlobalSat `BU-353N5` +IMU in der Admin-Seite: + +- Status +- Adresse +- Beschleunigung +- Gyro +- Magnetfeld +- Quaternion +- `Roll / Pitch / Yaw` + ### Ports | Port | Verwendung | @@ -149,6 +180,34 @@ ssh pi@192.168.1.9 "docker cp /home/pi/ExoMy_Software/gui/index.html exomy_autos # Kein Neustart nötig — Browser-Reload genügt ``` +IMU-/Admin-API-Änderungen: + +```bash +# Python-Datei auf den Pi kopieren +scp src/imu_node.py pi@192.168.1.9:/home/pi/ExoMy_Software/src/ +scp scripts/exomy_admin_api.py pi@192.168.1.9:/home/pi/ExoMy_Software/scripts/ +scp scripts/exomy-imu-ros.service pi@192.168.1.9:/home/pi/ExoMy_Software/scripts/ + +# Admin-API neu starten +ssh pi@192.168.1.9 "sudo systemctl restart exomy-admin-api.service" + +# IMU-ROS-Service aktualisieren/neu starten +ssh pi@192.168.1.9 "sudo cp /home/pi/ExoMy_Software/scripts/exomy-imu-ros.service /etc/systemd/system/exomy-imu-ros.service && sudo systemctl daemon-reload && sudo systemctl restart exomy-imu-ros.service" +``` + +Wichtige Checks: + +```bash +# IMU-Service auf dem Pi +systemctl status exomy-imu-ros.service + +# IMU-Topics im Container +docker exec exomy_autostart bash -lc "source /opt/ros/melodic/setup.bash && rostopic list | grep ^/imu" + +# Beispielstatus +docker exec exomy_autostart bash -lc "source /opt/ros/melodic/setup.bash && rostopic echo -n 1 /imu/status" +``` + --- ## Bekannte Probleme & Fixes diff --git a/gui/admin.html b/gui/admin.html index 966e7e8..af66f9a 100644 --- a/gui/admin.html +++ b/gui/admin.html @@ -133,6 +133,11 @@ Quaternion - +
+ + RPY + - +
Hinweis @@ -983,6 +988,7 @@ setText('imu_gyro', '--'); setText('imu_magnetic', '--'); setText('imu_quaternion', '--'); + setText('imu_roll_pitch_yaw', '--'); setText('imu_error', '--'); var bar = document.getElementById('bar-temp-adm'); if (bar) bar.style.width = '0%'; @@ -2530,6 +2536,7 @@ setText('imu_gyro', imu.gyro); setText('imu_magnetic', imu.magnetic); setText('imu_quaternion', imu.quaternion); + setText('imu_roll_pitch_yaw', imu.roll_pitch_yaw); setText('imu_error', imu.error || '-'); } diff --git a/scripts/exomy-imu-ros.service b/scripts/exomy-imu-ros.service new file mode 100644 index 0000000..f9f80eb --- /dev/null +++ b/scripts/exomy-imu-ros.service @@ -0,0 +1,18 @@ +[Unit] +Description=ExoMy IMU ROS Bridge +After=network-online.target docker.service +Wants=network-online.target + +[Service] +Type=simple +ExecStart=/usr/bin/python3 /home/pi/ExoMy_Software/src/imu_node.py +Environment=IMU_RATE_HZ=15 +Environment=IMU_FRAME_ID=imu_link +Environment=IMU_I2C_ADDRESS=0x4A +Environment=IMU_ROSBRIDGE_HOST=127.0.0.1 +Environment=IMU_ROSBRIDGE_PORT=9090 +Restart=always +RestartSec=2 + +[Install] +WantedBy=multi-user.target diff --git a/scripts/exomy_admin_api.py b/scripts/exomy_admin_api.py index 29899cb..bccea69 100644 --- a/scripts/exomy_admin_api.py +++ b/scripts/exomy_admin_api.py @@ -5,6 +5,7 @@ import fcntl import glob import http.server import json +import math import os import shutil import socket @@ -72,6 +73,24 @@ 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 = [] +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', + 'error': 'Noch keine IMU-Daten aus ROS', + 'updated_at': 0.0, +} class ReusableThreadingTCPServer(socketserver.ThreadingTCPServer): @@ -99,6 +118,171 @@ def _format_quaternion(values): ) +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 _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)), + ) + _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(_quaternion_to_euler_deg(quaternion)), + 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 + + 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') + imu_topic.subscribe(_on_imu_data_message) + mag_topic.subscribe(_on_imu_mag_message) + status_topic.subscribe(_on_imu_status_message) + IMU_ROS_CLIENT = client + IMU_ROS_TOPICS = [imu_topic, mag_topic, status_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 @@ -178,10 +362,12 @@ def read_imu_status(): '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', @@ -189,7 +375,8 @@ def read_imu_status(): '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(IMU_SENSOR.quaternion), + 'quaternion': _format_quaternion(quaternion), + 'roll_pitch_yaw': _format_euler_deg(_quaternion_to_euler_deg(quaternion)), 'error': '', } except Exception as exc: @@ -203,6 +390,7 @@ def read_imu_status(): 'gyro': 'unbekannt', 'magnetic': 'unbekannt', 'quaternion': 'unbekannt', + 'roll_pitch_yaw': 'unbekannt', 'error': IMU_LAST_ERROR, } @@ -653,7 +841,7 @@ def collect_status(): wifi_details = get_wifi_details() time_details = get_system_time_details() container_details = get_container_details(EXOMY_CONTAINER) - imu_details = read_imu_status() + imu_details = read_imu_ros_status() return { 'status': 'Bereit', 'system': { diff --git a/src/imu_node.py b/src/imu_node.py new file mode 100644 index 0000000..65e48ad --- /dev/null +++ b/src/imu_node.py @@ -0,0 +1,209 @@ +#!/usr/bin/env python3 +import os +import signal +import sys +import time +import warnings + +import roslibpy +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 + + +RATE_HZ = float(os.environ.get('IMU_RATE_HZ', '15')) +FRAME_ID = os.environ.get('IMU_FRAME_ID', 'imu_link') +ADDRESS = os.environ.get('IMU_I2C_ADDRESS', '0x4A') +ROSBRIDGE_HOST = os.environ.get('IMU_ROSBRIDGE_HOST', '127.0.0.1') +ROSBRIDGE_PORT = int(os.environ.get('IMU_ROSBRIDGE_PORT', '9090')) + +RUNNING = True +WORKAROUND_APPLIED = False + + +def handle_shutdown(signum, frame): + del signum, frame + global RUNNING + RUNNING = False + + +def apply_bno08x_workaround(): + global WORKAROUND_APPLIED + + if WORKAROUND_APPLIED: + return + + original_handle_packet = adafruit_bno08x.BNO08X._handle_packet + + def safe_handle_packet(self, packet): + try: + return original_handle_packet(self, packet) + except KeyError as exc: + if len(exc.args) == 1 and isinstance(exc.args[0], int): + return + raise + + adafruit_bno08x.BNO08X._handle_packet = safe_handle_packet + WORKAROUND_APPLIED = True + + +def connect_ros(): + client = roslibpy.Ros(host=ROSBRIDGE_HOST, port=ROSBRIDGE_PORT) + client.run() + start = time.time() + while RUNNING and not client.is_connected: + if time.time() - start > 10.0: + raise RuntimeError('rosbridge connection timeout') + time.sleep(0.1) + return client + + +def create_topics(client): + 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') + imu_topic.advertise() + mag_topic.advertise() + status_topic.advertise() + return imu_topic, mag_topic, status_topic + + +def create_sensor(): + with warnings.catch_warnings(): + warnings.filterwarnings( + 'ignore', + message='I2C frequency is not settable in python, ignoring!', + category=RuntimeWarning, + ) + apply_bno08x_workaround() + 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) + return sensor + + +def ros_time_now(): + current = time.time() + secs = int(current) + nsecs = int((current - secs) * 1000000000) + return {'secs': secs, 'nsecs': nsecs} + + +def publish_status(topic, text): + topic.publish(roslibpy.Message({'data': text})) + + +def publish_measurements(imu_topic, mag_topic, status_topic, sensor): + stamp = ros_time_now() + acceleration = sensor.acceleration + gyro = sensor.gyro + magnetic = sensor.magnetic + quaternion = sensor.quaternion + + imu_topic.publish(roslibpy.Message({ + 'header': {'stamp': stamp, 'frame_id': FRAME_ID}, + 'orientation': { + 'x': quaternion[0], + 'y': quaternion[1], + 'z': quaternion[2], + 'w': quaternion[3], + }, + 'orientation_covariance': [0.0] * 9, + 'angular_velocity': { + 'x': gyro[0], + 'y': gyro[1], + 'z': gyro[2], + }, + 'angular_velocity_covariance': [0.0] * 9, + 'linear_acceleration': { + 'x': acceleration[0], + 'y': acceleration[1], + 'z': acceleration[2], + }, + 'linear_acceleration_covariance': [0.0] * 9, + })) + + mag_topic.publish(roslibpy.Message({ + 'header': {'stamp': stamp, 'frame_id': FRAME_ID}, + 'magnetic_field': { + 'x': magnetic[0] * 1e-6, + 'y': magnetic[1] * 1e-6, + 'z': magnetic[2] * 1e-6, + }, + 'magnetic_field_covariance': [0.0] * 9, + })) + + publish_status(status_topic, 'BNO085 verbunden auf {0} mit {1:.0f} Hz'.format(ADDRESS, RATE_HZ)) + + +def main(): + signal.signal(signal.SIGINT, handle_shutdown) + signal.signal(signal.SIGTERM, handle_shutdown) + + print('imu_node.py startet mit {0:.0f} Hz'.format(RATE_HZ)) + sys.stdout.flush() + + ros_client = None + imu_topic = None + mag_topic = None + status_topic = None + sensor = None + sleep_seconds = 1.0 / max(1.0, RATE_HZ) + + while RUNNING: + try: + if ros_client is None or not ros_client.is_connected: + ros_client = connect_ros() + imu_topic, mag_topic, status_topic = create_topics(ros_client) + print('rosbridge verbunden') + sys.stdout.flush() + + if sensor is None: + sensor = create_sensor() + publish_status(status_topic, 'BNO085 initialisiert auf {0}'.format(ADDRESS)) + print('BNO085 initialisiert') + sys.stdout.flush() + + publish_measurements(imu_topic, mag_topic, status_topic, sensor) + time.sleep(sleep_seconds) + except Exception as exc: + message = 'IMU Fehler: {0}'.format(exc) + print(message) + sys.stdout.flush() + if status_topic is not None and ros_client is not None and ros_client.is_connected: + try: + publish_status(status_topic, message) + except Exception: + pass + sensor = None + time.sleep(1.0) + + if ros_client is not None: + try: + if imu_topic is not None: + imu_topic.unadvertise() + if mag_topic is not None: + mag_topic.unadvertise() + if status_topic is not None: + status_topic.unadvertise() + ros_client.terminate() + except Exception: + pass + + +if __name__ == '__main__': + main()