Ressources supplémentaires sur l’obtention de l’orientation du robot (à l’aide du capteur Imu)

Nous fournissons ici un exemple de code sur la façon d’obtenir des données d’orientation absolues du robot à l’aide d’un capteur Imu intégré.

Exemple de code

Ce code simple s’abonne au sujet du capteur Imu /imu_plugin/out, traite les données des quaternions aux angles d’Euler et imprime l’angle de lacet (c’est-à-dire la rotation autour de l’axe Z) en degrés.

#!/usr/bin/env python3
"""
Script pour tester le capteur imu pour l'orientation du robot
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu

import numpy as np


classe ImuTest (node) :
    def __init__(soi) :
        super().__init__("test_imu")

        # Créer un abonné pour le capteur imu
        self.imu_sub = self.create_subscription(
            Imu, 
            "/imu_plugin/out",
            self.imu_callback, 1
        )

    def imu_callback(self, msg):
        """
        Fonction de rappel Imu
        """
        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] L'angle de lacet est de [ %.3f ] deg" % (np.degrees(yaw)))



    def euler_from_quaternion(self, quaternion) :
        """
        Convertit le quaternion (w à la dernière place) en roulis, tangage et lacet d'Euler
        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()

N’hésitez pas à utiliser ce code dans votre solution pour les tâches 1 et 2 si nécessaire.