Getting IMU and Sensor Data in ROS

In this tutorial, you will learn how to display ZED cameras sensor data using PlotJuggler and subscribe to the sensors data streams.

Sensor Data visualization with PlotJuggler

In this tutorial, you will learn in detail how to configure the PlotJuggler tool to display data values in multiple dynamic live plots.

The package provides two launch files:

plot_sensors_zedm.launch: launches the ZED node preconfigured for the ZED-M and PlotJuggler preconfigured to show ZED-M sensor data
plot_sensors_zed2.launch: launches the ZED node preconfigured for the ZED2 and PlotJuggler preconfigured to show ZED2 sensor data
plot_sensors_zed2i.launch: launches the ZED node preconfigured for the ZED2i and PlotJuggler preconfigured to show ZED2i sensor data

ZED-M

Start the ZED-M sensor data plotting with the command:

$ roslaunch zed_sensors_sub_tutorial plot_sensors_zedm.launch

PlotJuggler will start asking about “ROS Topic Subscriber”, click OK to start subscribing to the sensor topics.

ROS Topic Subscriber

The next window will show all the available topics. Press OK to choose the pre-selected:

Topic list

Finally PlotJuggler will start showing the sensor data published by the ZED node:

ZED-M Plots

On the left you can find the list of all the available subscribed topics, on the right the live plots:

accelerometer data on the top right
gyroscope data on top left
orientation in quaternion form on bottom left

ZED 2 / ZED 2i

Start the ZED 2 sensor data plotting with the command:

$ roslaunch zed_sensors_sub_tutorial plot_sensors_zed2.launch

Start the ZED 2i sensor data plotting with the command:

$ roslaunch zed_sensors_sub_tutorial plot_sensors_zed2i.launch

PlotJuggler will start asking about “ROS Topic Subscriber”, click OK to start subscribing to the sensor topics.

ROS Topic Subscriber

The next window will show all the available topics. Press OK to choose the pre-selected:

Topic list

Finally PlotJuggler will start showing the sensor data published by the ZED node:

ZED 2 Plots

On the left you can find the list of all the available subscribed topics, on the right the live plots:

accelerometer data on the top right
gyroscope data on top left
orientation in quaternion form on middle left
magnetometer data on middle right
barometer data on bottom left
temperature data on bottom right

Sensor Data subscribing in C++

In this tutorial, you will learn how to write a simple C++ node that subscribes to messages of type sensor_msgs/Imu, sensor_msgs/Temperature, sensor_msgs/MagneticField and sensor_msgs/FluidPressure. This lets you retrieve the data from all the sensors available in the ZED2 camera.

Note: the tutorial is valid also for a ZED-M camera, but only topics of type sensor_msgs/Imu will be available.

Introduction

Use this command to connect the ZED 2 camera to the ROS network:

$ roslaunch zed_wrapper zed2.launch

or this command if you are using a ZED 2i

$ roslaunch zed_wrapper zed2i.launch

The ZED node will start to publish object detection data in the network only if there is another node that subscribes to the relative topic.

Running the tutorial

If you properly followed the ROS Installation Guide, the executable of this tutorial has been compiled and you can run the subscriber node using this command:

$ rosrun zed_sensors_sub_tutorial zed_sensors_sub

If the ZED node is running, and a ZED 2 or a ZED 2i is connected or you have loaded an SVO file, you will receive the following stream of messages confirming that you have correctly subscribed to the ZED image topics (with a ZED Mini you will receive only the IMU data):

[ INFO] [1576844361.799432833]: Accel: 0.517,-0.146,9.749 [m/s^2] - Ang. vel: -0.002,-0.000,-0.001 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.799457532]: Mag. Field: -0.000,0.000,0.000 [uT]
[ INFO] [1576844361.799474450]: IMU temperature: 34.09 [C]
[ INFO] [1576844361.802636040]: Accel: 0.529,-0.161,9.758 [m/s^2] - Ang. vel: 0.000,0.002,-0.001 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.802658603]: IMU temperature: 34.09 [C]
[ INFO] [1576844361.804891549]: Accel: 0.529,-0.161,9.758 [m/s^2] - Ang. vel: 0.000,0.002,-0.001 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.804913269]: IMU temperature: 34.09 [C]
[ INFO] [1576844361.807097286]: Accel: 0.531,-0.163,9.765 [m/s^2] - Ang. vel: 0.003,-0.002,0.003 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.807119117]: IMU temperature: 34.11 [C]
[ INFO] [1576844361.809285372]: Accel: 0.531,-0.168,9.770 [m/s^2] - Ang. vel: 0.000,0.001,-0.000 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.809305127]: IMU temperature: 34.11 [C]
[ INFO] [1576844361.812520685]: Accel: 0.529,-0.168,9.775 [m/s^2] - Ang. vel: 0.001,0.001,-0.001 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.812557808]: IMU temperature: 34.11 [C]
[ INFO] [1576844361.814767190]: Accel: 0.521,-0.165,9.770 [m/s^2] - Ang. vel: 0.001,0.001,-0.000 [deg/sec] - Orient. Quat: -0.008,-0.027,-0.001,1.000
[ INFO] [1576844361.814793950]: IMU temperature: 34.11 [C]
[ INFO] [1576844361.814813679]: Atmospheric Pressure: 1010.30 [hPa]
[ INFO] [1576844361.814827208]: Left CMOS temperature: 33.18 [C]
[ INFO] [1576844361.814842668]: Right CMOS temperature: 33.56 [C]

The code

The source code of the subscriber node zed_obj_det_sub_tutorial.cpp:


