IMU jetzt sauber als ROS Node eingebunden

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://<IP>: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

gui/admin.html

@@ -133,6 +133,11 @@
             <span class="sr-key">Quaternion</span>
             <strong id="imu_quaternion" class="sr-val">-</strong>
           </div>
+          <div class="sr">
+            <span class="sr-led led-off"></span>
+            <span class="sr-key">RPY</span>
+            <strong id="imu_roll_pitch_yaw" class="sr-val">-</strong>
+          </div>
           <div class="sr">
             <span class="sr-led led-off"></span>
             <span class="sr-key">Hinweis</span>
@@ -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 || '-');
   }
 

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

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': {

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()