Basically a robocop driver is an interface to an existing real driver code which adds code to update and/or read the robot model. Robocop imposes no specific execution policy for drivers, it is left to the driver implementation to define this policy, that is :

  • synchronous, meaning that interaction with hardware is executed in the same loop as the controller that generates command and read state to/from it. This is not always feasible in practice as it requires the driver to be non blocking on read/write operations. Furthermore if any periodicity constraint applies to this driver, then the related period must match the period of the controller.
  • asynchronous, meaning that interaction with hardware is executed in another thread, which is the most common case since it decouples driver execution and controller execution and avoid most of problems (read/write to hardware, which is a long operation, is performed in another execution thread ; period can be different ; read can eventually be blocking). In this case, special caution must be taken on data sharing between driver and controller.

If Robocop does not impose a strict and specific pattern for drivers implementation, we highly recommend following the pattern proposed by rpc-interfaces package. Most if not all of existing RoboCoP drivers have been implemented using this pattern. If you need clarifications please read this tutorial.

The tutorial takes as example the driver defined in robocop-flir-ptu-driver package. You can deploy it in your workspace to have a complete view of the driver resulting from this tutorial:

pid workspace deploy package=robocop-flir-ptu-driver

CMake description

Create a new package for the drobocop driver in your application workspace. For instance:

pid workspace create package=robocop-flir-ptu-driver

In root CMakeLists.txt of this package simply add the following dependencies:

