ROS2 Compatibility Layer

Motivation

In late 2021, we started working on a solution to gradually change the foundations of our robot soccer framework from B-Human 2013 to ROS2. The main idea was to use ROS2 to develop new features while keeping interoperability with the existing framwork. The motivation for doing this is twofold.

First of all, maintaining a large and complex custom framework takes time and resources which could be devoted to algorithm development. Using a battle-tested middleware maintained by a large community such as ROS2 should make that possible. Moreover, the inherently distributed nature of ROS2 should favor modularity and component based design, which usually translate into higher testability and maintainability. Integrating with existing simulators (Webots, RaiSim etc.) and tools (RViz, RQt) is also usually quite straightforward when using ROS2.

Secondly, one of the major issues that relatively small teams like ours face when recruiting new members is that students with little to no experience with software development are often not that motivated to learn how to use a large codebase based on a custom framework, like B-Human, which is not adopted outside of RoboCup SPL. On the other hand, getting familiar with ROS2, an industry-grade, thoroughly documented framework to develop robotics applications, is the kind of experience many students pursuing a career in robotics are likely to be interested in.

ROS-BHuman bridge architecture

The bridge is made up of three components:

• Streamable backed by ROS messages
• Representation streaming via ROS nodes.
• Process integration for pure ROS messages.

Streamable backed by ROS messages

Implementation: FieldWrapper with helpers from StreamableHelpers.

Underlying idea:

• Let BHuman representations extend from ROS messages
  • Declare the ROS messages for every representation with STREAMABLE_DECLARE defining a common base class that applies multi-inheritance to both Streamable class and optional the ROS message.
  • Use template magic to only apply the inheritance from the ROS message if it is defined by including the generated message file (note this template magic could be replaced with macros, as it was found that a separate STREAMABLE_ROS macro was necessary regardless).
• Replace data fields in representation created within auto streamable classes with references to the underlying data type.
  • Introduces the FIELD_WRAPPER (with and without specified defaults) macro that converts to the standard BHuman syntax (type) without ROS or FieldWrapperROS<Type> with ROS enabled (this cannot be done without the macro as ROS requires underscores for field names, but representations use camel case, and applying this transformations within the streamable macro would be insanely painful if at all possible)
  • Let FieldWrapperROS be a wrapper class that simulates a reference similar to the reference_wrapper (which is often used in containers that can otherwise not hold references as these are not reassignable).
  • The FieldWrapperROS class should act as it was the original type. Template magic is used to dispatch different types to different implementations of FieldWrapperROS.
    • For integral types, floating point types and std::string this is relatively simple, as the field can be directly mapped with a pointer in the ROS base class as it used the same type. Only assignment operators and user-defined conversions have to added to make this work.
    • For enums a mapping to a 8-bit integer has to be made as ROS messages do not support enums itself.
    • For static (c-style) sized arrays a mapping has to be made to a std::array as that is the type that ROS uses.
    • For vectors it gets more difficult as delegated functions need to be added to redirect calls to the underlying vector implementation.
    • Children that are representations themselves make it all much harder. Primarily because these fields should not link to the field defined in the base class, but to the children of the composite ROS node. Ensuring that this mapping is correctly preserved during (copy) construction and assignment is critical. This required a few fixes as it does not work with plain references to fields used in modules (custom conversions are not possible in this case).
    • Arrays and vectors of representations are also complex, as they need to work around the fact that the ROS message type is only the base class of a representation and not the representations itself and C++ do not support covariant containers. The wrapper use two containers where the representation container is mapped to the ROS message in the underlying ROS container. For vectors care has to be taken as these mapping could break when memory is reallocated (e.g. during push_back).
    • Finally custom ROS backed types are implemented for the custom streamables Vector2, Vector3, Pose2D, Pose3D, Matrix2x2 and Matrix3x3 as these are used as children fields, but not auto streamables.
• Streamables that are used as children in other representations need to use STREAMABLE_ROS to add necessary custom constructors that apply the correct mapping and field initialization (it adds two constructors that are called with an internal alias and init tag to implement the correct mapping and initialization of children fields to their backed types). Copy construction is implemented using assignment operator, as otherwise the reference cannot be mapped correctly before the assignment takes place (there caveats with this approach, but it should be safe here).

Representation streaming via ROS nodes

Implementation: ROS ModuleHelpers using representation helpers instantiated via ROSHelpers.

Underlying idea:

• With representations backed by ROS nodes, we could read/write to the blackboard using the ROS messages from ROS topics instead of the direct representations deserialized/serialized to memory-mapped streams when sending and receiving packages.
• The module manager has a pointer to the ROS node for the ROS process (e.g. Cognition) if ROS is enabled (nullptr otherwise) that is passed to the module when reading/writing representations to modules that provide/need them.
• After some magic eventually the logic invokes the in and out representation specific functions in the modules that serialize/deserialize the memory-mapped streams to the representations in the Blackboard for legacy modules, before calling the ROS-specific logic.
• For ROS it invokes the underlying helpers that automatically create publishers/subscribers (using a global lock) for the representation on the first invocation.
• Afterwards it publishes the representation base class ROS message on the topic (first making a copy of the representation as it can only write the base of that class, and the representation could have been mapped to a ROS message in another class), or it takes the latest message from the topic, if the topic has a messages, overwriting the previous representation data on success.

Process integration to publish/subscribe

Implementation: Modified ModuleManager

Underlying idea:

• To publish pure ROS messages externally to representations without a legacy module the global ExternalModule is used that can publish to every possible representation (but it doesn't publish anything in practice, as the external ROS node does that). It can be identified with the external keyword in modules.cfg (it is similar to the default constant which uses a global module that just always provides the default value). The 'external' module marks the shared representation as being provided externally to ensure the memory-mapped streams are not used.
• To ensure pure ROS messages can be subscribed without using legacy modules (with REQUIRES macros) they need to explicitly marked 'shared' in the module manager to ensure the read/write logic for that representation is invoked. This is automatically the case for modules published by modules in other processes (or by ExternalModule) and subscribed in the current one. Otherwise requirements can be added explicitly instead from any modern module.