OOP with ROS in Cpp

In a previous tutorial I’ve shown you how to write OOP code with ROS in Python. Now, time to use OOP with ROS in Cpp!

I’ll use here the same application to write an example, so you can easily compare all 4 codes: Python/Cpp with/without OOP.

If you haven’t read the Python tutorial: the application is a simple number counter, including a ROS publisher, a ROS subscriber, and a ROS service. The subscriber will receive a number and add it to a global counter. As soon as the counter is updated, the publisher will publish the new value of the counter. And the service will be used to reset that counter to zero.

The Cpp ROS program without OOP

Complete Cpp code

#include <ros/ros.h>
#include <std_msgs/Int64.h>
#include <std_srvs/SetBool.h>

int counter = 0;
ros::Publisher pub;

void callback_number(const std_msgs::Int64& msg)
{
    counter += msg.data;
    std_msgs::Int64 new_msg;
    new_msg.data = counter;
    pub.publish(new_msg);
}

bool callback_reset_counter(std_srvs::SetBool::Request &req, 
                            std_srvs::SetBool::Response &res)
{
    if (req.data) {
        counter = 0;
        res.success = true;
        res.message = "Counter has been successfully reset";
    }
    else {
        res.success = false;
        res.message = "Counter has not been reset";
    }

    return true;
}

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

    ros::Subscriber sub = nh.subscribe("/number", 1000, callback_number);
    pub = nh.advertise<std_msgs::Int64>("/number_count", 10);
    ros::ServiceServer reset_service = nh.advertiseService("/reset_counter", 
        callback_reset_counter);

    ros::spin();
}

Breaking down the code line by line

#include <ros/ros.h>
#include <std_msgs/Int64.h>
#include <std_srvs/SetBool.h>

Here we import the roscpp libary, as well as the Int64 message, and SetBool service that we’ll use in this program.

int counter = 0;
ros::Publisher pub;

If we want to be able to use the counter in every function, we have to either add it as a parameter in every function, or declare it in the global scope of the program. So we have a “counter” variable initialized to zero in the global scope. Also note that we have a ROS publisher in the global scope, as we want to use it in a ROS callback function.

void callback_number(const std_msgs::Int64& msg)
{
    counter += msg.data;
    std_msgs::Int64 new_msg;
    new_msg.data = counter;
    pub.publish(new_msg);
}

This is the callback for the subscriber. When the node receives a number, it will add it to the global counter, and publish the new value of the counter. Hence the decision to make those 2 variables global.

bool callback_reset_counter(std_srvs::SetBool::Request &req, 
                            std_srvs::SetBool::Response &res)
{
    if (req.data) {
        counter = 0;
        res.success = true;
        res.message = "Counter has been successfully reset";
    }
    else {
        res.success = false;
        res.message = "Counter has not been reset";
    }

    return true;
}

The callback for the ROS service. Depending on the boolean value inside the service request, we reset (or not) the counter, and return an appropriate boolean (success flag) and message.

Note that for service callbacks in Cpp, the return value is true if the service has correctly finished the execution. Contrary to Python, this is not the value to return to the service client. This value should be written inside the “res” variable that we got from the parameters of the callback function.

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

    ros::Subscriber sub = nh.subscribe("/number", 1000, callback_number);
    pub = nh.advertise<std_msgs::Int64>("/number_count", 10);
    ros::ServiceServer reset_service = nh.advertiseService("/reset_counter", 
        callback_reset_counter);

    ros::spin();
}

And finally the main function of the program. After initializing the node and creating a NodeHandle (you have to create one or else your node won’t work), we can declare and initialize all 3 publisher, subscriber, and service server.

Finally, don’t forget the ros::spin(), so your program is being kept alive with all the callbacks.

What to think about this code

If you’ve read the Python tutorial, well that’s the same problem here. As you scale your program it will become increasingly harder to pass your variables from one place to another, unless you have a very clear organization. Your code might start to look like a “spaghetti code”, where everything is linked to everything and you can’t move further.

The Cpp ROS program with OOP

Complete Cpp code

#include <ros/ros.h>
#include <std_msgs/Int64.h>
#include <std_srvs/SetBool.h>

class NumberCounter {

    private:
    int counter;
    ros::Publisher pub;
    ros::Subscriber number_subscriber;
    ros::ServiceServer reset_service;