#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/FluidPressure.h>
#include <sensor_msgs/Temperature.h>

/**
 * Subscriber callbacks
 */

void imuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
    ROS_INFO( "Accel: %.3f,%.3f,%.3f [m/s^2] - Ang. vel: %.3f,%.3f,%.3f [deg/sec] - Orient. Quat: %.3f,%.3f,%.3f,%.3f",
              msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,
              msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,
              msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
}

void imuTempCallback(const sensor_msgs::Temperature::ConstPtr& msg) {
    ROS_INFO( "IMU temperature: %.2f [C]",
              msg->temperature);
}

void leftTempCallback(const sensor_msgs::Temperature::ConstPtr& msg) {
    ROS_INFO( "Left CMOS temperature: %.2f [C]",
              msg->temperature);
}

void rightTempCallback(const sensor_msgs::Temperature::ConstPtr& msg) {
    ROS_INFO( "Right CMOS temperature: %.2f [C]",
              msg->temperature);
}

void magCallback(const sensor_msgs::MagneticField::ConstPtr& msg) {
    ROS_INFO( "Mag. Field: %.3f,%.3f,%.3f [uT]",
              msg->magnetic_field.x*1e-6, msg->magnetic_field.y*1e-6, msg->magnetic_field.z*1e-6);
}

void pressureCallback(const sensor_msgs::FluidPressure::ConstPtr& msg) {
    ROS_INFO( "Atmospheric Pressure: %.2f [hPa]",
              msg->fluid_pressure*100.f);
}

/**
 * Node main function
 */
int main(int argc, char** argv) {
    ros::init(argc, argv, "zed_sensors_subscriber");
    ros::NodeHandle n;

    ros::Subscriber subImu = n.subscribe("/zed/zed_node/imu/data", 10, imuCallback);
    ros::Subscriber subImuTemp = n.subscribe("/zed/zed_node/imu/temperature", 10, imuTempCallback);
    ros::Subscriber subLeftTemp = n.subscribe("/zed/zed_node/temperature/left", 10, leftTempCallback);
    ros::Subscriber subRightTemp = n.subscribe("/zed/zed_node/temperature/right", 10, rightTempCallback);
    ros::Subscriber subPress = n.subscribe("/zed/zed_node/atm_press", 10, pressureCallback);
    ros::Subscriber subMag = n.subscribe("/zed/zed_node/imu/mag", 10, magCallback);

    ros::spin();

    return 0;
}

The code explained

The following is a brief explanation about the above source code:

void imuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
    ROS_INFO( "Accel: %.3f,%.3f,%.3f [m/s^2] - Ang. vel: %.3f,%.3f,%.3f [deg/sec] - Orient. Quat: %.3f,%.3f,%.3f,%.3f",
              msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,
              msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,
              msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
}

void imuTempCallback(const sensor_msgs::Temperature::ConstPtr& msg) {
    ROS_INFO( "IMU temperature: %.2f [C]",
              msg->temperature);
}

void leftTempCallback(const sensor_msgs::Temperature::ConstPtr& msg) {
    ROS_INFO( "Left CMOS temperature: %.2f [C]",
              msg->temperature);
}

void rightTempCallback(const sensor_msgs::Temperature::ConstPtr& msg) {
    ROS_INFO( "Right CMOS temperature: %.2f [C]",
              msg->temperature);
}

void magCallback(const sensor_msgs::MagneticField::ConstPtr& msg) {
    ROS_INFO( "Mag. Field: %.3f,%.3f,%.3f [uT]",
              msg->magnetic_field.x*1e-6, msg->magnetic_field.y*1e-6, msg->magnetic_field.z*1e-6);
}

void pressureCallback(const sensor_msgs::FluidPressure::ConstPtr& msg) {
    ROS_INFO( "Atmospheric Pressure: %.2f [hPa]",
              msg->fluid_pressure*100.f);
}

This is a list of callbacks functions that handles the receiving of the different types of message topics bringing sensor data information. Each callback has a boost::shared_ptr to the received message as parameter, this means you don’t have to worry about memory management. The code of each callback is very simple and demonstrates how to access the fields of the relative message, printing the sensor data information to screen.

The main function is very standard and is explained in details in the “Talker/Listener” ROS tutorial.

The most important lesson of the above code is how the subscribers are defined:

    // Subscriber
    ros::Subscriber subImu = n.subscribe("/zed/zed_node/imu/data", 10, imuCallback);
    ros::Subscriber subImuTemp = n.subscribe("/zed/zed_node/imu/temperature", 10, imuTempCallback);
    ros::Subscriber subLeftTemp = n.subscribe("/zed/zed_node/temperature/left", 10, leftTempCallback);
    ros::Subscriber subRightTemp = n.subscribe("/zed/zed_node/temperature/right", 10, rightTempCallback);
    ros::Subscriber subPress = n.subscribe("/zed/zed_node/atm_press", 10, pressureCallback);
    ros::Subscriber subMag = n.subscribe("/zed/zed_node/imu/mag", 10, magCallback);

A ros::Subscriber is a ROS object that listens on the network and waits for its own topic message to be available. When a message is received, it executes the callback assigned to it.

We declared a subscriber for each of the callback functions that we defined above, taking care to use the correct topic name, e.g. /zed/zed_node/imu/data for IMU data.

Conclusion

The full source code of this tutorial is available on GitHub in the zed_sensors_sub_tutorial sub-package. Along with the node source code are the package.xml and CMakeLists.txt files that complete the tutorial package.