Overview of Codebase

Introduction

In this article, we will present a high-level overview of the ros2 control codebase, which involves two repositories: ros2_control and ros2_controllers. Collectively, we refer to these as the ros2 control library for the ease of discussion.

The ros2 control library consists of following major components:

• Control Node Executable: An executable responsible for launching the Controller Manager node

• Controller Manager: This is the central entity that manages the life cycle of controllers. It loads, unloads, starts, and stops controllers, and also handles their configuration.

• Resource Manager: This component manages resources like sensors and actuators, ensuring that they are properly allocated to controllers as needed.

• Hardware Interface: This component provides an abstraction over different hardware devices. It allows controllers to communicate with hardware without needing to know the specifics of the hardware.

• Controllers: Controllers are responsible for implementing specific control algorithms. Examples include joint trajectory controllers for arm movements, velocity controllers, position controllers, etc. They process input data and send command values to actuators.

In this article, we will guide you through each component and present the interaction betwteen them.

Control Node

The first component is control node. Strictly speaking, this is just an executable called ros2_control_node defined in ros2_control/controller_manager. The source file of this executable is ros2_control_node.cpp. The main responsibility of this executable is to launch a ControllerManager and start the main update loop. The snippets below shows the critical section of the script:

auto cm = std::make_shared<controller_manager::ControllerManager>(executor, manager_node_name);
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
// ...
while (rclcpp::ok())
{
    // calculate measured period
    auto const current_time = cm->now();
    auto const measured_period = current_time - previous_time;
    previous_time = current_time;

    // execute update loop
    cm->read(cm->now(), measured_period);
    cm->update(cm->now(), measured_period);
    cm->write(cm->now(), measured_period);

    // wait until we hit the end of the period
    next_iteration_time += period;
    std::this_thread::sleep_until(next_iteration_time);
}

This loop continuously reads states from hardware, updates commands, and then writes these commands to the hardware. The loop's frequency is set by the update rate parameter.

Controller Manager

From previous section, we established that the ControllerManager is a critical piece in the ros2 control library. Given the extensive size of the source code, it's impractical to cover every detail here. Instead, this section will focus on the code snippets of the read, update, and write functions.

void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period)
{
    resource_manager_->write(time, period);
}


void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
    resource_manager_->read(time, period);
}


controller_interface::return_type ControllerManager::update(
        const rclcpp::Time & time, const rclcpp::Duration & period)
{
    // ...
    for (auto loaded_controller : rt_controller_list)
    {
        if (is_controller_active(*loaded_controller.c))
        {
            const auto controller_update_rate = loaded_controller.c->get_update_rate();
            const auto controller_update_factor =
                (controller_update_rate == 0) || (controller_update_rate >= update_rate_)
                ? 1u
                : update_rate_ / controller_update_rate;

            bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0);

            if (controller_go)
            {
                auto controller_ret = loaded_controller.c->update(
                        time, (controller_update_factor != 1u)
                        ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
                        : period);

                if (controller_ret != controller_interface::return_type::OK)
                {
                    ret = controller_ret;
                }
            }
        }
    }

    // ...
    return ret;
}

We make the following observations based on the listed code above:

• The read and write operations are delegated to the ResourceManager

• ControllerManager has access to a list of controllers.

We will see in the later section that the ResourceManager is responsible for dealing with the hardware whereas the controllers are used to apply control theory algirhtms.

The following parameters are invovled in ResourceManager:

• activate_components_on_start: Determines whether components are activated at startup.

• configure_components_on_start: Specifies if components should be configured when starting.

• robot_description: Contains the description of the robot configuration.

• update_rate: Sets the frequency at which updates are made.

• use_sim_time: Indicates whether to use simulated time.

Resource Manager

The first thing we want to highlight is that ResourceManager is defined in ros2_control/hardware_interface, which means it's closed related to the actual hardware. Secondly, ResourceManager class consumes the URDF information as indicated by the constructor taking an urdf argument and the load_urdf function. Third, it has a field of type ResoureStorage.

The read and write functions delegate the operations to the components in ResourceStorage as indicated by the code below:

// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::read(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  std::lock_guard<std::recursive_mutex> guard(resources_lock_);
  read_write_status.ok = true;
  read_write_status.failed_hardware_names.clear();

  auto read_components = [&](auto & components)
  {
    for (auto & component : components)
    {
      if (component.read(time, period) != return_type::OK)
      {
        read_write_status.ok = false;
        read_write_status.failed_hardware_names.push_back(component.get_name());
        resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
      }
    }
  };

  read_components(resource_storage_->actuators_);
  read_components(resource_storage_->sensors_);
  read_components(resource_storage_->systems_);

  return read_write_status;
}

// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::write(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  std::lock_guard<std::recursive_mutex> guard(resources_lock_);
  read_write_status.ok = true;
  read_write_status.failed_hardware_names.clear();

  auto write_components = [&](auto & components)
  {
    for (auto & component : components)
    {
      if (component.write(time, period) != return_type::OK)
      {
        read_write_status.ok = false;
        read_write_status.failed_hardware_names.push_back(component.get_name());
        resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
      }
    }
  };

  write_components(resource_storage_->actuators_);
  write_components(resource_storage_->systems_);

  return read_write_status;
}

sensor, actuator, and system are hardware type, which is specified by the type attribute in the ros2_control element. As we can see, the sensor type hardware is read-only whereas actuator and system type hardware can do both read and write.

We also want to highlight the classic technique dependency inversion employed in the implementation. The ResourceStorage class depends on the System class, which in turn depends on the SystemInterface as it's used as one of the arguments in the constructor. The user-provided plugin class needs to implements the SystemInterface so that it will be picked up by ros2 control automatically. It's interesting to notice the SystemInterface itself extends the lifecycle node interface. This suggests the hardware control code is in the form of a lifecycle node.

Controller

Now, let's focus on the update operations performed by the ControllerManager. We've see earlier that these operations are delegated to the Controller class.

One of the key line in the update method of ControllerManager class is the one below:

loaded_controller.c->update(...)

Where the variable loaded_controller is of type ControllerSpec, which has the following definition:

struct ControllerSpec
{
  hardware_interface::ControllerInfo info;
  controller_interface::ControllerInterfaceBaseSharedPtr c;
};

This means the update action is provided in the ControllerInterfaceBase (see code).

This setup is similar to the system/actuator/sensor interface. The ControllerSpec depends on the ControllerInterface and the user can provide their own controller, which will implement the ControllerInterface. The ros2_controllers packages provides implementations of many well-known controllers. For more details, please refer to the Github repo of the ros2_controllers package.

Let's take a closer look at how the controller work. We will study the ForwardCommandController, presumably the most simple one. According to the comment, this controller just forwards the command to the interface. Our previous focus was on the update function in different classes, so let's take a look at the implementation of the update function in ForwardCommandController class. The complete code can be found at this link and here we only list the essential part of this function:

controller_interface::return_type ForwardControllersBase::update(
        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{

    auto joint_commands = rt_command_ptr_.readFromRT();

    // ...

    for (auto index = 0ul; index < command_interfaces_.size(); ++index)
    {
        command_interfaces_[index].set_value((*joint_commands)->data[index]);
    }

    return controller_interface::return_type::OK;
}

As we can see, the update function simply read the command value from rt_command_ptr and then set the value in the command_interfaces_. We are already familiar with what command interfaces do and the remaining questions are (1) what is rt_command_ptr variable and (2) where it gets from the data. The rt_comamnd_ptr can be considered a buffer and the value is provided by the subscription to the command topic:

controller_interface::CallbackReturn ForwardControllersBase::on_configure(
        const rclcpp_lifecycle::State & /*previous_state*/)
{
    auto ret = this->read_parameters();
    if (ret != controller_interface::CallbackReturn::SUCCESS) { return ret; }

    joints_command_subscriber_ = get_node()->create_subscription<CmdType>(
            "~/commands", rclcpp::SystemDefaultsQoS(),
            [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });

    RCLCPP_INFO(get_node()->get_logger(), "configure successful");
    return controller_interface::CallbackReturn::SUCCESS;
}

Summary

In this article, we examined many key components of the ros2 control library. The term interface seems overloaded and its meaning varies in different contexts. Therefore, when we come across the term "interface" in the code or documentation, it is important to understand what exactly it represents. It could refer to a system interface, a command interface, a state interface, or merely the general concept of an interface in software engineering.

The root component of the ros2 control library is the ros2 control node executable, responsible for launching the ControllerManager node. The ControllerManager is the core of the library, it has access to the list of controllers and the resource manager. The resource manager is responsible for loading the URDF file and load and initialize the hardware information accordingly. These hardware information is stored in the resource storage, which also has access to the the user-implemented hardware plugin. The hardware plugin behaves like a driver from ROS2's perspective. It can read data from the hardware or send command to it.

A hardware can have many components and each of them is represented by a HardwareComponentInfo class, which contains the command data and the hardware state. Command interface and state interface in the ComponentInfo class are accessible by controllers. Controllers apply control theory algorithm to determine the optimal commands that needed to be sent to the hardware component. They are the key components for the update step in the update loop. Their implementation involves subscribing to the command topic and updating the state (i.e. command interface and state interface) of the component.

The diagram below illustrates the overall architecture: