Write a ROS Wrapper (C++)

You can find the original tutorial on The Robotics Back-End.

Please read What is a ROS Wrapper? first before you starts continuing this tutorial.

You have a Cpp driver for a hardware module, and you want to add a ROS interface to control it directly from your ROS application? Well, in this case, what you need to do is to write a Cpp ROS wrapper.

In this tutorial I’ll show you how to write this Cpp wrapper. Starting from a basic and dummy Cpp driver, you’ll see how to wrap all the driver’s functions with ROS core functionalities.

This tutorial applies mostly for wrapping hardware driver, but you can use the knowledge you got here to write a wrapper for virtually any library you want.

The Cpp hardware driver

A quick note on Cpp drivers vs Python drivers: a driver is usually directly talking to the hardware of your robot. This is a quite low-level part of your application, and oftentimes it requires more performance from your code because it needs to run fast (a few hundreds, sometimes thousands Hertz). That’s why you’ll often see Cpp drivers (and thus, Cpp wrappers) instead of Python drivers for robots using ROS.

Though Python can certainly be fine for many cases, if you need high performance, or to respect real-time constraints, you might consider using Cpp from the start.

So, before writing the Cpp ROS wrapper, let’s use a simple driver interface for a speed controlled motor.

#ifndef MOTOR_DRIVER_HPP
#define MOTOR_DRIVER_HPP
#include <map>
#include <string>
class MotorDriver 
{
private:
    int max_speed_;
    int current_speed_;
    int voltage_;
    int temperature_;
public:
    // Init communication, set default settings, ...
    MotorDriver(int max_speed=10);
    // Give a speed that the motor will try to reach
    void setSpeed(int speed);
    // Stop the motor by setting speed to 0
    void stop();
    // Return current speed
    int getSpeed();
    // Return hardware information from the motor
    std::map<std::string, std::string> getStatus();
};
#endif

That’s a pretty basic – and simplified – interface similar to what you can find on a classic hardware driver.

Let’s write some dummy code so we can actually use this driver and focus on the Cpp wrapper, without the need for a real piece of hardware.

#ifndef MOTOR_DRIVER_HPP
#define MOTOR_DRIVER_HPP
#include <map>
#include <string>
class MotorDriver 
{
private:
    int max_speed_;
    int current_speed_;
    int voltage_;
    int temperature_;
public:
    // Init communication, set default settings, ...
    MotorDriver(int max_speed=10)
    {
        max_speed_ = max_speed;
        current_speed_ = 0;
        voltage_ = 12;
        temperature_ = 47;
    }
    // Give a speed that the motor will try to reach
    void setSpeed(int speed)
    {
        if (speed <= max_speed_) {
            current_speed_ = speed;
        }
        else {
            current_speed_ = max_speed_;
        }
    }
    // Stop the motor by setting speed to 0
    void stop()
    {
        current_speed_ = 0;
    }
    // Return current speed
    int getSpeed()
    {
        return current_speed_;
    }
    // Return hardware information from the motor
    std::map<std::string, std::string> getStatus()
    {
        std::map<std::string, std::string> status;
        status["temperature"] = std::to_string(temperature_);
        status["voltage"] = std::to_string(voltage_);
        return status;
    }
};
#endif

Now let’s write a Cpp ROS wrapper around this driver!

Setup Cpp ROS wrapper node

Without OOP

#include <ros/ros.h>
#include "my_robot_driver/motor_driver.hpp"
int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_driver");
    ros::NodeHandle nh;
    MotorDriver motor = MotorDriver(8);
    ROS_INFO("Motor driver is now started");
    
    ros::spin();
    motor.stop();
}

This is the minimal code you need to write for this Cpp ROS wrapper. Although it doesn’t do anything yet, it’s already a good start.

So, how is structured this minimal ROS wrapper?

#include <ros/ros.h>
#include "my_robot_driver/motor_driver.hpp"
int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_driver");
    ros::NodeHandle nh;

First you need to include ROS and the Cpp driver header. Here we have created the driver in the same package as the wrapper.

