C++ ROS Node Structure

Overview

A generic guide and collection of design patterns for writing C++ ROS nodes and some C++ philosophy.
These are not hard-fast rules, rather they are designed to get you thinking in terms of design

Classes and Objects

A class is a description of data and the functions that operate on it
An object is an instance of a class: it is a specific piece of memory containing values corresponding to the members of the class
Object-oriented programming is a programming language design paradigm where classes and objects are the main method of abstraction
C++ has many object-oriented features, but it is a multi-paradigm language and also contains generic, functional, procedural elements
C++ is a vast language and very few people actually use all its features in a single program. One purpose of this document is to highlight features that are useful for robotics

Why Use Classes

Hide the state of the system and divide it into manageable pieces
- Private class members are inaccessible outside a class, so code outside the class is completely decoupled from them.
- Public members control how non-class members interact with the class
- Classes enforce invariants or constraints on the state of your program
  - Ideally, once a class is created, its state should always be valid and its behavior well-defined
  - Prefer const members functions to non-const because const member functions cannot accidentally invalidate a class
- Example of an invariant:
  - Here is a class representing a 2-D unit vector
```
class UnitVector2D
{
  public:
      UnitVector2D(double x, double y);

      void rotate(double radians);

  private:
      double x, y;
}
```
  - For the class to successfully enforce the invariant, the constructor and any non-const member functions must not allow objects to be created that violate the unit vector property.
  - To enforce the invariant, the constructor can:
    1. Throw an exception if x,y are not components of a unit vector
    2. The public member functions behave as if x,y were normalized
      - How this is done is an implementation detail, either the constructor normalizes x,y first, or each member function normalize x,y as needed
  - The member function rotate can never violate the unit vector invariant by its nature
  - It is easy to conceive of a design for this class that lets you violate the invariant, so it is up to you to make sure this does not happen.
A class defines a new type in the C++ language
- types are descriptions of variables
- Without specific conversion operations/type casting, one datatype cannot be used where another one is required (e.g., passing a parameter)
- The C++ compiler knows the type of each variable and can use this information to help you program
- Since C++11 the auto keyword enables the compiler to infer the type of a variable:
```
vector<double> some_fun() {...}
auto xvec = some_fun();

// let's throw in a c++11 style foreach loop
for(auto d : xvec)
{
}
```
  - See Almost Always Auto
  - Types can be used to get the compiler to enforce invariants at compile time
    - For example, in an extreme case, you could define separate classes for inches and gallons.
    - With operator overloading, they would behave like regular floats
    - But, with proper design, you could never accidently add inches and gallons as that is a non-sensical quantity This is due to types being enforced by the compiler

ROS Node Design

Most ROS nodes are long-running programs that communicate externally
They typically have publishers, subscribers, call-back functions and read parameters
Sometimes these callbacks need to share data with each other
By default, ROS C++ nodes do not create any threads
- This means that callbacks are executed sequentially
- The order in which the callbacks are executed is undefined however, one callback will always complete before another which means there are no data synchronization issues
- There are many good reasons to create multi-threaded C++ code; however, when using ROS it usually easier if code must run in parallel to create separate nodes
These designs are not mutually exclusive; techniques can be effectively combined as needed

Object-Oriented ROS Node

The preferred method because it is the most compatible with Node Composition and Managed Nodes
Callbacks are class members
Main creates the object, then spins
Work can be done purely in callbacks or from the main loop calling a member of the class

Example

#include"ros/ros.h"
#include"sensor_msgs/JointState.h"

class MyNode
{
    public:
        MyNode():
            nh{},
            pub(nh.advertise<sensor_msgs::JointState>("js", 5)),
            sub(nh.subscribe("topic", 1000, &MyNode::callback, this)),
            timer(nh.createTimer(ros::Duration(0.1), &MyNode::main_loop, this))
         {
         }

         void callback(const sensor_msgs::JointState & js) const
         {
         }

         void main_loop(const ros::TimerEvent &) const
         {
            // implement the state machine here
             pub.publish(sensor_msgs::JointState{});
         }

    private:
        ros::NodeHandle nh;
        ros::Publisher pub;
        ros::Subscriber sub;
        ros::Timer timer;
};

int main(int argc, char * argv[])
{
    ros::init(argc, argv, "nodename");
    MyNode node;
    ros::spin();
    return 0;
}

Advantages

