Use a roscpp Timer with ROS to Publish Data at a Fixed Rate

In this tutorial you’ll learn how to use a ROS timer in Cpp (roscpp Timer) to publish data at a fixed rate, inside a node.

That will be extremely useful for all kinds of sensors you’re using in a robot.

I have also create the same tutorial for Python, check it out here.

So, you want to monitor a sensor inside one of your ROS node. The simplest solution is to implement a ROS Rate to read and publish the data.

But with ROS Timers, you can easily improve your code (ex: oversampling + averaging), and make it more scalable for reading multiple sensors in the same node.

In this tutorial I’ll show you how to use a roscpp Timer to achieve that, step by step.

Let’s get started!

Publishing data from a sensor (no Timer)

#include <ros/ros.h>
#include <std_msgs/Float64.h>

double readTemperatureSensorData()
{
    // Code to read data from your sensor
    // Return measured temperature
    return 30.0;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "your_sensor_node");
    ros::NodeHandle nh;

    // Create a ROS publisher
    ros::Publisher dataPublisher =
        nh.advertise<std_msgs::Float64>("/temperature", 10);

    // Create a rate
    ros::Rate rate(10);

    // Use the rate to publish at a fixed rate
    // inside the main function of the program
    while (ros::ok())
    {
        std_msgs::Float64 msg;
        msg.data = readTemperatureSensorData();
        dataPublisher.publish(msg);
        rate.sleep();
    }
}

This example is one of the first thing you do when learning ROS. You create a Rate. Inside an infinite loop, you read data and you publish this data on a ROS topic. After doing that, you make the thread sleep until the next loop iteration.

On this code above, the readTemperatureSensorData() function is not implemented. Here you’ll put the code which is specific to your sensor – maybe using I2C communication, UART, etc. The rate we use here makes the node publish the data at 10Hz (10 times every second).

Publishing data from a sensor (no Timer – but with OOP)

Let’s rewrite this code example with OOP, which will make it more scalable. This is a preparation before using roscpp Timers in the next part.

#include <ros/ros.h>
#include <std_msgs/Float64.h>

class TemperatureSensor
{
public:
    TemperatureSensor(ros::NodeHandle *nh)
    {
        // Initialize temperature and ROS publisher
        temperature = 0.0;
        temperaturePublisher =
            nh->advertise<std_msgs::Float64>("/temperature", 10);
    }

    double readTemperatureSensorData()
    {
        // Code to read data from your sensor
        // Return measured temperature
        return 30.0;
    }

    void publishTemperature()
    {
        std_msgs::Float64 msg;
        msg.data = readTemperatureSensorData();
        temperaturePublisher.publish(msg);
    }

private:
    double temperature;
    ros::Publisher temperaturePublisher;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "your_sensor_node");
    ros::NodeHandle nh;

    // Create an instance of Temperature sensor
    TemperatureSensor temperatureSensor(&nh);

    // Create a rate
    ros::Rate rate(10);

    // Use the rate to publish at a fixed rate
    // inside the main function of the program
    while (ros::ok())
    {
        temperatureSensor.readTemperatureSensorData();
        temperatureSensor.publishTemperature();
        rate.sleep();
    }
}

The logic hasn’t changed, but as you can see, the temperature data and temperature publishers are now parts of the TemperatureSensor class.

Read and publish data asynchronously with a ROS Timer

One of the main problem of the previous code is that the action to read the data, and the action to publish the data, are synchronous. Let’s now use roscpp Timers to improve that.

Code with roscpp Timer

#include <ros/ros.h>
#include <std_msgs/Float64.h>

class TemperatureSensor
{
public:
    TemperatureSensor(ros::NodeHandle *nh)
    {
        // Initialize temperature and ROS publisher
        temperature = 0.0;
        temperaturePublisher =
            nh->advertise<std_msgs::Float64>("/temperature", 10);
    }

    double readTemperatureSensorData()
    {
        // Code to read data from your sensor
        // Return measured temperature
        return 30.0;
    }

    void publishTemperature()
    {
        std_msgs::Float64 msg;
        msg.data = readTemperatureSensorData();
        temperaturePublisher.publish(msg);
    }

private:
    double temperature;
    ros::Publisher temperaturePublisher;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "your_sensor_node");
    ros::NodeHandle nh;

    // Create an instance of Temperature sensor
    TemperatureSensor temperatureSensor(&nh);

    // Create a ROS timer for reading data
    // the callback function for the Timer must
    // be bound with std::bind or boost::bind
    ros::Timer timerReadTemperature =
        nh.createTimer(ros::Duration(1.0 / 10.0),
                       std::bind(&TemperatureSensor::readTemperatureSensorData, temperatureSensor));

    // Create a ROS timer for publishing temperature
    ros::Timer timerPublishTemperature =
        nh.createTimer(ros::Duration(1.0 / 10.0),
                       std::bind(&TemperatureSensor::publishTemperature, temperatureSensor));

    // We can now use spin, or do whatever 
    // we want in this node
    ros::spin();
}

