UP | HOME

ROS C++ Details

Overview

These notes discuss C++, as it appears in the ROS 2 Cpp publisher example below.

// From: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html#write-the-publisher-node0
// Accessed on: 1/17/2023
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a */
/* member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node
{
  public:
    MinimalPublisher()
    : Node("minimal_publisher"), count_(0)
    {
      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
      timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }

  private:
    void timer_callback()
    {
      auto message = std_msgs::msg::String();
      message.data = "Hello, world! " + std::to_string(count_++);
      RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      publisher_->publish(message);
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}

Includes

  • The .cpp file uses the pre-processor directive to #include several header files.
  • The pre-processor searches for these files on the include path and textually replaces them (i.e., copies and pastes the text directly), creating an intermediate file.
  • Files that are include with <> are searched for on the standard include path (which is provided by default).

    #include <chrono>      // timing-related functions
    #include <functional>  // functional programming
    #include <memory>      // smart pointers
    #include <string>      // strings
    
  • Other include files are for libraries (including other ROS and non-standard system libraries)

    #include "rclcpp/rclcpp.hpp"
    #include "std_msgs/msg/string.hpp"
    
  • These files, unless they are installed to a standard location on the system (i.e., /usr/include) must be explicitly added to the include path
  • When you target_link_libraries(mytarget alibrary) in CMakeLists.txt, the include directories necessary for mytarget to use alibrary are automatically added to the include path
  • If your code #include=s a header from a library, that library is a =build_depend dependency, and likely a build_export_depend, so it makes sense to add a depend in the package.xml
    • rosdep can be used to automatically fetch dependencies
    • Only dependencies that are registered with ros can be fetched with rosdep, but anybody can register a package (though there are humans who review such additions).

Understanding Standard Headers

  • The ultimate word on what a header does and how to use it its specification in the C++ standard.
  • However, https://cppreference.com tends to have accurate documentation about C++
  • chrono provides documentation on the <chrono> headerfile, for example.
Preprocessor
  • colcon build --cmake-args -DCMAKE_CXX_FLAGS=-save-temps will cause all intermediate files generated during the compilation process to be saved in the build directory
  • The files, for a given ${package} and ${CMakeTarget} (e.g., node or library) will be located in build/${package}/CMakeFiles/${CMakeTarget}.dir/src
  • Files that have been pre-processed but not compiled have a .ii extension

Macros

  • RCLCPP_INFO is a macro, which means the preprocessor makes a literal text replacement
  • RCLCPP_INFO_STREAM allows the use of C++ iostream style output rather than C style printf output
    • C++ output is generally preferred as it is type-safe and also can be extended more easily to custom types

Namespaces

  • The first encounter with a namespace is using namespace std::chrono_literals
  • Generally the form of using namspace X is discouraged, however, an exception is a namespace containing literal suffixes
  • Literal suffixes are a C++11 feature that enable you to write a custom literal suffix operator that converts a literal into a given type
    • Examples of built-in suffixes are f (for float) and u for unsigned (so 1f is a floating point number and 1u is an unsigned integer)
  • std::chrono_literals allows using some intuitive suffixes to specify time, such as
    • 11ms is 11 milliseconds and 12h is 12 hours.
  • C++ namespaces are used to prevent issues related to multiple projects having the same name for things (and thus making them incompatible with each other
  • Everything in C++ is in a namespace. The global namespace is blank and automatically visible, but it can be explicitly invoked using ::item, where item is something defined in the given namespace
  • To reference an item in a namespace (e.g., ns) you can provide the fully-qualified name ns::item
  • It is also possible to make a single item from a namespace visible with using ns::item, which makes item visible in the scope that using ns::item is called in.
    • For example if placed at the top of a file (and outside any function) item will be visible anywhere below the using ns::item
    • If used at the top of a function item will be visible within that function. More generally, if used within {}, item will be visible anywhere below the using ns::item that is also within the nearest block of {}
  • It is possible (but you should not do this) to make all items in a namespace visible by doing using namespace ns.
    • However, doing this can introduce name conflicts and unexpected behavior as it effectively overrides the purpose of namespaces

Argument Dependent Lookup

  • Argument Dependent Lookup is a set of rules that helps determine what namespaces are searched when calling a function
  • The namespaces that are searched are based on the function's arguments (hence Argument Dependent)
  • The rules are a bit tricky (especially in corner cases) but basically if an argument to a function is in a namespace (e.g. ns) than that namespace will be searched for functions
  • The upshot is that you do not need to explicitly bring in namespaces when calling functions on objects that are in that namespace. Instead they will be automatically found with ADL

Declaring and Defining in a Namespace

  • To make an item (e.g., a function or a class) an element of a namespace, it must be declared within that namespace
  • With namespace myns { }, anything inside the {} will be within the namespace
  • Namespaces may be nested
  • An anonymous (e.g., namespace {}), which means only items in the same source file can access the items in that namespace
    • Essentially, an anonymous namespace is like private on a source-file rather than class level.

Class Definition

  • The class MinimalPublisher : public rclcpp::Node is declared.
  • The MinimalPublisher class is a subclass (or child) of the rclcpp::Node class
  • Subclassing the rclcpp::Node class is the "preferred" method of creating ROS2 nodes in C++
    • The reason for this preference is explained in Node Composition and Managed Nodes are two other types of ROS nodes in C++ and these nodes depend upon sub-classing
    • Composable nodes can be loaded into the same process at runtime, thus reducing the overhead of any messages/services called between them
    • Managed Nodes (also called lifecycle nodes) enable a user to control when a node starts, shuts down and other events in the lifetime of a node
  • The canonical documentation for rclcpp is located in the API docs

Inheritance in C++

  • In C++, a class can inherit from one or more classes
  • The derived class takes on the methods and members of the base classes and can access its public and protected members
  • When constructing an object of a class that is derived from other classes, the base classes must be constructed first,
    • The derived class can pass parameters to the base classes using the constructor initializer list
  • Inheritance in C++ can be public, private, or protected (though protected should generally be avoided and is rarely used)
    • In public inheritance, the derived class "is-a" base class. That means it can be used anywhere the base class can be used
      • All non-private members of the base class are public in the derived class
      • The private members of the base class are inaccessible from the derived class
    • With private inheritance, the derived class is "implemented-in-terms-of" the base class
      • The public and protected members of the base class become private in the derived class
      • The private members of the base class are inaccessible from the derived class
      • Essentially, this is very similar to using composition (so instead of privately deriving from base the "derived" class has a base object as a member.
    • Generally, prefer composition to inheritance to reduce coupling between classes
  • You should only derive from a class if one of the following conditions is met CPP Core Guidelines C.35
    • The destructor is public and virtual
    • The destructor is protected and non-virtual
    • If the creator of the class did not declare a destructor according to these guidelines you cannot safely derive from the class
    • To prevent somebody from deriving from a class it can be declared final
    • If you look at the rclcpp::Node() class, you will se that it has a virtual destructor.
Virtual Methods
  • In C++ derived classes can override members of the base class
    • Except in extremely rare circumstances, any method that can be overridden should be declared virtual
    • virtual methods ensure that, when a reference to the base class refers to a derived class, the derived class's methods are called
    • the override keyword should be used when a method you are writing is intended to override a function in the base class (since C++11)
    • If a virtual method is declared like ~virtual return_type method() = 0!, then this creates an abstract base class
      • The function is declared but not defined in the base class. Instead the base class is providing an interface that must be implemented in a derived class
  • If a class has any virtual function, it uses a vtable to call its methods
    • The vtable adds a layer of indirection to calling class methods, but also enables derived classes to override base class methods
    • The reason for having a virtual keyword is so that classes that do not participate in inheritance do not pay this performance penalty
  • Object Slicing:
    • A derived class can be used anywhere the base class is accepted
    • This feature allows writing code that uses the interface defined on a base class, but having different behaviors depending on what derived class is actually used and is called runtime or dynamic polymorphism
    • Generally, you only want to access derived classes through a base class reference (or pointer) rather than by value
      • When you assign a derived class to a base class (by value) object slicing occurs, meaning that the parts of the object that belong to the derived class are lost, which leads to lots of confusing behavior.

Constructor

Constructor Initializer List

  • Prior to the body of the constructor being called, the class is constructed as follows
    1. It's base class(es) are constructed
    2. It's members are constructed, in the order in which they are specified in the class
  • By placing a : after the constructor, the initialization process can be controlled by passing values to any base class constructors and the members
  • Members can also be provided a default value by assigning a value to them when they are declared.

This pointer

  • Every member function implicitly has a pointer to the current object, which is called this
  • this holds the memory address of the object being called
  • *this is the object being called (the pointer is being dereferenced)
  • this-> can be used to refer specifically to members of the class, however the this-> is, in most cases, implied, therefore there is no need (and you should not) preface calls to member functions or class members with this-> in most code (as is done in the example

RAII

  • RAII stands for "Resource Acquisition is Initialization", and this is the primary tool used for memory and exception safety in C++
  • What this means is that constructors acquire resources and destructors release them
  • RAII is very important in the face of exceptions
    • Imagine you need to allocate 3 resources, but the allocation may throw an exception
    • If the second allocation throws an exception you need to free the 1st resource
    • If the third allocatin throws you need to free the 1st and 2nd resource
    • By freeing resources in destructors this happens automatically, otherwise you would need to handle each of the above cases manually, which can lead to a lot of error handling code
  • std::vector<> uses RAII
    • The vector class allocates memory in its constructor, and it is automatically freed when the vector is destructed

Resources in C++

A resource is any entity that needs to be requested by the programmer from the operating system before it can be used. Here are some examples

Memory

In C++ there are two main areas of memory:

  1. The stack
    • This is temporary storage for items like local variables
    • When a function is entered, memory is allocated on the stack
    • When a function returns (or throws an exception) memory is deallocated from the stack
    • Allocating and deallocating memory from the stack is a quick operation, typically requiring just adding-to or subtracting-from the stack pointer
    • Before an object is deallocated from the stack, C++ automatically calls its destructor
  2. The heap
    • Global variables and other variables that must persist between function calls are stored on the heap
    • To use the heap, the programmer must explicitly allocate memory, causing the operating system to find memory that the program can use
    • When the program is done with the memory, the programmer must manually deallocate the memory so it can be used by others.

Other Resources

  • Files
  • Network sockets
  • Database connections

Motivating RAII

Consider the following code, which allocates some memory, does some "stuff", and frees the memory:

void myfunc()
{
    auto * x = new int(10);
    auto * y = new double[50];
    auto * myobj = new SomeObject();

    do_stuff();

    delete x;
    delete [] y;
    delete myobj;
}

The code seemingly works and remembers to free all the memory. However, there are several issues

  1. The SomeObject() constructor, any of the memory allocations, or do_stuff might throw an exception. This would cause the code that deletes x and y to be skipped

Perhaps, in this case, the risk of throwing can be skipped

void myfunc()
{
    int * x = nullptr;
    int * y = nullptr;
    SomeObject * myobj = nullptr;
    try
    {
        x = new int(10);
        y = new double[50];
        myobj = new SomeObject();

        do_stuff();
        delete x;
        delete y;
        delete myobj;
    }
    catch(...)
    {
        if(x)
        {
            delete x;
        }
        if(y)
        {
            delete [] y;
        }
        if(myobj)
        {
            delete myobj;
        }
    }
}

Here, if there is an exception, we catch it and make sure to call delete. However, we need to make sure to only call delete on objects that were previously constructed, which adds additional complexity.

RAII allows us to avoid writing this boiler-plate code by being sure to always

  1. Allocate resources in constructors
  2. Deallocate resources in destructors

If every object follows the RAII rule then, whenever an exception is thrown, the destructors of all the objects within scope that had already been constructed will be called automatically. If an object hadn't yet been constructed, its destructor would not be called

Smart Pointers

  • Smart pointers are used to track and automatically free memory
  • There are two main smart pointer types in C++
    • std::unique_ptr<T>, which allows one "owner" of the memory
      • What this means is that there is only ever 1 valid unique_ptr pointing to memory at any given time
      • Anytime a unique_ptr is "copied", the source pointer is invalidated and using it again results in an error
      • When the unique_ptr goes out of scope, the memory is freed
    • std::shared_ptr<T>, which allows multiple shared "owners" of the memory
      • The shared_ptr is reference counted, meaning that every time a shared_ptr is copied, it's reference count is increased
      • When a shared_ptr is destructed, the reference count is decremented. If the reference count is 0 (meaning all =shared_ptr=s referring to given memory were deallocated) the memory is freed
  • Shared pointers enable automatic memory management and work as long as their are no circular reference (A -> B and B -> A). Circular references can be broken with weak_ptr

Example

Here is one way in which smart pointers can help eliminate redundant error-handling code

void myfunc()
{
    auto x = std::make_unique<int>(10);
    auto y = std::make_unique<int[]>(50);
    auto myobj = std::make_unique<SomeObject>();
    do_stuff()
}

if any exceptions are thrown, because the memory is allocated in each individual object's constructor and deallocated in each individual destructor, there will be no memory leaks or double deallocation.

Argument Passing Semantics

  • Pass by value (void dostuff(int x)), when calling dostuff(y) the value of y is copied into x.
    • Changes made to x within dostuff() happen in the copy and do not modify y
    • This type of argument is an [in] argument as it is an input to a function
  • Pass by reference (void dostuff(int & x)), when calling dostuff(y), x is a reference to y
    • No copy is made (which is good for a large datastructure)
    • Changes to x within dostuff() are reflected in x
    • This can be either an [in] argument (if dostuff() reads from x) and [out] argument (if dostuff() writes to x) or both
  • Pass by reference to const (void dostuff(const int & x)) or equivalently void dostuff(int const & x)
    • This method is the same as pass by reference but dostuff cannot modify x, so it is always an [in] value
    • This method is the preferred way of passing large objects to functions
  • Move (void dostuff(std::vector<int> && x))
    • This method requires that dostuff is called with an lvalue reference (basically some value that cannot appear on the left-hand side of an assignment (=) expression
    • dostuff is allowed to modify and destroy x, since it can never be referenced again
    • For example dostuff(std::vector<int>(10)) creates a temporary vector which, since it is temporary can't be referenced again. Therefore its memory, instead of being deallocated, can be reused within dostuff

Smart Pointers

  • If you pass something by smart pointer, you are making a statement about ownership
    • Passing a shared_ptr to a function says that the function is being given ownership of the pointer.
      • Therefore it is allowed to store that pointer somewhere (say as a member variable in a class or a global variable) that persists after the end of the function call.
      • The memory will only be deallocated when it stops being used
    • Passing a unique_ptr to a function says that the function is being transferred ownership
      • The original unique_ptr is no longer valid because the function now owns it and each unique_ptr can have only one owner.
  • Generally, a function does not store its arguments, it just does a calculation on them and returns.
    • In this case, you would usually want to pass by const reference, and do not need to transfer ownership.

Constness

  • In C++ variables and class methods can be marked const
  • If a method does not modify any class members then it should generally be marked const
    • const methods can be invoked on const objects (whereas non-const methods cannot)
  • const correctness is enforced by the compiler. It is helpful because when functions can't change values you can rely on them being the same (which makes debugging easier).

Author: Matthew Elwin