Eskimue/ExoMy_Cuno
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IMU jetzt sauber als ROS Node eingebunden

Browse Source
This commit is contained in:
Eskimue
2026-05-28 15:18:11 +02:00
parent 580029730a
commit 2ed835d679
5 changed files with 483 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+59
View File
@@ -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
+7
View File
@@ -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
+18
View File
@@ -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
+190 -2
View File
@@ -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
+209
View File
@@ -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()