Then you just create a normal node, and a NodeHandle (specific to roscpp) just after initializing the node.

    MotorDriver motor = MotorDriver(8);
    ROS_INFO("Motor driver is now started");
    
    ros::spin();
    motor.stop();
}

You can now create an instance of your MotorDriver, and initialize it. As a bonus, write a log to tell the user that the driver is ready.

The function ros::spin() will keep the program – and thus the driver – alive, and execute the node callbacks, while the node is still alive.

After the node is killed, we call the stop() method from the driver, to stop the motor. We don’t want the motor to continue running without any program controlling it! Here you can do cleanup tasks, like shutting down communication with your hardware in a clean way.

In Python we have the rospy.on_shutdown() function which gives us a hook before the node shuts down. There is no such thing in Cpp so we have to find a different way.

Using an AsyncSpinner

For your driver and wrapper, if you want to avoid being stuck on callbacks because one ROS callback takes too long, I suggest you use the AsyncSpinner instead of the classic spinner. That’s what we’ll do here.

Here is an AsyncSpinner tutorial if you don’t know what is it and how to use it.

#include <ros/ros.h>
#include "my_robot_driver/motor_driver.hpp"
int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_driver");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(4);
    spinner.start();
    MotorDriver motor = MotorDriver(8);
    ROS_INFO("Motor driver is now started");
    
    ros::waitForShutdown();
    motor.stop();
}

We use an AsyncSpinner with 4 threads. The spinning will be run in the background, and we use ros::waitForShutdown() to block until the node is killed.

Cpp ROS wrapper with OOP

Let’s re-write the previous code with OOP. Check out OOP with ROS in Cpp if you need to refresh your knowledge or are not comfortable with it yet.

#include <ros/ros.h>
#include "my_robot_driver/motor_driver.hpp"
#include <memory>
class MotorDriverROSWrapper 
{
private:
    std::unique_ptr<MotorDriver> motor;
public:
    MotorDriverROSWrapper()
    {
        motor.reset(new MotorDriver(8));
    }
    void stop()
    {
        motor->stop();
    }
};
int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_driver");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(4);
    spinner.start();
    MotorDriverROSWrapper motor_driver_wrapper;
    ROS_INFO("Motor driver is now started");
    
    ros::waitForShutdown();
    motor_driver_wrapper.stop();
}

We’re using a smart pointer to hold the MotorDriver instance, which will be more practical.

From now on we’ll continue with the OOP version of the Cpp ROS wrapper.

Add functionalities to your Cpp ROS wrapper

Get default settings from ROS params

The default max_speed is hard-coded. It would be better to get it – as well as all other default settings – from ROS parameters.

MotorDriverROSWrapper()
{
    int max_speed;
    if (!ros::param::get("~max_speed", max_speed)) {
        max_speed = 8;
    }
    motor.reset(new MotorDriver(max_speed));
}

In the wrapper constructor, we get the max_speed setting from a ROS param, and then we pass this value to the constructor of the driver.

If you’re not so familiar with ROS params and roscpp, check out how to get and set parameters from your code.

Receive speed commands from a ROS topic (Subscriber)

We’ll use a roscpp Subscriber to wrap the setSpeed() method from the driver.

...
#include <std_msgs/Int32.h>
...
private:
    ...
    ros::Subscriber speed_command_subscriber_;
public:
    MotorDriverROSWrapper(ros::NodeHandle *nh)
    {
        ...
        speed_command_subscriber_ = nh->subscribe(
            "speed_command", 10, &MotorDriverROSWrapper::callbackSpeedCommand, this);
    }
    ...
    void callbackSpeedCommand(const std_msgs::Int32 &msg)
    {
        motor_->setSpeed(msg.data);
    }
...
int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_driver");
    ros::NodeHandle nh;
    ...
    MotorDriverROSWrapper motor_driver_wrapper(&nh);
    ...
}

A few modifications required here. To start the Subscriber we need to use a ROS NodeHandle. We could create one inside the class directly, or pass it from the constructor, which is what we did.