Node class can be re-used in other ROS nodes, or more easily ported to nodelets
Whatever you make private in the class cannot be used outside the class
You can even make multiple classes with just the callbacks that share data, although this can indicate that you might want separate nodes
If the callback is not supposed to modify data it can be delcared const

Disadvantages

Introduces an extra class that in many cases is unnecessary
In some instances you may need to think carefully about class initialization rules in the constructor
Private class data is still global to everything that is in the class
Encourages the coupling of logic directly to ROS constructs (although you can still keep it separate if you are careful)

Procedural ROS Node

Data is shared between callbacks using static global variables
- These variables can only be seen within the compilation module
Most processing is done in the "main loop"
- Can be a while loop in main, using ros::Rate to run at a fixed frequency
- Can also be a timer callback
- A useful pattern for the main loop is a finite state machine
Callbacks do minimal processing
- Save data and signal the main loop
- Can cause state transitions or generate "events" that cause the main loop to transition

Example

Here is an example of this basic structure of a ROS node (pseudo-C++)

/// Static variables used by callbacks here

enum class State {STOP, GO, END};

static State state = State::STOP;

void stop_callback(const MessageType & msg)
{
    // process message.  Maybe messages of this type
    // mean its supposed to go into STOP mode
    state = State::STOP;
}

int main(int argc, char * argv[])
{
    ros::init(argc, argv, "node_name");
    auto node = std::make_shared<Node>("node_name");
    // read parameters, create publishers/subscribers
    const auto sub = nh.subscribe("stop", 1000, stop_callback);

    ros::Rate r(frequency)
    while(ros::ok())
    {
        switch(s)
        {   
            case State::STOP:
                // do stop state stuff
                break;
            case State::GO:
                // do go state stuff
                break;
            case State::END:
                // do end state stuff
                break;
            default:
                // should never get here
                throw std::logic_error("Invalid State");
        }
        ros::spinOnce();
        r.sleep()
    }
    return 0;   
}

Advantages

Keeps most of the logic in one place (the main loop) since most work does not happen in callbacks
The single state machine makes relationship between logic and state explicit rather than implicitly existing in many variables
Relatively simple structure
Simplifies decoupling the logic from ROS specific code.

Disadvantages

The state machine structure may be overkill for very simple cases
The global variables may get messy for complicated cases
- Using objects to organize the global variables can help
You cannot easily re-use the ROS-specific parts of the code in another node

Lambda ROS Node

Callbacks are written as lambda functions
Allows direct specification of which variables each callback can see and modify
Requires modern C++ standard, and has some convoluted syntax
Not the way that most ROS nodes are currently written

int main(int argc, char * argv[])
{
   ros::init(argc, argv, "name");
   NodeHandle nh;
   const auto pub =  nh.advertise<sensor_msgs::JointState>("js", 5));
   int a = 2, b = 4;

    // first template argument is message type, second argument is the type that the callback takes
   const auto sub = nh.subscribe<sensor_msgs::JointState, const sensor_msgs::JointState &>(
           "topic", 1000, [&a,b](const auto & x)
            {
                // I can modify a, read a copy of B can't see pub
            });
    // the rest of the program
}

Similarly you can use a "functor object", that is an object that defines operator(), which is the callback
A lambda function is essentially creates such an object behind the scenes

Reactive ROS Node

A reactive ROS node is where a subscriber directly publishes a messages from within the callback.

Any of the above methods can be made reactive by publishing from the subscriber instead of (or in addition to) the main loop.
These nodes are useful when the purpose of the node is to convert one message into another, or when you want the node that is publishing messages to set the frequency
This node should almost always ros::spin() rather than having its own loop with a delay. The ros::spin() will ensure that messages are constantly processed and callbacks happen immediately.

Spin vs SpinOnce

When publishing messages at a fixed rate, the differences between calling ros::spin() from a timer versus looping, sleeping, and calling ros::spinOnce() are usually inconsequential
For reactive nodes (where subscriber callbacks publish messages) ros::spin() is preferred because it constantly checks the message queue and issues callbacks
- ros::spinOnce() when called from a loop that sleeps according to ros::Rate, will only check for callbacks once per iteration. Potentially, during the sleeping time, a callback could arrive, and it will not be handled until the next iteration
Timers are also preferred to a custom while loop with ros::spinOnce() if you have multiple tasks that need to occur at separate frequencies
- Although often it makes sense to divide these tasks into different nodes

Resources

See Publishers and Subscribers for C++ specific details of publishers and subscribers
See Callbacks And Spinning for more details about the thread model