Drivers

Device drivers tend to be very different from device to device. Some only read or send data, some do both, they can do that synchronously (blocking operations) or asynchronously (non-blocking operations), etc. They usually have some order of operations that need to be respected (e.g connect before read). Finally, they are often developed by different people/companies and so look very different from one to the other.

In order to bring some coherence, both in terms of interfaces and functionalities, the rpc-interfaces package provides an rpc::Driver class and a set of generic interfaces (rpc::SynchronousInput, rpc::AsynchronousOutput, …) that can be combined to create a standardized wrapper around existing (or newly created) drivers.

Let’s look how this can be achieved with an example.

The existing code

Let’s say we have an existing driver for a robotic arm that looks like this:

// in funky/robot_driver.h

namespace funky {

class RobotDriver {
    // Robot's IP & UDP port
    RobotDriver(std::string ip, int port);

    // Initialize the communication with the robot using the IP and port passed on construction
    bool init();

    // Closes the communication with the robot
    bool end();

    // Wait for a new state coming from the robot
    // Returns true if new data is available and false in case of a communication error
    bool wait();

    // Get a copy of the last joint positions received from the robot
    std::array<double, 6> get_last_joint_positions() const;

    // Set the position targets to be sent to the robot
    void set_joint_position_targets(std::array<double, 6> pos);
};

}

This driver communicates over UDP with the robot and so must be given an IP address and a port number.

Then, as with most robot drivers, the robot sends its current state at a fixed rate (e.g 1000Hz) and expect the driver to send it back the new joint commands as soon as possible.

So we have a wait() function that blocks the calling thread execution until new data arrives, and when it unblocks, we can ask for the current robot’s state, make our computations and send the new position targets to the robot.

The device

Before implementing the driver, the first thing to do is to create a device representing the robot.

Having the device decoupled from the driver allows us to have multiple drivers (e.g real robot + simulator) for the same device without having to change how the client code deals with the device itself.

Here is a proposed device for our robot, reusing the standard components of rpc-interfaces:

// in rpc/devices/funky_robot_device.h

#include <phyq/phyq.h>

namespace rpc::dev {

// Data read from the robot (state)
struct FunkyRobotState {
    phyq::Vector<phyq::Position, 6> position{phyq::zero};
};

// Data sent to the robot (command)
struct FunkyRobotCommand {
    phyq::Vector<phyq::Position, 6> position{phyq::zero};
};

// The robot itself, made of a command data and a state data
struct FunkyRobot : rpc::Robot<FunkyRobotCommand, FunkyRobotState> {
};

}

First, we define our types inside the rpc::dev namespace, as it contains all code related to devices and their drivers.

Then, since we have a robot here, we split the data into its state (FunkyRobotState) and command (FunkyRobotCommand) and create a device, FunkyRobot, deriving from rpc::Robot<FunkyRobotCommand, FunkyRobotState>.

rpc::Robot is a simple class taking the command data type and the state data type as template parameters, storing instances of those and provide access to them through the state() and command() member functions.

You can take a look at the rpc::Robot class template documentation to learn more about it.

You can also notice that we use phyq::Vector<phyq::Position, 6> instead of std::array<double, 6> as in the original driver. Even if not strictly related to drivers, using types from the physical-quantities library has many advantages over using basic types (e.g double) and containers (e.g std::vector, std::array, Eigen::Matrix) and we strongly encourage you to use them as well. You can head over the library’s documentation for more details.

Now that we have our device ready, we can start wrapping the original driver following RPC conventions.

The driver

The first question to ask yourself before wrapping a driver is what kind of driver it is (synchronous vs asynchronous) and what functionalities it provides (read, write, sync, etc).

Having answers for these questions will allow you to pick the correct interfaces for your driver. These interfaces will shape your driver and guide you in what functions to implement so it is very important to get it right.

At the time of writing, rpc-interfaces provides the following driver interfaces:

Most of the time, SynchronousIO or AsynchronousIO will be enough but you can mix and match any of these interfaces together as long as they don’t conflict with each other (e.g SynchronousInput and AsynchronousInput).

For more details about the driver interfaces, you can check out their documentation here.

In our example, the driver is asynchronous, hinted by wait and get_last_xxx functions. The available operations are read/write from/to the device.

So AsynchronousIO is a good fit for this one.

Now that we know what interfaces to use, we can write our driver wrapper:

// in rpc/devices/funky_robot_driver.h

#include <rpc/devices/funky_robot_device.h>
#include <rpc/driver.h>

#include <funky/robot_driver.h>

#include <string>
#include <iostream>
#include <algorithm>

namespace rpc::dev {

class FunkyRobotAsyncDriver final : public rpc::Driver<FunkyRobot, rpc::AsynchronousIO> {
public:
    FunkyRobotAsyncDriver(FunkyRobot& robot, std::string ip, int port) :
        Driver{robot}, // pass the reference to the device to the parent class
        funky_driver_{ip, port} // initialize the original funky driver
    {
    }

    ~FunkyRobotAsyncDriver() final {
        // Don't forget to call disconnect() in the destructor as this cannot be done automatically
        if(not disconnect()) {
            // Log something on error as we can't do much more here
            std::cerr << "Failed to disconnect during destruction\n";
        }
    }

private:
    funky::RobotDriver funky_driver_;

