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