When receiving an integer from the “speed_command” topic, the Cpp ROS wrapper will call the hardware driver setSpeed() method.

Stop the motor with a ROS Service

Now, you should be able to stop the motor from one single command. That’s why the stop() command exists in the driver. Let’s wrap it with a roscpp Service server.

...
#include <std_srvs/Trigger.h>
...
    MotorDriverROSWrapper(ros::NodeHandle *nh)
    {
        ...
        stop_motor_server_ = nh->advertiseService(
            "stop_motor", &MotorDriverROSWrapper::callbackStop, this);
    }
    ...
    bool callbackStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
    {
        stop();
        res.success = true;
        res.message = "Successfully stopped motor.";
        return true;
    }
...

The Trigger service contains an empty request, and a success flag + a message as a response, which is exactly what we need here.

Again, we use the NodeHandle to create the Service server. In the callback method, we call the stop() method from MotorDriverROSWrapper, which itself will call the stop() method from MotorDriver.

Send current speed and status to ROS topics (Publisher)

Everything is working now: we can choose default settings using ROS parameters, we can send speed commands to the motor, and we can stop it. What’s missing is the data we can get back from the motor.

Here we’ll use 2 roscpp Publishers: one for the current speed, and one for the status, which for now contains the temperature and voltage of the motor.

For each publisher we’ll use a roscpp Timer to be able to publish data at a fixed rate. Those publishers will wrap the getSpeed() and getStatus() method from the driver.