    // Must initiate the communication with the device
    bool connect_to_device() final {
        return funky_driver_.init();
    }

    // Must close the communication with the device
    bool disconnect_from_device() final {
        return funky_driver_.end();
    }

    // Must update the device's state without blocking
    bool read_from_device() final {
        const auto pos = funky_driver_.get_last_joint_positions();
        // Wrap the std::array into a phyq::Vector (no copies here) and assign it to our device state position
        device().state().position = phyq::map<phyq::Vector<phyq::Position>>(pos);
        return true;
    }

    // Must send the device's command without blocking
    bool write_to_device() final {
        std::array<double, 6> pos;
        // Copy the raw values (here double, not phyq::Position) from the device command vector to the std::array
        std::copy(raw(begin(device().command().position)), raw(end(device().command().position)), begin(pos));
        // Send the position commands to the robot
        funky_driver_.set_joint_position_targets(pos);
        return true;
    }

    // Must block until new data is available
    rpc::AsynchronousProcess::Status async_process() final {
        // Just wait for new data to come and return the appropriate status
        if(funky_driver_.wait()) {
            return rpc::AsynchronousProcess::Status::DataUpdated;
        }
        else {
            return rpc::AsynchronousProcess::Status::Error;
        }
    }
};

}

As you can see, there’s not a lot of code involved here as we’re simply calling the right functions and performing the necessary data conversions.

You will notice that all the functions, except for the constructor, are private. This is to force the users to go through the rpc::Driver public API which makes sure that the driver is always in the correct state before performing an operation. For instance, calling read() will call connect() first if not already connected. If you were to call read_from_device() directly, nothing guarantees that the call will succeed and, if it does, that it provides any meaningful data.

Usage

Now that we have our device and driver ready, we can use them like this:

#include <rpc/devices/funky_robot_device.h>
#include <rpc/devices/funky_robot_driver.h>

int main() {
    rpc::dev::FunkyRobotDevice robot;
    rpc::dev::FunkyRobotAsyncDriver driver{robot, "192.168.1.2", 4789};

    if(not driver.connect()) {
        std::cerr << "Cannot connect to the robot\n";
        return 1;
    }

    for(int i=0; i<10; i++) {
        if(not driver.sync()) {
            std::cerr << "Failed to synchronize with the robot\n";
            return 2;
        }

        if(not driver.read()) {
            std::cerr << "Failed to read the robot state\n";
            return 3;
        }

        robot.command().position = robot.state().position + phyq::Vector<phyq::Position, 6>::random();

        if(not driver.write()) {
            std::cerr << "Failed to send the command to the robot\n";
            return 4;
        }
    }

    if(not driver.disconnect()) {
        std::cerr << "Failed to disconnect from the robot\n";
        return 5;
    }
}

Or, by relying more on the rpc::Driver internal state machine and grouping checks together:

#include <rpc/devices/funky_robot_device.h>
#include <rpc/devices/funky_robot_driver.h>

int main() {
    rpc::dev::FunkyRobotDevice robot;
    rpc::dev::FunkyRobotAsyncDriver driver{robot, "192.168.1.2", 4789};

    for(int i=0; i<10; i++) {
        if(not (driver.sync() and driver.read())) {
            std::cerr << "Failed to read the robot state\n";
            return 1;
        }

        robot.command().position = robot.state().position + phyq::Vector<phyq::Position, 6>::random();

        if(not driver.write()) {
            std::cerr << "Failed to send the command to the robot\n";
            return 2;
        }
    }

    // Disconnection happens on destruction automatically
}

It is recommended at always check the driver’s functions return value (warning will be issues if you don’t) and react accordingly. Here we simply log the error and exit but depending on the application you might want to do something else (e.g retry N times before reporting an actual error).

Side notes

1. It is difficult to demonstrate all the possibilities in a tutorial like this, so if you want to look at some real-world implementations you an check out:
   • panda-robot: driver for the Franka Panda robot. Makes use of rpc::control::ControlModes to deal with the multiple control modes properly
   • ati-force-sensor-driver: driver for ATI force sensors connected on a Comedi compatible acquisition card. Features multiple driver implementations for the same device.

2. While rpc::Driver provides a uniform interface for all kinds of drivers, each driver has its own type without any common base class so you can’t put a bunch them in a container and deal with them uniformly. If you require such feature, you can take a look at rpc::AnyDriver and rpc::DriverGroup.

3. Asynchronous drivers have an execution policy which dictate if the communication thread should be created and handled automatically or not. By default everything is automatic but if you require more control, e.g in an RTOS, you might want to have a look to the documentation of rpc::AsyncPolicy and rpc::AsynchronousProcess.

4. In the example above, FunkyRobotAsyncDriver holds an instance of funky::RobotDriver directly. This implies that the funky/robot_driver.h header must be included where FunkyRobotAsyncDriver is declared, even though the user doesn’t have access to funky_driver_ and so cannot make use of its API. Reducing the number of included files is good for compilation times and might avoid bringing some macros and globals into the public interface, which can have impacts on the client code. To solve that problem, you can rely on the pointer to implementation idiom (pimpl for short). The drivers listed in 1. make use of this and you can read more about it here.

5. If you want to log messages in your driver, avoid printing them to the standard output directly as it can introduce a lot of noise in client applications. You can use pid-log instead for a PID-integrated solution or spdlog for a third-party one.