PID_Dependency(robocop-core VERSION 0.1)
PID_Dependency(rpc-interfaces VERSION 1.1)
PID_Dependency(flir-sdk VERSION 2.0)
PID_Dependency(yaml-cpp FROM VERSION 0.6.2)
  • robocop-core is the mandatory dependency when using robocop
  • flir-sdk is the real driver for the FLIR pan-tilt unit
  • yaml-cpp package is used for robocop configuration parsing
  • It is recommended to use RPC way of defining driver as the integration with robocop will be straightforward, so you can also add rpc-interfaces as dependency. In the example flir-sdk is already a RPC` defined driver so the dependency is not mandatory.

It is recommended to add example to test the driver, so you should add the depenency to the package that provides robot/sensor description when building examples, for instance:

if(BUILD_EXAMPLES)
    PID_Dependency(robocop-flir-ptu-description VERSION 0.1)
endif()

Finally you should add the newly created package to the robocop framework for a nice referencing, something like:

PID_Publishing(
    PROJECT https://gite.lirmm.fr/robocop/driver/robocop-flir-ptu-driver
    DESCRIPTION Wrapper around rpc/flir-sdk
    FRAMEWORK robocop
    CATEGORIES driver
    PUBLISH_DEVELOPMENT_INFO
    ALLOWED_PLATFORMS
        x86_64_linux_stdc++11__ub20_gcc9__
        x86_64_linux_stdc++11__ub20_clang10__
        x86_64_linux_stdc++11__arch_gcc__
        x86_64_linux_stdc++11__arch_clang__
        x86_64_linux_stdc++11__ub18_gcc9__
        x86_64_linux_stdc++11__fedo36_gcc12__
)

The driver is impelmented as a library, so package’s src/CMakeLists.txt is like:

PID_Component(
    processors
    DEPEND
        robocop/core
        yaml-cpp/yaml-cpp
    WARNING_LEVEL ALL
)

PID_Component(
    flir-ptu-driver
    DESCRIPTION RoboCoP wrapper around the FLIR Pan tilt unit driver
    USAGE robocop/driver/flir_ptu.h
    EXPORT
        robocop/core
        rpc/interfaces
    DEPEND
        robocop-flir-ptu-driver/processors
        robocop/data-logger
        flir-sdk/flir-ptu
    CXX_STANDARD 17
    WARNING_LEVEL ALL
)

Explanations:

  • The first component defines a module library called processors (this name is not mandatory but advised for homogeneity).This library is in charge of code customization provided by the driver during robocop code generation process. It must depend on robocop/core for getting access to the RoboCoP API and yaml-cpp/yaml-cpp to parse configuration options.

  • The second library implements the RoboCoP driver, by convention its name should be the name of the package without the robocop prefix (flir-ptu-driver in the example). It also depends on robocop/core to get access to RoboCoP APIs, because a driver needs to manipulate concepts like World, JointGroup, etc. We want the driver to conform to RPC driver so it depends on rpc/interfaces. There are also private dependencies:

    • the processors library. This dependency is usefull for automatic retrieval of the processors library used to configure the driver.
    • the real driver component. In the example it is rpc/kuka-lwr-driver.
    • other dependencies required to write the code. Here robocop/data-logger is used to generate log traces from driver asynchronosu execution.

Example

To test your driver add an example in apps/CMakeLists.txt:

PID_Component(
    flir-ptu-driver-example
    EXAMPLE
    DESCRIPTION Demonstrate how to control the Pan tilt unit in joint position mode
    DEPEND
        robocop/flir-ptu-driver
        robocop/flir-ptu-description
        robocop/data-logger
        pid/log
    CXX_STANDARD 17
    WARNING_LEVEL ALL
)

Robocop_Generate(
    flir-ptu-driver-example
    robocop-flir-ptu-driver-examples/config.yaml
)

The example depends on the driver and the description of the device, in the example respectively robocop/flir-ptu-driver and robocop/flir-ptu-description. It can also use other dependencies as needed.

The World used in example is generated from the configruation file as usual in RoboCoP so you have to call the Robocop_Generate CMake function with this file as parameter. In the example the file is share/resources/robocop-flir-ptu-driver-examples/config.yaml. We advise to edit this file during the development of the driver to track driver options you have to manage. In a first time the YAML code is:

models:
  - include: robocop-flir-ptu-description/models/ptu_e46.yaml

processors:
  driver:
    type: robocop-flir-ptu-driver/processors/driver

For now, the application declares using the FLIR ptu E46 and the driver library currently developped.

Finaly simply create the file apps/flir-ptu-driver-example/flir_ptu_driver_example.cpp with en empty main function:

#include <robocop/driver/flir_ptu.h>
#include "robocop/world.h"
int main() {}

Coding guidelines

Let’s start driver code development.

Structure

You should follow the following structure for driver libraries:

  • for the public interface: include/<library_name>/robocop/driver is the folder containing driver public headers. There should be a unique global public header <name of device> at the root of this directory, in the example: include/flir-ptu-driver/robocop/driver/flir_ptu.h. It is the unique entry point for accessing your library definitions. If your driver code required extra code put more headers into a include/<library_name>/robocop/driver/<name of device> folder and include these headers in library global header. Please note that you should reference the global header in the component description (see USAGE in previous section).
  • for the implementation of the driver library: src/<library_name>/ is the folder containing all sources, structured the way you want. In the example package we have a folder src/flir-ptu-driver.
  • for the implementation of the processors module library: src/processors s the folder containing module sources.

Interface

First thing to do is to define the interface of the driver library. flir-ptu-driver is a simple library so we need a unique public header include/flir-ptu-driver/robocop/driver/flir_ptu.h:

#pragma once

#include <robocop/core.h>
#include <rpc/driver/driver.h>
#include <memory>

namespace robocop {
struct FlirPTUDevice {
    JointGroupBase* joint_group{};
};

class FlirPTUAsyncDriver final
    : public rpc::Driver<robocop::FlirPTUDevice, rpc::AsynchronousIO>,
      public DriverCommandAdapter<AsyncKinematicCommandAdapterBase,
                                  FlirPTUAsyncDriver> {
public:
    FlirPTUAsyncDriver(WorldRef& world, std::string_view processor_name);
    ~FlirPTUAsyncDriver();

    template <typename T, typename... Args>
    T& replace_command_adapter(Args&&... args) {
        return DriverCommandAdapter::replace_command_adapter<T>(
            *device_.joint_group, std::forward<Args>(args)...);
    }

private:
    [[nodiscard]] bool connect_to_device() final;
    [[nodiscard]] bool disconnect_from_device() final;
    [[nodiscard]] bool read_from_device() final;
    [[nodiscard]] bool write_to_device() final;
    rpc::AsynchronousProcess::Status async_process() final;

    robocop::FlirPTUDevice device_;

    class pImpl;
    std::unique_ptr<pImpl> impl_;
};
} // namespace robocop
  1. You need to include :
    • #include <robocop/core.h> to get access to RoboCoP API.
    • #include <rpc/driver/driver.h> to create a RPC conformant driver.
  2. Then you must put your C++ declarations in the robocop namespace.
  3. Then you must define a device. A device (in RPC) is a simple class containing data read/updated by a driver. In robocop a driver basically contains the elements of the world that are read/updated (most of time a joint_group). We use pointers to reference elements of the world but we could also use references or even objects (because robocop types are reference). In the example the device type is FlirPTUDevice and contains a pointer to the joint group containing pan and tilt joints.
  4. Then comes the driver class definition (FlirPTUAsyncDriver). Here our driver conforms to RPC style so the class inherits from rpc::Driver, we stringly recommend to use RPC stlye definition for all robocop drivers. For deeper understanding of how RPC drivers work and how to create new ones, you can read this tutorial. In the example we define an asynchronous driver that reads/writes a device of type FlirPTUDevice so FlirPTUAsyncDriver inherits rpc::Driver<robocop::FlirPTUDevice, rpc::AsynchronousIO>. Due to this inheritance we have to overload abstract methods connect_to_device, disconnect_from_device, read_from_device, write_to_device and async_process. Constructor also inherits DriverCommandAdapter but this will be explained later.
  5. Define a constructor that takes a WorldRef and string_view as argument (FlirPTUAsyncDriver(WorldRef& world, std::string_view processor_name)). The WorldRef is the object reprensenting the RoboCoP world that contains data read/updated by the drive. The string view is the name of the processor configuration used to configure the driver, it is a user defined name corresponding to an entry in application configuration file. In the example application configuration file the corresponding name is driver.
  6. Add an attribute of device type to the class: robocop::FlirPTUDevice device_.

In the example we use the pimpl idiom to hide real driver implementation, which is a recommended technic to avoid exporting too many headers in the end. So mostly everything is performed in source files.

Now let’s see how a driver is implemented.

Implementation

First of all, you need to understand how RPC drivers are implemented, have a look at this tutorial if needed.

Now let’s see how a driver is implemented (complete code is in file src/flir-ptu-driver/flir_ptu.cpp).

#include <robocop/driver/flir_ptu.h>
...
namespace robocop {
...
class FlirPTUAsyncDriver::pImpl {...};

FlirPTUAsyncDriver::FlirPTUAsyncDriver(WorldRef& robot,
                                       std::string_view processor_name)
    : Driver{&device_},
      DriverCommandAdapter<AsyncKinematicCommandAdapterBase,
                           FlirPTUAsyncDriver>(),
      impl_{std::make_unique<pImpl>(robot, this, processor_name)} {
}
FlirPTUAsyncDriver::~FlirPTUAsyncDriver() {
    (void)disconnect();
}
bool FlirPTUAsyncDriver::connect_to_device() {
    return impl_->connect_to_device();
}
bool FlirPTUAsyncDriver::disconnect_from_device() {
    return impl_->disconnect_from_device();
}
bool FlirPTUAsyncDriver::read_from_device() {
    return impl_->read_from_device();
}
bool FlirPTUAsyncDriver::write_to_device() {
    return impl_->write_to_device();
}
rpc::AsynchronousProcess::Status FlirPTUAsyncDriver::async_process() {
    return impl_->async();
}
} // namespace robocop

The FlirPTUAsyncDriver class definition follows the pimpl idiom so:

  • its constructor instanciate the implementation object (using a unique pointer for instance), named impl_.
  • most of its methods definitions are a simple forwarding to the real implementation provided by this implementation object.

The FlirPTUAsyncDriver class definition follows the RPC interfaces pattern so:

  • its construuctor call the rpc:Driver super class constructor. The indea here is to pass a pointer to the local device_ object, even if this later is not instanciated yet (we are sure it is instanciated when FlirPTUAsyncDriver construction ends).
  • its destructor has to call the disconnect() method.

The FlirPTUAsyncDriver being a DriverCommandAdapter its constructor also call this super class constructor, but this will be explained later.

For now all the real implementation is performed in the implementation, so let’s see its code:

#include <robocop/driver/flir_ptu.h>
#include <rpc/devices/flir_ptu.h>

namespace robocop {
namespace {
struct Options {
    ...
};
} // namespace

class FlirPTUAsyncDriver::pImpl {
public:
    pImpl(WorldRef& world, FlirPTUAsyncDriver* driver,
          std::string_view processor_name)
        : options_{processor_name},
          robocop_device_{&driver->device()},
          mode_manager_{world.joint_group(options_.joint_group)},
          rpc_device_{},
          rpc_driver_{rpc_device_, options_.connection} {
        rpc_driver_.policy() = rpc::AsyncPolicy::ManualScheduling;
        
        robocop_device_->joint_group = &mode_manager_.joint_group();
        robocop_device_->joint_group->command().update<Period>(
        [this](JointPeriod& cmd_period) {
            cmd_period.set_constant(rpc_driver_.command_period());
        });
        ...
    }

    bool connect_to_device() {
        return rpc_driver_.connect();
    }

    bool disconnect_from_device() {
        return rpc_driver_.disconnect();
    }

   ...

    bool read_from_device() {
        if (not rpc_driver_.read()) {
            return false;
        }

        if (options_.read_states.joint_position) {
            auto jp = robocop_device_->joint_group->state()
                            .get<robocop::JointPosition>();
            jp[0] = rpc_device_.state().pan.position;
            jp[1] = rpc_device_.state().tilt.position;
            robocop_device_->joint_group->state().set(jp);
        }
        ... //same for velocity and acceleration

        if (robocop_device_->joint_group->limits()
                .lower()
                .has<robocop::JointPosition>()) {
            robocop_device_->joint_group->limits().lower().set(
                robocop::JointPosition{
                    {rpc_device_.limits.pan.min_position.value(),
                        rpc_device_.limits.tilt.min_position.value()}});
        }
        ... //same for other limits
        return true;
    }

private:
    Options options_;
    ControlModeManager mode_manager_;

    robocop::FlirPTUDevice* robocop_device_;
    // real implementation
    rpc::dev::FlirPTU rpc_device_;
    rpc::dev::FlirPTUDriver rpc_driver_;
};
...
}

There are two important and complementary things that a robocop driver has to manage:

  • the call to the real driver implementation to really do the job.
  • the configuration of the driver from application options. Options are particularly usefull to delegate configuration of some aspects of the driver to the application configuration time, typically network connections. Options management is discussed in next section. Just consider for now that options are managed using a dedicated Option object.

The implementation code uses a ControlModeManager that is an utility used to ease management of control mode when the driver has an asynchronous execution. To sum up it is used to enforce data protection and copy while reading the world: read variables depend on the current control mode for joints, set by the robot controller. Control mode management as well as command adaptation will be explained in last section.

The basic role of the driver implementation is to define a bridge between the real driver and RoboCoP world. More preciselly it has to:

  • set inputs of the real driver with joints commands defined in the world (those typically written by controllers).
  • get outputs of the real driver to set joints states defined in the world (those typically read by controllers).

That is why it is common to have in driver classes a pointer to the device used inside the robocop driver: in the example robocop_device_ attribute. The role of the constructor is to initialize robocop_device_ depending on options, so here depending on the target joint group. Remember that the joint group is not prefixed and only the application code can define which joint group is targetted by a driver. Corresponding code is:

robocop_device_->joint_group = &mode_manager_.joint_group();
robocop_device_->joint_group->command().update<Period>(
    [this](JointPeriod& cmd_period) {
        cmd_period.set_constant(rpc_driver_.command_period());
    });

The joint_group of the robocop device owned by Driver interface class is set to the joint group defined by the mode manager (instead of the joint group directly defined in the world but it is similar). Then command_period of this joint group is set: this will set the period at which joints of the group are really set by the driver. This last action is really important for command adaptation as well as for the controller of these joints.

Other attributes are related to real driver implementation. In the example rpc_device_ and rpc_driver_ are respectively the base device and driver of the pan tilt unit following the same pattern has the robocop driver. The pImpl constructors call constructors for these attributes. You should notice that connection information coming from options is given when calling the constructor of the real driver. The driver being a rpc asynchronous driver its communication thread is autmoatically managed by default, what we want to avoid because the communication thread will be handled by default by FlirPTUAsyncDriver (because it is also a RPC driver). So we need to deactivate default behavior in constructor:

rpc_driver_.policy() = rpc::AsyncPolicy::ManualScheduling;

With RPC based real driver connection management is straightforward because it is a direct delegation:

bool connect_to_device() {
    return rpc_driver_.connect();
}
bool disconnect_from_device() {
    return rpc_driver_.disconnect();
}

With a RPC based real driver that is synchronous, write function is also really straightforward to implement: it reads information on world’s joints and set the RPC device accordingly before calling the write function of the real driver. This could be implemented like:

bool write_to_device() {
    auto jp = robocop_device_->joint_group->command()
                        .get<robocop::JointPosition>();
    rpc_device_.command().pan.position = jp[0];
    rpc_device_.command().tilt.position = jp[1];
    return rpc_driver_.write()
}

Unfortunately most of RoboCoP driver are asynchronous so things are a bit more difficult to cleanly deal with data protection while reading/writing from/to the world, what is achieved using the mode_manager_ and explained later.

Thread read method is kept unchanged whether the driver is asynchronous or synchronous: it reads the state of the robot using RPC driver read method and then update the RoboCoP world (joints state) from the updated RPC device:

bool read_from_device() {
    //read the real robot state using real driver
    if (not rpc_driver_.read()) {
        return false;
    }

    if (options_.read_states.joint_position) {
        //build the current joint position vector from rpc device state 
        auto jp = robocop_device_->joint_group->state()
                        .get<robocop::JointPosition>();
        jp[0] = rpc_device_.state().pan.position;
        jp[1] = rpc_device_.state().tilt.position;
        //set the the current state of the joint group in RoboCoP world
        robocop_device_->joint_group->state().set(jp);
    }
   ... //same for velocity and acceleration

    if (robocop_device_->joint_group->limits()
            .lower()
            .has<robocop::JointPosition>()) {
        //set current limits of the joint group in RoboCoP world
        robocop_device_->joint_group->limits().lower().set(
            //build the joint position lower limit vector from rpc device state 
            robocop::JointPosition{
                {rpc_device_.limits.pan.min_position.value(),
                    rpc_device_.limits.tilt.min_position.value()}});
    }
   ... //same for other limits
    return true;
}

Options management

As already said options management is particularly important to adequately configure the drivers from application configuration. Options of the driver should be managed using a specific object (called options_ in the example).

Option object is in charge of parsing the YAML options to get all information required to configure the driver. Those information are used partcularly for:

  • configuring the RoboCoP device information, basically joint group controlled by the driver (pan and tilt joints in the examle).
  • configuring the real driver required information like connection data.
  • eventually configuring the behavior of the robocop driver itself.

Code for managing options always more or less follows the followingsame pattern, as in example:

namespace robocop {
namespace {
struct Options {
    struct DataToRead {
        explicit DataToRead(const YAML::Node& config)
            : joint_position{config["joint_position"].as<bool>(false)},
              joint_velocity{config["joint_velocity"].as<bool>(false)},
              joint_acceleration{config["joint_acceleration"].as<bool>(false)} {
        }

        bool joint_position{};
        bool joint_velocity{};
        bool joint_acceleration{};
    };

    explicit Options(std::string_view processor_name)
        : connection{ProcessorsConfig::option_for<std::string>(
              processor_name, "connection", "")},
          joint_group{ProcessorsConfig::option_for<std::string>(
              processor_name, "joint_group", "")},
          read_states{ProcessorsConfig::option_for<YAML::Node>(
              processor_name, "read", YAML::Node{})} {
    }

    std::string connection;
    std::string joint_group;
    DataToRead read_states;
};
} // namespace
...
}

An Options class holds the attributes required to store configuration information, in the example:

  • connection IP
  • target joint group name in
  • model update flags joint_position, joint_velocity, joint_acceleration, used to control update of the joints corresponding states. When those field are themselves entries of a submap, they are embedded in an inner class, like for DataToRead in the example.

All the job is done at object construction time, in the constructor that takes at least the processor name as input (you may also need to pass WorldRef to directly access world). The processor name matches an enty of the application configuration file. To memorize possible options it is important to edit the test example configuration file and define them directly under the standard options field of the processor:

models:
  - include: robocop-flir-ptu-description/models/ptu_e46.yaml

processors:
  driver:
    type: robocop-flir-ptu-driver/processors/driver
    options:
        connection: "tcp:192.168.0.101"
        joint_group: ptu
        read: #I read everything
            joint_position: true
            joint_velocity: true
            joint_acceleration: true

The processor name in the example is driver, but we could have used any other unique identifier (e.g. my-super-ptu-driver). The code of the Options class can then call the dedicated function provided by RoboCoP to parse the options field: ProcessorsConfig::option_for paramaterized with the adequate types for the given entries. For instance, in Options constructor:

Options(std::string_view processor_name):
    //convert to a string
    connection{ProcessorsConfig::option_for<std::string>(processor_name, "connection", "")},
    ...
    // For maps or complex types use `YAML::Node`
    read_states{ProcessorsConfig::option_for<YAML::Node>(processor_name, "read", YAML::Node{})}...

There is no much to do with option except using them to configure the driver. Nevertheless, we did no validity check of the YAML content at this stage so we consider this check as already been done.

Processor

The configuration file is loaded and parsed at compile time by the RoboCoP generator:

  • to generate a world containing adequate features (typically adequate features for joint state/commands).
  • to ensure validity of the configuration file content. Checks for YAML content validity should be performed at this time so validity check should be written in the code of the driver processor module library.

Remaining part of this section shows how to implement the a processor module. Implementation of a processor module simply relies on the implementation of the generate_content function:

#include <robocop/core/generate_content.h>
#include <fmt/format.h>
#include <yaml-cpp/yaml.h>

bool robocop::generate_content(const std::string& name,
                               const std::string& config,
                               WorldGenerator& world) noexcept {
    ...
    return true;
}

Arguments are:

  • name: the last name of processor type (driver in the example). Please notice that the type field of processors in configuration file is parsed this way:
    • left most name before first / if the package name (in the example robocop-flir-ptu-driver).
    • following name before next / if the module library name (in the example processors).
    • last name is the processor name (in the example driver). This is a unique identifier in the context of the module library that matches a given class, here the FlirPTUAsyncDriver class.
  • config is the YAML string corresponding to the options section of an entry of the processors map in application configuration file.
  • world is the reference to the world generator currently running and that the processor can tune.

The generate_content should first check the structural validity of the YAML file content:

...
bool robocop::generate_content(const std::string& name,
                               const std::string& config,
                               WorldGenerator& world) noexcept {
    if (name != "driver") {
        return false;
    }
    auto options = YAML::Load(config);
    if (not options["connection"]) {
        fmt::print(stderr,
                   "When configuring processor {}: no connection defined\n",
                   name);
        return false;
    }
    if (not options["joint_group"]) {
        fmt::print(stderr,
                   "When configuring processor {}: no joint_group defined\n",
                   name);
        return false;
    }
    auto jg_name = options["joint_group"].as<std::string>();
    if (not world.has_joint_group(jg_name)) {
        fmt::print(stderr,
                   "When configuring processor {}: the given "
                   "joint group {} is not part of the world\n",
                   name, jg_name);
        return false;
    }
    if (world.joint_group(jg_name).size() != 2) {
        fmt::print(stderr,
                   "When configuring processor {}: invalid number of "
                   "joints in joint group {}. Expected 2, got {}\n",
                   name, jg_name, world.joint_group(jg_name).size());
        return false;
    }
    ...
    return true;
}

the generate_content function returns true on success and false if there is a description error in the YAML configuration file.

What is checked in the above example:

  • only driver type is known in package robocop-flir-ptu-driver.
  • options connection and joint_group are mandatory.
  • the target joint group must exist in the world and contains 2 joints.

After verification part comes the optional code generation part:

...
bool robocop::generate_content(const std::string& name,
                               const std::string& config,
                               WorldGenerator& world) noexcept {
   ...
    // now adding required attributes to the joint grop
    // only arguments written locally (i.e. state) are declared here
    const auto read = options["read"].as<std::map<std::string, bool>>();

    auto value_or = [](const auto& map, const auto& key,
                       const auto& default_value) {
        if (auto it = map.find(key); it != map.end()) {
            return it->second;
        } else {
            return default_value;
        }
    };
    if (value_or(read, "joint_position", false)) {
        world.add_joint_group_state(jg_name, "JointPosition");
    }
    if (value_or(read, "joint_velocity", false)) {
        world.add_joint_group_state(jg_name, "JointVelocity");
    }
    if (value_or(read, "joint_acceleration", false)) {
        world.add_joint_group_state(jg_name, "JointAcceleration");
    }

    world.add_joint_group_command(jg_name, "Period");

    return true;
}

Depending on the read entry in the configuration file, the target joints state contains or not some data: JointPosition, JointVelocity, JointAcceleration . Furthermore the target joints command contains a Period data describing the driver internal update rate (that can be tuned in this situation).

When processor module hs been written options are fully managed by your package. To test if your generate_functoin behaves as intended (more spcifically if you ant to test if errors are reported) you can simply call the code generator:

pid build -DBUILD_EXAMPLES=ON

The code generator is automatically called because examples are built. You can see on screen if any error is reported with a valid description inside share/resources/robocop-flir-ptu-driver-examples/config.yaml and reversely you can introduce voluntary description errors to check if they are well detected.

Additionnal methods: control mode management, command adaptation and thread logging

For now large parts of driver code has been left unexplained and its time to go into much details. Again, all elements explained in this section are optional but we strongly recommend to apply the following methods as often as possible. Indeed those methods and related code have been design to simplify and robustify driver code writing.

Command adapter

The first important concept to understand is the command adaptation. Command adaptation is te mechanism that has to take place when control mode of joints defined by the driver is no the same as the controller output of joints defined by the controller, which is a quite common situation. For instance the controller may set the joints velocity command while the driver waits for position commands. Then the code needs to integrate velocity commands to get position commands. Furthermore if driver is asynchronous this integration must take care of the driver execution period. Beyond this example, one can imagine various scenarios for command adaptation and RoboCoP provide a generic mechanism to deal with that.

Command adaptation is realized by inheriting the DriverCommandAdapter template class, like in the flir_ptu.h file:

...
class FlirPTUAsyncDriver final
    : public rpc::Driver<robocop::FlirPTUDevice, rpc::AsynchronousIO>,
      public DriverCommandAdapter<AsyncKinematicCommandAdapterBase,
                                  FlirPTUAsyncDriver> {
public:
    ...
    template <typename T, typename... Args>
    T& replace_command_adapter(Args&&... args) {
        return DriverCommandAdapter::replace_command_adapter<T>(
            *device_.joint_group, std::forward<Args>(args)...);
    }
    ...
};
...

The method template replace_command_adapter is defined to allow application code to change the command adapter used by the driver to transform commands. Template parameter T represents the command adaptation class to be used, while args are arguments eventually used for constructing adapter object of type T. Its implementation uses the base utility function replace_command_adapter provided by DriverCommandAdapter class. The DriverCommandAdapter is templated with:

  • a base adaptor class, in the example AsyncKinematicCommandAdapterBase. This base class represents all possible adaptation of kinelmatic commands (position, velocity, acceleration). This is the only existing base adapter type but user can defined ne ones if needed.
  • the driver class itself (FlirPTUAsyncDriver). This is implementation related (based on CRTP).

This method is used in constructor to initialize the default command adapter used:

class FlirPTUAsyncDriver::pImpl {
public:
    pImpl(WorldRef& world, FlirPTUAsyncDriver* driver,
          std::string_view processor_name) ... {
        ...
        driver->replace_command_adapter<AsyncKinematicCommandAdapterBase>();
        ...
    }
    ...
};

The constructor code declare using the base implementation for kinematic command adaptation. This base implementation in fact performs no adaptation. So if there is a mismatch between controller output and control mode of joints, an error will be generated (because no code writes the joint position command).

The only default defined command adapters class is SimpleAsyncKinematicCommandAdapter. It is used to perform any possible adaptation between kinematic (position, velocity and acceleration) commands using adequate time derivation and integration.

So somewhere in the application code we could enforce command adaptation:

    ...
    driver->replace_command_adapter<SimpleAsyncKinematicCommandAdapter>();
    ...

All this mechanism is used to enforce flexibility in command adaptation, allowing to customize command adaptation without modifying drivers code. Next section shows how exploit command adaptation.

Important Note: the DriverCommandAdapter mechanism is usefull only for asynchronous drivers (most common situation). For synchronous drivers, world access (no need for data protection) and time derivation/integration are straightforward (same control period between controller and driver dy definition) so adaptation can directly take place in the write_to_device() function of the driver. RoboCoP provides the SimpleKinematicCommandAdapter class that can be used direcly to adapt commands before write using real driver.

Control mode management

For asynchronous drivers an important topic is to enure data protection between conroller thread and driver thread. This data protection consists in copying information coming from the world when write() function is called in conroller thread into a protected buffer. Then when the async() function is called by thread this buffer is read to get data that will be use to set inputs of real drivers write() function (or equivalent if real driver is not RPC conformant).

Furthermore, depending on the actual control mode of joints in the driver, actions to realize differ. For instance the command to set may be different whether the robot is controlled in velocity or position mode.

To deal with control mode management in a clean and homogeneous way, RoboCoP provides a ControlModeManager class. Here is the code using the contro mode object:

class FlirPTUAsyncDriver::pImpl {
public:
    pImpl(WorldRef& world, FlirPTUAsyncDriver* driver,
          std::string_view processor_name) : ...
          mode_manager_{world.joint_group(options_.joint_group)}, ... {
        
        mode_manager_.register_mode(
            control_modes::none, [] { return true; },
            [this] {
                rpc_device_.command().reset();
                return rpc_driver_.write();
            });
       ...
    }

    ...

    rpc::AsynchronousProcess::Status async() {
        if (mode_manager_.process()) {
            return rpc_driver_.run_async_process();
        } else {
            return rpc::AsynchronousProcess::Status::Error;
        }
    }

    bool write_to_device() {
        return mode_manager_.read_from_world();
    }

    ...
};

At driver construction time the mode manager is constructed with the driver joint group passed as argument, to know what to read and memorize from the world:

mode_manager_{world.joint_group(options_.joint_group)}, ...

And then the available control modes of the driver are declared using the register_mode method of the mode manager:

mode_manager_.register_mode(
        control_modes::none, [] { return true; },
        [this] {
            rpc_device_.command().reset();
            return rpc_driver_.write();
        });
...

As first argument, this function takes the control mode being defined. As second and third arguments, it takes two functions, we use lambda expressions to define them:

  • first function implements the behavior of the driver when the write_to_device() method (or equivalent) is called when the corresponding control mode is active.
  • second function implements the behavior of the driver when the async() method (or equivalent threaded periodic function) is called when the corresponding control mode is active.

To put in place those reactions on corresponding call:

rpc::AsynchronousProcess::Status async() {
    if (mode_manager_.process()) {
        return rpc_driver_.run_async_process();
    } else {
        return rpc::AsynchronousProcess::Status::Error;
    }
}

bool write_to_device() {
    return mode_manager_.read_from_world();
}

The write_to_device() function is simply calling the mode manager read_from_world() that automatically does the correct operations to memorize adequate informations coming from the world depending on the joints control mode and controller output while protecing shared memory between controller thread and async driver thread.

The async() function call the process() function of the mode manager that performs adequate actions depnending on the mode. The real driver being asynchronous it also has write() and run_async_process() functions. In this situation both wll be executed in the same thread:

  • write() is called by mode manager lambda, so set the value of the data shared with its async() function.
  • run_async_process() is called only if mode manager process() succeeded.

If rpc_driver_ was synchronous we could have avoided the call to run_async_process(), simply dealing with returned error code depending on process result.

Command adaptation by the mode manager

Now let’s see how the control mode manager uses the DriverCommandAdapter mechanism to put in place command adaptation:

class FlirPTUAsyncDriver::pImpl {
public:
    pImpl(WorldRef& world, FlirPTUAsyncDriver* driver,
          std::string_view processor_name)...{
        ...

        mode_manager_.register_mode(
            control_modes::none, [] { return true; },
            [this] {
                rpc_device_.command().reset();
                return rpc_driver_.write();
            });

        mode_manager_.register_mode(
            control_modes::position,
            [driver, &world] {
                return driver->command_adapter().read_from_world(world);
            },
            [this, driver] {
                if (driver->command_adapter().process() and
                    driver->command_adapter().output().position) {
                    auto& cmd =
                        rpc_device_.command()
                            .get_and_switch_to<
                                rpc::dev::FlirPTUJointPositionCommand>();

                    // set the limits to the maximum
                    cmd.pan.max_velocity = rpc_device_.limits.pan.max_velocity;
                    cmd.tilt.max_velocity =
                        rpc_device_.limits.tilt.max_velocity;
                    // set the position
                    const auto& pos_cmd =
                        *driver->command_adapter().output().position;
                    cmd.pan.target_position = pos_cmd[0];
                    cmd.tilt.target_position = pos_cmd[1];
                    return rpc_driver_.write();
                } else {
                    return false;
                }
            });

        mode_manager_.register_mode(
            control_modes::velocity,
            [driver, &world] {
                return driver->command_adapter().read_from_world(world);
            },
            [this, driver] {
                if (driver->command_adapter().process() and
                    driver->command_adapter().output().velocity) {
                    auto& cmd =
                        rpc_device_.command()
                            .get_and_switch_to<
                                rpc::dev::FlirPTUJointVelocityCommand>();
                    const auto& vel_cmd =
                        *driver->command_adapter().output().velocity;
                    cmd.pan = vel_cmd[0];
                    cmd.tilt = vel_cmd[1];
                    return rpc_driver_.write();
                } else {
                    return false;
                }
            });
    }

    ...
};

The first lambda function of register_mode is called whenever the mode manager’s read_from_world() function is called and the driver is in the corresponding control mode. The read operation itself is performed by the command adapter object:

return driver->command_adapter().read_from_world(world);

Indeed the command adapter knows read data it is capable to adapt, in the example kinematics data (position, velocity and acceleration).

Then when the mode manager process() function is called, the mode manager runs the adequate reactin depnding on currrent control mode of the driver. For instance in velocity control mode it runs:

if (driver->command_adapter().process() and
    driver->command_adapter().output().velocity) {
    auto& cmd =
        rpc_device_.command()
            .get_and_switch_to<
                rpc::dev::FlirPTUJointVelocityCommand>();
    const auto& vel_cmd =
        *driver->command_adapter().output().velocity;
    cmd.pan = vel_cmd[0];
    cmd.tilt = vel_cmd[1];
    return rpc_driver_.write();
} else {
    return false;
}

At that time the user defined lambda call the process() function of the command adapter that generates, if implemented, the corrent output for the driver. In the previous example code the command adapter must generate a velocity command (driver->command_adapter().output().velocity) that can be passed to the real driver.

Then the real driver is called, here it is performed in 2 steps because the real driver follows the RPC pattern:

  1. set the device related data.
  2. call write() function of the real driver configured with this device.

Asynchronous logging

With asynchronous drives logging can become quite tricky because we may want to log commands and state at the same period as the driver to avoid missing some commands applied to the robot. To do this it is suggested to implement a specific logger.

Here is the advised procedure to do so:

in root CMakeLists.txt add a dependency to the data logger:

PID_Dependency(data-juggler VERSION 0.5)

and in src/CMakeLists.txt use the data logger in the driver library:

PID_Component(
    flir-ptu-driver
    EXPORT
        robocop/core
        rpc/interfaces
        rpc/data-juggler #depends on the logger
    ...
)

In driver declaration, in file flir_ptu.h:

...
//add following line
#include <rpc/utils/data_juggler.h>
...
class FlirPTUAsyncDriver final
    : public rpc::Driver<robocop::FlirPTUDevice, rpc::AsynchronousIO>,
      public DriverCommandAdapter<AsyncKinematicCommandAdapterBase,
                                  FlirPTUAsyncDriver> {
public:
    ...
    //declare following function
    rpc::utils::DataLogger& create_async_command_logger(std::string_view path);
    ...
};
...

In driver implementation:

...
class FlirPTUAsyncDriver::pImpl {
public:
    ...
    //function implementation
    rpc::utils::DataLogger& create_async_command_logger(std::string_view path) {
        return cmd_logger_.emplace(path);
    }

    rpc::AsynchronousProcess::Status async() {
        if (mode_manager_.process()) {
            const auto ret = rpc_driver_.run_async_process();
            //logging the data
            if (ret == rpc::AsynchronousProcess::Status::DataUpdated and
                cmd_logger_) {
                cmd_logger_->log();
            }
            return ret;
        } else {
            return rpc::AsynchronousProcess::Status::Error;
        }
    }
private:
    ...
    //add the logger object attribute
    std::optional<rpc::utils::DataLogger> cmd_logger_;
};

//add implementation for publicly declared function
rpc::utils::DataLogger&
FlirPTUAsyncDriver::create_async_command_logger(std::string_view path) {
    //forward to implementation object
    return impl_->create_async_command_logger(path);
}

The idea is quite simple: providing a command logger object that can be configured from outside of the driver but capable of logging data that is only available inside the driver. The logged data depends on how the logger is configured BUT the moment is always the same: after command has been written in the async() function.

What data is logged depends on the application, for instance:

// This logger will be called asynchronously whenever a command has been
// sent to the device
auto& async_logger = driver.create_async_command_logger("/tmp");

// output().position is an optional<JointPosition> so we must provide a
// valid initial value (for configuration only)
async_logger.add("async joint position",
                    driver.command_adapter().output().position,
                    robocop::JointPosition::zero(ptu.dofs()));

The logger will write log files to the /tmp folder and log file contain joint positions generated as output of the command adapter.

Important Note: As you may have noticed the asynchronous command adapter is not a robocop::DataLogger but a rpc::utils::DataLogger (i.e. the direct real implementation class of the robocop data logger). Indeed data logged in the driver thread is not direcly issued from the robocop world so the robocop data loggr is of no help.