Additional Resources on Obtaining Robot Orientation (using Imu Sensor)

Here we provide example code on how to obtain absolute orientation data of the robot using an onboard Imu sensor.

Example code

This simple code subscribes to the Imu sensor topic /imu_plugin/out, processes the data from quaternions to euler angles and the prints out the yaw angle (i.e., rotation around Z axis) in degrees.

#!/usr/bin/env python3
"""
Script to test imu sensor for robot orientation
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu

import numpy as np


class ImuTest(Node):
    def __init__(self):
        super().__init__("test_imu")

        # Create a subscriber for the imu sensor
        self.imu_sub = self.create_subscription(
            Imu, 
            "/imu_plugin/out",
            self.imu_callback, 1
        )

    def imu_callback(self, msg):
        """
        Imu callback function
        """
        orientation_list = [msg.orientation.x,
                            msg.orientation.y, 
                            msg.orientation.z,
                            msg.orientation.w]
        (roll, pitch, yaw) = self.euler_from_quaternion(orientation_list)

        self.get_logger().info("[IMU] Yaw angle is [ %.3f ] deg" % (np.degrees(yaw)))



    def euler_from_quaternion(self, quaternion):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quaternion = [x, y, z, w]
        """
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]

        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw



def main(args=None):
    rclpy.init(args=args)

    test_imu = ImuTest()
    rclpy.spin(test_imu)

    rclpy.shutdown()


if __name__ == "__main__":
    main()

Please feel free to use this code in your solution for Task 1 & 2 as needed.