    public:
    NumberCounter(ros::NodeHandle *nh) {
        counter = 0;

        pub = nh->advertise<std_msgs::Int64>("/number_count", 10);	
        number_subscriber = nh->subscribe("/number", 1000, 
            &NumberCounter::callback_number, this);
        reset_service = nh->advertiseService("/reset_counter", 
            &NumberCounter::callback_reset_counter, this);
    }

    void callback_number(const std_msgs::Int64& msg) {
        counter += msg.data;
        std_msgs::Int64 new_msg;
        new_msg.data = counter;
        pub.publish(new_msg);
    }

    bool callback_reset_counter(std_srvs::SetBool::Request &req, 
                                std_srvs::SetBool::Response &res)
    {
        if (req.data) {
            counter = 0;
            res.success = true;
            res.message = "Counter has been successfully reset";
        }
        else {
            res.success = false;
            res.message = "Counter has not been reset";
        }

        return true;
    }
};

int main (int argc, char **argv)
{
    ros::init(argc, argv, "number_counter");
    ros::NodeHandle nh;
    NumberCounter nc = NumberCounter(&nh);
    ros::spin();
}

Breaking down the code line by line

#include <ros/ros.h>
#include <std_msgs/Int64.h>
#include <std_srvs/SetBool.h>

No new stuff: we still need the same dependencies here.

class NumberCounter {

    private:
    int counter;
    ros::Publisher pub;
    ros::Subscriber number_subscriber;
    ros::ServiceServer reset_service;

We create the NumberCounter Cpp class. Inside this class we can declare private attributes. So, we declare here the counter, which will be accessible by all methods in this class. And we also manage the publisher, subscriber, and service servers directly in the class.

By making the attributes “private”, we make sure that nothing outside the class can directly access them.

    public:
    NumberCounter(ros::NodeHandle *nh) {
        counter = 0;

        pub = nh->advertise<std_msgs::Int64>("/number_count", 10);	
        number_subscriber = nh->subscribe("/number", 1000, 
            &NumberCounter::callback_number, this);
        reset_service = nh->advertiseService("/reset_counter", 
            &NumberCounter::callback_reset_counter, this);
    }

We can now write the interface of the class: the methods that will be available from the outside.

The first method is the constructor. Here, I chose to get a reference to the NodeHandle. You could also declare a NodeHandle directly in your private attributes and use it the same way. Then, we initialize all the attributes that we’ve just created before.

Important note: you might have noticed that the prototypes for ROS functionalities with callbacks are different when you use them in a class.

Let’s talk about the subscriber. Here, instead of having 2 parameters (name and callback function), we have 3: the name, the callback function, AND a reference to the class, with the keyword “this”. It’s important that you respect this syntax when using OOP with ROS in Cpp.

    void callback_number(const std_msgs::Int64& msg) {
        counter += msg.data;
        std_msgs::Int64 new_msg;
        new_msg.data = counter;
        pub.publish(new_msg);
    }

This subscriber callback is exactly the same as with the non-OOP code. But this time, the counter and publishers are private attributes of the class, not global variables.

    bool callback_reset_counter(std_srvs::SetBool::Request &req, 
                                std_srvs::SetBool::Response &res)
    {
        if (req.data) {
            counter = 0;
            res.success = true;
            res.message = "Counter has been successfully reset";
        }
        else {
            res.success = false;
            res.message = "Counter has not been reset";
        }

        return true;
    }
};

Same for this service callback: nothing really changes.

int main (int argc, char **argv)
{
    ros::init(argc, argv, "number_counter");
    ros::NodeHandle nh;
    NumberCounter nc = NumberCounter(&nh);
    ros::spin();
}

The main function of the program has finally become smaller. It’s nice when your main() can be that small.

Here, after initializing the node, we create a NumberCounter object, and pass it the reference of the NodeHandle we’ve just created. And of course, don’t forget the ros::spin().

The advantages of using OOP with ROS in Cpp

The scope problem is solved. The “counter” variable is now inside a class, and its scope is the entire class, but only this class. From there, we could easily create multiple NumberCounter instances without mixing all the counters.

We could also create other different classes that interact with this NumberCounter. As the counter is private, nothing can happen to it from the outside. So, unless you mess things up directly in your class, you can consider it more “safe”.

Using OOP with ROS in Cpp will bring more modularity to your programs. You’ll be able to create small chunks of code, which is quite useful in robotics. One class for one sensor, another class for an actuator, a class for a motor driver, a class for a machine learning algorithm, etc.

Writing monolith codes is pretty common in robotics, but don’t fall in this trap. Aim for modularity, or else you’ll be stuck quite fast as you (try to) scale your application.

Leave a Comment