Now that we have our low-cost IMU working in ROS, we are going to create a new node that subscribes to the imu/data topics from the Razor IMU. For example, we can see in our robot model the pitch, roll, and heading depending on the IMU data. So, let's create a new node based on our c8_odom.cpp. We will name this file c8_odom_with_imu.cpp.

Now, we are going to make some modifications. First, we will include sensor_msgs/Imu to be able to subscribe to IMU topics:

#include <sensor_msgs/Imu.h&gt; 

Also, we need a global variable to store IMU data:

sensor_msgs::Imu imu; 

Then, we are going to create a callback function to obtain IMU data:

void imuCallback(const sensor_msgs::Imu &imu_msg) 
{ 
  imu = imu_msg; 
} 

In the main function, we need to declare a subscriber to /imu_data topic:

ros::Subscriber imu_sub = n.subscribe("imu_data", 10, 
 imuCallback);

And we have to assign each odom transform to the right IMU orientation data:

odom_trans.transform.rotation.x = imu.orientation.x; 
odom_trans.transform.rotation.y = imu.orientation.y; 
odom_trans.transform.rotation.z = imu.orientation.z; 
odom_trans.transform.rotation.w = imu.orientation.w; 

Also, we have to assign the pose of the robot to the orientation obtained by the IMU:

odom.pose.pose.orientation.x = imu.orientation.x; 
odom.pose.pose.orientation.y = imu.orientation.y; 
odom.pose.pose.orientation.z = imu.orientation.z; 
odom.pose.pose.orientation.w = imu.orientation.w; 

Once we have created this file and compile it, we are able to run this example:

    $ roslaunch razor_imu_9dof razor-pub-and-display.launch
    $ roslaunch chapter8_tutorials chapter8_robot_imu.launch
  

You will see the Razor IMU display and the robot model in the RViz visualizer.
The robot model will have the pose and orientation of the Razor IMU.