Introduction

This section documents the software implementation of the Orientation package. The package is responsible for capturing data from the BNO08X orientation sensor and converting it into standard ROS 2 IMU messages for integration with other ROS 2 components.

---

Overview of the Package

The Orientation package consists of two main components:

1. Orientation Publisher:
   A ROS 2 node that captures data from the BNO08X orientation sensor and publishes it to the orientation_topic.
2. Orientation to IMU Converter:
   A ROS 2 node that subscribes to the orientation_topic, processes the data, and publishes it to the /imu topic in the standard sensor_msgs/Imu format.

Key functionalities include:

• Data Capture: Collecting acceleration, angular velocity, and orientation data from the BNO08X sensor.
• Custom Message Publication: Publishing orientation data in a custom ROS 2 message format (orientation_msg/Orientation).
• Data Conversion: Converting custom orientation data to the standardized sensor_msgs/Imu message format for use in navigation and localization.

---

Code Walkthrough

1. Orientation Publisher

The OrientationPublisher node initializes the BNO08X orientation sensor and publishes raw sensor data to the orientation_topic. The node:

• Uses the I2C protocol to communicate with the sensor.
• Enables multiple sensor features, including accelerometer, gyroscope, magnetometer, linear acceleration, and rotation vector.
• Publishes data at a fixed interval using a timer.
• Reads in data at 10hz to stabilize imu readings and reduce computational load on the Raspberry Pi

Key Features:

• Error Handling: Includes runtime error handling to manage sensor issues gracefully.
• ROS 2 Publisher: Uses the custom orientation_msg/Orientation message type to transmit data.

---

2. Orientation to IMU Converter

The OrientationToImu node subscribes to the orientation_topic and converts the data into a standard ROS 2 Imu message. It:

• Maps orientation, angular velocity, and linear acceleration data to the appropriate fields in the sensor_msgs/Imu message format.
• Publishes the converted message to the /imu topic for use in localization and navigation.

---

Challenges

1. Data Synchronization:
   Ensuring that orientation updates are synchronized with other sensor data to maintain an accurate representation of the robot’s state.

2. Error Management:
   Handling exceptions during sensor data retrieval to prevent interruptions in data flow.

3. Standardization:
   Mapping custom orientation data to the standardized Imu format required careful alignment of data fields.

---

Code Listings

Orientation Publisher

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from orientation_msg.msg import Orientation
import board
import busio
from adafruit_bno08x.i2c import BNO08X_I2C
import adafruit_bno08x
import time

class OrientationPublisher(Node):
    def __init__(self):
        super().__init__('orientation_publisher')

        # Initialize I2C bus and BNO08x sensor
        self.i2c = busio.I2C(board.SCL, board.SDA)
        self.bno = BNO08X_I2C(self.i2c)

        # Enable features
        self.bno.enable_feature(adafruit_bno08x.BNO_REPORT_ACCELEROMETER)
        self.bno.enable_feature(adafruit_bno08x.BNO_REPORT_GYROSCOPE)
        self.bno.enable_feature(adafruit_bno08x.BNO_REPORT_MAGNETOMETER)
        self.bno.enable_feature(adafruit_bno08x.BNO_REPORT_LINEAR_ACCELERATION)
        self.bno.enable_feature(adafruit_bno08x.BNO_REPORT_ROTATION_VECTOR)
        
        self.publisher_ = self.create_publisher(Orientation, 'orientation_topic', 1)
        self.timer = self.create_timer(.1, self.timer_callback)

    def timer_callback(self):
        try:
            # Read sensor data
            accel_x, accel_y, accel_z = self.bno.acceleration
            gyro_x, gyro_y, gyro_z = self.bno.gyro
            mag_x, mag_y, mag_z = self.bno.magnetic
            linear_accel_x, linear_accel_y, linear_accel_z = self.bno.linear_acceleration
            quat_x, quat_y, quat_z, quat_w = self.bno.quaternion
            
            # Create and populate the ROS 2 message
            msg = Orientation()
            msg.acc_x = accel_x
            msg.acc_y = accel_y
            msg.acc_z = accel_z
            msg.gyro_x = gyro_x
            msg.gyro_y = gyro_y
            msg.gyro_z = gyro_z
            msg.mag_x = mag_x
            msg.mag_y = mag_y
            msg.mag_z = mag_z
            msg.lin_acc_x = linear_accel_x
            msg.lin_acc_y = linear_accel_y
            msg.lin_acc_z = linear_accel_z
            msg.quat_x = quat_x
            msg.quat_y = quat_y
            msg.quat_z = quat_z
            msg.quat_w = quat_w
            
            # Publish the message
            self.publisher_.publish(msg)
            #self.get_logger().info(f'Publishing: {msg}')

        except RuntimeError as e:
            pass
            self.get_logger().error(f"Runtime error: {e}")
        except Exception as e:
            pass
            self.get_logger().error(f"Error: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = OrientationPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

Orientation to IMU Converter

import rclpy
from rclpy.node import Node
from orientation_msg.msg import Orientation
from sensor_msgs.msg import Imu
import numpy as np

class OrientationToImu(Node):
    def __init__(self):
        super().__init__('orientation_to_imu')
        self.subscription = self.create_subscription(
            Orientation,
            'orientation_topic',
            self.listener_callback,
            10)
        self.publisher_ = self.create_publisher(Imu, '/imu', 10)
        self.last_time = self.get_clock().now()

    def listener_callback(self, msg):
        current_time = self.get_clock().now()
        dt = (current_time - self.last_time).nanoseconds / 1e9
        self.last_time = current_time

        imu_msg = Imu()
        imu_msg.header.stamp = current_time.to_msg()
        imu_msg.header.frame_id = 'imu'

        # Populate IMU message with orientation data
        imu_msg.linear_acceleration.x = msg.lin_acc_x
        imu_msg.linear_acceleration.y = msg.lin_acc_y
        imu_msg.linear_acceleration.z = msg.lin_acc_z
        imu_msg.angular_velocity.x = msg.gyro_x
        imu_msg.angular_velocity.y = msg.gyro_y
        imu_msg.angular_velocity.z = msg.gyro_z
        imu_msg.orientation.x = msg.quat_x
        imu_msg.orientation.y = msg.quat_y
        imu_msg.orientation.z = msg.quat_z
        imu_msg.orientation.w = msg.quat_w

        self.publisher_.publish(imu_msg)

def main(args=None):
    rclpy.init(args=args)
    node = OrientationToImu()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()