In the main() we create 2 ROS Timers. One for reading data, and one for publishing the data.

To create a Timer, you need to give 2 arguments: the duration between 2 callback triggers, and of course the callback function. Pay attention to this detail: with a ROS Rate you need to give the frequency as a parameter, but for a ROS Timer you give the period.

Once created, the timer will return so the following of the code will be executed. It will then, in another thread, call the callback every X seconds, where X is the first parameter you gave to the Timer.

Don’t forget to add ros::spin() so the node doesn’t exit, and all the callbacks can be called.

Here you have 2 main improvements in your code:

  • You can read and publish data asynchronously. If you want to read data at 57Hz and publish it at 26Hz, you just need to change the parameters inside the Timers.
  • After you’ve created the Timers you can continue doing some actions in your main().

Callback with std::bind

Note the syntax with std::bind for the callback function. If you gave a simple function inside no class, you could just give the name of the function. Here you need to use std::bind (or boost::bind). The first parameter is the name of the function, the second is the instance to use.

If you decide to create those 2 Timers directly inside the TemperatureSensor class, the syntax will be different. Instead of giving the name of the instance to use, you’ll use “this”.

TemperatureSensor(ros::NodeHandle *nh)
{
    // Initialize temperature and ROS publisher
    temperature = 0.0;
    temperaturePublisher =
        nh->advertise<std_msgs::Float64>("/temperature", 10);

    // If you want to start the ROS timers inside
    // the constructor, use this syntax
    ros::Timer timerReadTemperature =
        nh->createTimer(ros::Duration(1.0 / 10.0),
                        std::bind(&TemperatureSensor::readTemperatureSensorData, this));

    ros::Timer timerPublishTemperature =
        nh->createTimer(ros::Duration(1.0 / 10.0),
                        std::bind(&TemperatureSensor::publishTemperature, this));
}

Averaging and oversampling with ROS Timers

Let’s not stop there and continue to improve this code.

Oversampling means that you read the data faster than you publish it, so you can have a more up-to-date value. Let’s say you want to publish data at 10Hz. If you read the temperature value at 100Hz, the value you publish will be fresher.

When you start to get more values you can also filter the data in order to reduce noise and have a smoother evolution of the values you read. Filters like complementary filter, Kalman filter, etc, are often used when measuring data from sensors. Here we’ll use a basic averaging method – temperature will be updated using the average of the 2 last read values.

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <vector>
#include <numeric>

class TemperatureSensor
{
public:
    TemperatureSensor(ros::NodeHandle *nh)
    {
        // Initialize temperature and ROS publisher
        temperature = 0.0;
        temperaturePublisher =
            nh->advertise<std_msgs::Float64>("/temperature", 10);

        tempIndex = 0;
        tempMaxIndex = 1;
        tempData.clear();
    }

    double readTemperatureSensorData()
    {
        if (tempIndex < tempMaxIndex)
        {
            // Add read temperature to the array
            tempData.push_back(30.0);
            tempIndex++;
        }
        else
        {
            // Compute the mean value of the array
            temperature = std::accumulate(
                              tempData.begin(), tempData.end(), 0) /
                          (double)tempData.size();
            tempIndex = 0;
            tempData.clear();
        }
    }

    void publishTemperature()
    {
        std_msgs::Float64 msg;
        msg.data = readTemperatureSensorData();
        temperaturePublisher.publish(msg);
    }

private:
    double temperature;
    ros::Publisher temperaturePublisher;
    std::vector<double> tempData;
    int tempIndex;
    int tempMaxIndex;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "your_sensor_node");
    ros::NodeHandle nh;

    // Create an instance of Temperature sensor
    TemperatureSensor temperatureSensor(&nh);

    // Create a ROS timer for reading data
    ros::Timer timerReadTemperature =
        nh.createTimer(ros::Duration(1.0 / 100.0),
                       std::bind(&TemperatureSensor::readTemperatureSensorData, temperatureSensor));

    // Create a ROS timer for publishing temperature
    ros::Timer timerPublishTemperature =
        nh.createTimer(ros::Duration(1.0 / 10.0),
                       std::bind(&TemperatureSensor::publishTemperature, temperatureSensor));

    ros::spin();
}

The publishing frequency is still the same (10Hz), however here we have increased the reading frequency to 100Hz.

Inside the reading function, we wait until we have read 2 values. When 2 values are read, we compute the average and update the temperature value. With this code example it would be simple to increase the averaging sample to 3, 4 or more values.

So, the “real” temperature update frequency is now 50Hz.

We have a 5x oversampling factor, with an already filtered value. And all that with just a few lines of code using roscpp Timers.

Conclusion: Use Timers whenever you need to do something with ROS at a fixed rate. This is one of the best way to achieve that while making your code more scalable.

 

Leave a Comment