Make an ROS 2 topic for BMI088 IMU data (gyro and accelerometer) from Crazyflie 2.1

[YHuniv]

Hi,

I am working with the Crazyflie 2.1 and trying to extract IMU data (gyroscope and accelerometer) under ROS2.
I noticed that variables like gyro.x, gyro.y, gyro.z, and possibly acc.x, acc.y, acc.z are available via LogConfig.
However, I couldn’t find a default ROS2 topic like /imu for these values.

Is there an existing configuration or script in this repository to publish IMU data to ROS2 topics?

Thank you for your assistance!

[knmcguire]

Hi! We don't have a default topic for IMU values yet for Crazyswarm2 (see the overview here)

So the only way now is to make a custom log in crazyflies.yaml: https://imrclab.github.io/crazyswarm2/usage.html#crazyflies-yaml

But if you'd like to add this default topic to Crazyswarm2 it would be a nice addition as a pull request. Let us know if you need any assistance with that.

[YHuniv]

Hi,
Thank for your answer.

1. I tried get data from custum_topics :

firmware_logging:
      enabled: true
      custom_topics:
        imu/data: 
          frequency: 10
          vars: ["acc.x", "acc.y"]

I have to error :

ros2 topic echo /cf231/imu/data
The message type 'crazyflie_interfaces/msg/LogDataGeneric' is invalid

2. I tried to add a default_topics for the IMU by changinf the .yaml and crazyflie_server.py :

firmware_logging:
    enabled: true
    default_topics: 
     
      pose:
        frequency: 10 # Hz
      status:
        frequency: 1 # Hz
      imu_data:
        frequency: 10 # Hz

For the crazyflie_server.py i add this part in the script :

from sensor_msgs.msg import Imu

and :

       self.default_log_type = {"pose": PoseStamped,
                                 "scan": LaserScan,
                                 "odom": Odometry,
                                 "imu_data": Imu, # Ajout du type pour le nouveau topic
                                 "status": Status}

       self.default_log_vars = {"pose": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z',
                                          'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'],
                                 "scan": ['range.front', 'range.left', 'range.back', 'range.right'],
                                 "odom": ['stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z',
                                          'stabilizer.yaw', 'stabilizer.roll', 'stabilizer.pitch',
                                          'kalman.statePX', 'kalman.statePY', 'kalman.statePZ',
                                          'gyro.z', 'gyro.x', 'gyro.y'],
                                 "imu_data": ['acc.x', 'acc.y', 'acc.z', 'gyro.x', 'gyro.y', 'gyro.z'
                                              'stabilizer.roll', 'stabilizer.pitch', 'stabilizer.yaw'],
                                 "status": ['supervisor.info', 'pm.vbatMV', 'pm.state',
                                          'radio.rssi']}

        self.default_log_fnc = {"pose": self._log_pose_data_callback,
                                "scan": self._log_scan_data_callback,
                                "odom": self._log_odom_data_callback,
                                "imu_data": self._log_imu_data_callback,
                                "status": self._log_status_data_callback}

and added a IMU calback :

def _log_imu_data_callback(self, timestamp, data, logconf, uri):
        """
        Once IMU data is retrieved from the Crazyflie,
            send out the ROS 2 topic for IMU data.
        """
        cf_name = self.cf_dict[uri]`

        # Récupération des données d'accélération et du gyroscope
        accel_x = data.get('acc.x')
        accel_y = data.get('acc.y')
        accel_z = data.get('acc.z')
        gyro_x = data.get('gyro.x')
        gyro_y = data.get('gyro.y')
        gyro_z = data.get('gyro.z')

        # Orientation : Réutilisation des quaternions si nécessaire
        roll = radians(data.get('stabilizer.roll', 0.0))
        pitch = radians(-1.0 * data.get('stabilizer.pitch', 0.0))
        yaw = radians(data.get('stabilizer.yaw', 0.0))
        q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)

        # Création du message Imu
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = self.world_tf_name

        # Orientation : Inclut les quaternions
        msg.orientation.x = q[0]
        msg.orientation.y = q[1]
        msg.orientation.z = q[2]
        msg.orientation.w = q[3]
        msg.orientation_covariance = [-1] * 9  # Pas de covariance fournie

        # Données du gyroscope
        msg.angular_velocity.x = gyro_x
        msg.angular_velocity.y = gyro_y
        msg.angular_velocity.z = gyro_z
        msg.angular_velocity_covariance = [-1] * 9  # Pas de covariance fournie

        # Données de l'accélération linéaire
        msg.linear_acceleration.x = accel_x
        msg.linear_acceleration.y = accel_y
        msg.linear_acceleration.z = accel_z
        msg.linear_acceleration_covariance = [-1] * 9  # Pas de covariance fournie

        # Publication des données sur le topic
        try:
            self.swarm._cfs[uri].logging["imu_data_publisher"].publish(msg)
        except:
            self.get_logger().info("Could not publish imu_data message, stopping imu_data log")
            self.swarm._cfs[uri].logging["imu_data_log_config"].stop()

        # Transformation TF (si nécessaire)
        t_imu = TransformStamped()
        t_imu.header.stamp = self.get_clock().now().to_msg()
        t_imu.header.frame_id = self.world_tf_name
        t_imu.child_frame_id = cf_name + "_imu"
        t_imu.transform.translation.x = 0.0  # Pas de position spécifique à l'IMU
        t_imu.transform.translation.y = 0.0
        t_imu.transform.translation.z = 0.0
        t_imu.transform.rotation.x = q[0]
        t_imu.transform.rotation.y = q[1]
        t_imu.transform.rotation.z = q[2]
        t_imu.transform.rotation.w = q[3]

        try:
            self.tfbr.sendTransform(t_imu)
        except:
            self.get_logger().info("Could not publish imu tf")

After that I did :

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

. install/local_setup.bash

ros2 launch crazyflie launch.py

But no topic /imu_data was published !
Did I missed something ?

Thank you by advance for your help

[knmcguire]

Hi! In 1. did you source the terminal where you retrieved the ros2 info with the crazyswarm2 package info?

and in 2. It might be that you are hitting the maximum of the crazyflie logging variables, which is only 26 bytes (see the documentation here). I do wonder why the odometry ever worked then... (update: it's before of the FP16 notion, forgot about that! )

https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/user-guides/python_api/#variables-and-logging

Could you try less variables for imu_data?

[knmcguire]

Also just to add here, at 1. it probably won't work with that slash, so try it with imu_data instead