How to transform IMU data from sensor frame to baselink frame?

Question

I have two IMU sensors (front and rear) on my robot. I want to transform the linear acceleration data from both sensors to the robot's base_link frame and then calculate the pitch and roll using this data. Then, compare it with a threshold values. I'm new to ROS, and with ChatGPT's help, I wrote the code below for the transformation process. The code receives the inputs and prints the output, but I'm concerned about whether the transformation process is correct or if there are any potential issues in the code.

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>  // Updated header file
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <Eigen/Dense>

class IMUFusionNode : public rclcpp::Node
{
public:
    IMUFusionNode() : Node("imu_fusion_node"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_)
    {
        // Declare and get the pitch and roll threshold parameters
        this->declare_parameter("pitch_threshold", 0.5); // radians
        this->declare_parameter("roll_threshold", 0.5);  // radians
        this->get_parameter("pitch_threshold", pitch_threshold_);
        this->get_parameter("roll_threshold", roll_threshold_);

        // Define a compatible QoS profile (Best-effort, transient-local, etc.)
        auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort();

        // Subscribe to IMU topics
        front_imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/ouster_front/imu", qos_profile,
            std::bind(&IMUFusionNode::frontIMUCallback, this, std::placeholders::_1));

        rear_imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/ouster_rear/imu", qos_profile,
            std::bind(&IMUFusionNode::rearIMUCallback, this, std::placeholders::_1));
    }

private:
    void frontIMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        front_linear_acceleration_ = msg->linear_acceleration; // Store linear acceleration data
        processIMUData(msg); // Process IMU data
    }

    void rearIMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        rear_linear_acceleration_ = msg->linear_acceleration; // Store linear acceleration data
        processIMUData(msg); // Process IMU data
    }

    void processIMUData(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        try
        {
            geometry_msgs::msg::TransformStamped transform_stamped;
            transform_stamped = tf_buffer_.lookupTransform("base_link", msg->header.frame_id, tf2::TimePointZero);

            // Transform the linear acceleration to the base_link frame
            tf2::Vector3 linear_accel;
            tf2::fromMsg(msg->linear_acceleration, linear_accel);
            tf2::Transform tf2_transform;
            tf2::fromMsg(transform_stamped.transform, tf2_transform);
            linear_accel = tf2_transform * linear_accel; // Apply the transformation

            // Calculate pitch and roll using linear acceleration
            double roll = atan2(linear_accel.y(), linear_accel.z());
            double pitch = atan2(-linear_accel.x(), sqrt(linear_accel.y() * linear_accel.y() + linear_accel.z() * linear_accel.z()));

            // Check thresholds
            if (std::abs(pitch) > pitch_threshold_ || std::abs(roll) > roll_threshold_)
            {
                RCLCPP_ERROR(this->get_logger(), "Dangerous state detected! Pitch: %.2f, Roll: %.2f", pitch, roll);
                // Optionally publish an alert message or stop the robot
            }
            else
            {
                RCLCPP_INFO(this->get_logger(), "Safe state. Pitch: %.2f, Roll: %.2f", pitch, roll);
            }
        }
        catch (tf2::TransformException &ex)
        {
            RCLCPP_WARN(this->get_logger(), "Could not transform IMU data: %s", ex.what());
        }
    }

    // ROS 2 subscription handles
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr front_imu_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr rear_imu_sub_;

    // TF2 related objects
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    // Linear acceleration storage
    geometry_msgs::msg::Vector3 front_linear_acceleration_;
    geometry_msgs::msg::Vector3 rear_linear_acceleration_;

    // Thresholds
    double pitch_threshold_, roll_threshold_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<IMUFusionNode>());
    rclcpp::shutdown();
    return 0;
}

Answer 1

Yes, that implementation does look correct. For context and to add some sources, "the tf::Transform class overloads the * operator to combine transforms". Source