#include <map>
#include <string>
#include <vector>
...
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/KeyValue.h>
...
class MotorDriverROSWrapper
{
private:
    ...
    ros::Publisher current_speed_publisher_;
    ros::Publisher motor_status_publisher_;
    ros::Timer current_speed_timer_;
    ros::Timer motor_status_timer_;
    double publish_current_speed_frequency_;
    double publish_motor_status_frequency_;
public:
    MotorDriverROSWrapper(ros::NodeHandle *nh)
    {
        ...
        if (!ros::param::get("~publish_current_speed_frequency", publish_current_speed_frequency_))
        {
            publish_current_speed_frequency_ = 5.0;
        }
        if (!ros::param::get("~publish_motor_status_frequency", publish_motor_status_frequency_))
        {
            publish_motor_status_frequency_ = 1.0;
        }
        
        ...
        current_speed_publisher_ = nh->advertise<std_msgs::Int32>("current_speed", 10);
        motor_status_publisher_ = nh->advertise<diagnostic_msgs::DiagnosticStatus>("motor_status", 10);
        current_speed_timer_ = nh->createTimer(
            ros::Duration(1.0 / publish_current_speed_frequency_),
            &MotorDriverROSWrapper::publishCurrentSpeed, this);
        motor_status_timer_ = nh->createTimer(
            ros::Duration(1.0 / publish_motor_status_frequency_),
            &MotorDriverROSWrapper::publishMotorStatus, this);
    }
    void publishCurrentSpeed(const ros::TimerEvent &event)
    {
        std_msgs::Int32 msg;
        msg.data = motor_->getSpeed();
        current_speed_publisher_.publish(msg);
    }
    void publishMotorStatus(const ros::TimerEvent &event)
    {
        diagnostic_msgs::DiagnosticStatus msg;
        std::map<std::string, std::string> status = motor_->getStatus();
        std::vector<diagnostic_msgs::KeyValue> values;
        for (auto const &x : status)
        {
            diagnostic_msgs::KeyValue value;
            value.key = x.first;
            value.value = x.second;
            values.push_back(value);
        }
        msg.values = values;
        motor_status_publisher_.publish(msg);
    }
...

For each method wrap, we use one publisher, one roscpp Timer to publish data, and one ROS param to get the publishing frequency.

On this code you can also see how to transform a std::map into a vector of KeyValue messages. Those messages are part of the DiagnosticStatus message. Check out this latter message definition to see what information you can add – I just used the key values, but there are more.

Complete Cpp ROS wrapper code

The wrapper is now working: we can control the motor, get data from it, and change its default settings.

Here is the complete cpp code for this tutorial.

#include <ros/ros.h>
#include "my_robot_driver/motor_driver.hpp"
#include <memory>
#include <map>
#include <string>
#include <vector>
#include <std_msgs/Int32.h>
#include <std_srvs/Trigger.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/KeyValue.h>
class MotorDriverROSWrapper
{
private:
    std::unique_ptr<MotorDriver> motor_;
    ros::Subscriber speed_command_subscriber_;
    ros::ServiceServer stop_motor_server_;
    ros::Publisher current_speed_publisher_;
    ros::Publisher motor_status_publisher_;
    ros::Timer current_speed_timer_;
    ros::Timer motor_status_timer_;
    double publish_current_speed_frequency_;
    double publish_motor_status_frequency_;
public:
    MotorDriverROSWrapper(ros::NodeHandle *nh)
    {
        int max_speed;
        if (!ros::param::get("~max_speed", max_speed))
        {
            max_speed = 8;
        }
        if (!ros::param::get("~publish_current_speed_frequency", publish_current_speed_frequency_))
        {
            publish_current_speed_frequency_ = 5.0;
        }
        if (!ros::param::get("~publish_motor_status_frequency", publish_motor_status_frequency_))
        {
            publish_motor_status_frequency_ = 1.0;
        }
        motor_.reset(new MotorDriver(max_speed));
        speed_command_subscriber_ = nh->subscribe(
            "speed_command", 10, &MotorDriverROSWrapper::callbackSpeedCommand, this);
        stop_motor_server_ = nh->advertiseService(
            "stop_motor", &MotorDriverROSWrapper::callbackStop, this);
        current_speed_publisher_ = nh->advertise<std_msgs::Int32>("current_speed", 10);
        motor_status_publisher_ = nh->advertise<diagnostic_msgs::DiagnosticStatus>("motor_status", 10);
        current_speed_timer_ = nh->createTimer(
            ros::Duration(1.0 / publish_current_speed_frequency_),
            &MotorDriverROSWrapper::publishCurrentSpeed, this);
        motor_status_timer_ = nh->createTimer(
            ros::Duration(1.0 / publish_motor_status_frequency_),
            &MotorDriverROSWrapper::publishMotorStatus, this);
    }
    void publishCurrentSpeed(const ros::TimerEvent &event)
    {
        std_msgs::Int32 msg;
        msg.data = motor_->getSpeed();
        current_speed_publisher_.publish(msg);
    }
    void publishMotorStatus(const ros::TimerEvent &event)
    {
        diagnostic_msgs::DiagnosticStatus msg;
        std::map<std::string, std::string> status = motor_->getStatus();
        std::vector<diagnostic_msgs::KeyValue> values;
        for (auto const &x : status)
        {
            diagnostic_msgs::KeyValue value;
            value.key = x.first;
            value.value = x.second;
            values.push_back(value);
        }
        msg.values = values;
        motor_status_publisher_.publish(msg);
    }
    void stop()
    {
        motor_->stop();
    }
    void callbackSpeedCommand(const std_msgs::Int32 &msg)
    {
        motor_->setSpeed(msg.data);
    }
    bool callbackStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
    {
        stop();
        res.success = true;
        res.message = "Successfully stopped motor.";
        return true;
    }
};
int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor_driver");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(4);
    spinner.start();
    MotorDriverROSWrapper motor_driver_wrapper(&nh);
    ROS_INFO("Motor driver is now started");
    ros::waitForShutdown();
    motor_driver_wrapper.stop();
}

That’s about twice the length of the same wrapper written in Python. As you may know, with Cpp, you get better performance, but be prepared to write more code, and spend more time developing – which can definitely be worth it.

This code example will set you up for writing your own Cpp ROS wrapper. As you can see, wrapping functions can be done with just the ROS core functionalities: topics, services, (actions), parameters, timers, etc.

You can continue with Package and Test Your Driver if you want to.