IMU jetzt sauber als ROS Node eingebunden
This commit is contained in:
Eskimue
+190
-2
@@ -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': {
|
||||
|
||||
Reference in New Issue
Block a user