Let's discuss the code from the beginning. The following Arduino headers help us read IMU values using the I2C protocol. The MPU6050_6Axis_MotionApps20.h header has functions to enable DMP and retrieve values from it.

    #include "Wire.h" 
    #include "I2Cdev.h" 
    #include "MPU6050_6Axis_MotionApps20.h" 

The following line of code will create an MPU6050 handle, which can be used for the MPU-9250. We can use this object to initialize and retrieve values from the IMU.

    MPU6050 mpu; 

As you know, we have to include ros.h to access ROS serial client APIs. We are also including Vector3.h, which has the definition of the Vector3 ROS message. This message can carry three values of orientation. The tf/transform_broadcaster.h header has TF broadcaster classes, which basically send transforms of IMU values with respect to a fixed frame:

    #include <ros.h> 
    #include <geometry_msgs/Vector3.h> 
    #include <tf/transform_broadcaster.h> 

After defining headers, we have to define handles of the TF message and broadcaster, as given here:

    geometry_msgs::TransformStamped t; 
    tf::TransformBroadcaster broadcaster; 

In the next line of code, we are creating a NodeHandle, which essentially helps us subscribe to and publish ROS topics like a normal ROS node:

    ros::NodeHandle nh; 

To hold the orientation values, which are yaw, pitch, and roll, we are creating a Vector3 ROS message. This message is published by the Arduino node on a topic named /imu_data.

    geometry_msgs::Vector3 orient; 

The following line of code creates a publisher object for the /imu_data topic. We are publishing the orientation using this object.

    ros::Publisher imu_pub("imu_data", &orient); 

The frameid value is /base_link, which is static, and the child frame is /imu_frame which moves according to the IMU data.

    char frameid[] = "/base_link"; 
    char child[] = "/imu_frame"; 

These are variables to hold orientation values, such as Quaternion, gravity vector, and yaw, pitch, and roll:

    Quaternion q; 
    VectorFloat gravity; 
    float ypr[3]; 

Here is the interrupt-detection routine, for whenever data is ready to be read from the IMU. The routine basically sets the mpuInterrupt variable as true.

    volatile bool mpuInterrupt = false; 
    void dmpDataReady() { 
        mpuInterrupt = true; 
    } 

Next is the setup() function of Arduino, which does several I2C initializations for the Arduino, ROS node handler, TF broadcaster, ROS publisher, MPU object, and DMP inside MPU:

    void setup() { 
        Wire.begin(); 
        nh.initNode(); 
        broadcaster.init(nh); 
        nh.advertise(imu_pub); 
        mpu.initialize(); 
        devStatus = mpu.dmpInitialize(); 

If DMP is initialized, we can enable it and attach an interrupt on the Arduino's second digital pin, which is the first interrupt pin of the Arduino. Whenever data is ready to be read from buffer, the IMU will generate an interrupt. The code also checks the DMP status and sets a variable to check whether DMP is ready or not. This will be useful while executing the loop() function. We are also using a variable called packetSize to store the MPU buffer size.

        if (devStatus == 0) { 
            mpu.setDMPEnabled(true); 
            attachInterrupt(0, dmpDataReady, RISING); 
            mpuIntStatus = mpu.getIntStatus(); 
            dmpReady = true; 
            packetSize = mpu.dmpGetFIFOPacketSize(); 
        } 

Inside the loop() function, the code checks whether dmpReady is true or not. If it is not true, that means DMP is not initialized, so it will not execute any code. If it is ready, it will wait for interrupts from the MPU.

        if (!dmpReady) return; 
          while (!mpuInterrupt && fifoCount < packetSize) { 
          ; 
        } 

If there is an interrupt, it will go to the dmpDataReady() interrupt-detection routine and set the mpuInterrupt flag as true. If it is true, then the previous while loop will exit and start running the following code. We are resetting the mpuInterrupt flag to false, reading the current status of the MPU, and retrieving the first-in first-out (FIFO) count. FIFO is basically a buffer, and the first entry to the buffer will be processed first.

        mpuInterrupt = false; 
        mpuIntStatus = mpu.getIntStatus(); 
        fifoCount = mpu.getFIFOCount(); 

After reading the status and FIFO count, we can reset the FIFO if an overflow is detected. Overflows can happen if your code is too inefficient.

       if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 
            mpu.resetFIFO(); 

If the data is ready, we will again compare the FIFO buffer size and the DMP packet size; if equal, FIFO data will be dumped into the fifoBuffer variable.

    else if (mpuIntStatus & 0x01) { 
            while (fifoCount < packetSize) fifoCount =  
            mpu.getFIFOCount(); 
            mpu.getFIFOBytes(fifoBuffer, packetSize); 
            fifoCount -= packetSize; 

After storing the DMP data in the buffer, we can extract the rotation components, such as quaternion, gravity vector, and Euler angle.

    mpu.dmpGetQuaternion(&q, fifoBuffer); 
    mpu.dmpGetGravity(&gravity, &q); 
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 

We need to get the Euler angle in degrees, and it is going to be published in the /imu_data topic. Here is the code for doing it. The ypr value we're getting from the MPU object will be in radians, which should be converted to degree using the following equations:

    orient.x = ypr[0] * 180/M_PI; 
    orient.y = ypr[1] * 180/M_PI; 
    orient.z = ypr[2] * 180/M_PI; 
    imu_pub.publish(&orient); 

Here is how we'll publish the TF data. We have to insert the frame, quaternion values, and time stamping to the TF message headers. Using the TF broadcaster, we can publish it.

    t.header.frame_id = frameid; 
    t.child_frame_id = child; 
    t.transform.translation.x = 1.0; 
    t.transform.rotation.x = q.x; 
    t.transform.rotation.y = q.y; 
    t.transform.rotation.z = q.z; 
    t.transform.rotation.w = q.w; 
    t.header.stamp = nh.now(); 
    broadcaster.sendTransform(t); 

We have to call nh.spinOnce() to process each operation we have performed using ROS APIs, so the publishing and subscribing operations are performed only while calling the spinOnce() function. We are also blinking the onboard LED to indicate the program activity.

    nh.spinOnce(); 
    delay(200); 
    blinkState = !blinkState; 
    digitalWrite(LED_PIN, blinkState); 
    delay(200); 

That is all about the ROS-Arduino node. Now what you can do is compile and upload this code to Arduino. Make sure that all other settings on the Arduino IDE are correct.

After uploading the code to the Arduino, the remaining work is on the PC side. We have to run the ROS serial server node to obtain the Arduino node topics. The first step to verify the IMU data from Arduino is by visualizing it. We can visualize the IMU data by observing the TF values in Rviz.