ros2 组件 component C++节点编写，多线程实现与线程内通讯

ros2 组件 component C++节点编写，多线程实现与线程内通讯

注：环境

• ubuntu 22.04
• ros2 humble

感谢RM视觉救命群里各位佬们的帮助！

0. 你需要了解的

• ros2 的一些基本知识，如环境配置，编写简易的订阅和发布节点
• 进程和线程的基本概念

1. 简介

我们一般使用的ros2 一般是建立在独立的进程上的。一个Node 是以一个独立进程来运行的，因此进行通讯的时候难免会因进程之间的解码和编码导致通讯速率降低，因而在ROS2中，推荐使用组件（Component）来编写代码。我们也可以在ros2常见的插件或包中看到这种形式。

ros2的组件一般可以理解成通过一种通过动态链接二进制库的形式来执行节点的，因而便于实现进程内通讯，以节省通讯造成的编码解码开销。

每个节点都是一个进程

组件都共用一个进程，因而容易实现进程内通讯

2. 编写 .hpp 和 .cpp 文件

在编写 .hpp 与 .cpp 文件中，普通节点的区别和组件的区别在于是否由main 函数，因为组件是作为一个库被使用的，而不是一个独立的程序，因此组件不需要main函数作为启动点。你需要在Cmake 中指定你在程序中编写的，节点类（就是public 继承rclcpp::Node 的类）来便于ros2 挂载和启动组件。

为了系统学习并规范代码的书写，可以参考官方的例子进行学习：

考虑到入门的简洁性的需求，我自己完成了一个例子，实现了组件和进程内通讯（注意，该程序只适用于Linux 系统。作者系统为ubuntu 22.04，ros版本为 ros2 humble。

包目录如下

component_demo
├── CMakeLists.txt
├── include
│ └── component_demo
│ ├── component_demo_pub.hpp
│ └── component_demo_sub.hpp
├── launch
│ └── component_demo.launch.py
├── package.xml
└── src
├── component_demo_pub.cpp
└── component_demo_sub.cpp

component_demo_pub.hpp

#ifndef COMPONENT_DEMO__COMPONENT_DEMO_PUB_HPP_
#define COMPONENT_DEMO__COMPONENT_DEMO_PUB_HPP_

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

namespace ne
{
    class ComponentDemoPub : public rclcpp::Node
    {
    private:
        size_t count_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
        rclcpp::TimerBase::SharedPtr timer_;

    public:
        
        explicit ComponentDemoPub(const rclcpp::NodeOptions & options);
        ~ComponentDemoPub() = default;

    protected:
        void on_timer();
    };
}

#endif

component_demo_pub.cpp

#include "component_demo/component_demo_pub.hpp"

#include <chrono>
#include <iostream>
#include <memory>
#include <utility>

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

namespace ne
{
    ComponentDemoPub::ComponentDemoPub(const rclcpp::NodeOptions & options)
        : Node("component_demo_pub", options), count_(0)
    {
        
        pub_ = create_publisher<std_msgs::msg::String>("demo_component/topic", 10);

        timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]() {return this->on_timer();});
        
    }
    
    void ComponentDemoPub::on_timer()
    {
        auto msg = std::make_unique<std_msgs::msg::String>();
        msg->data = "Hello World:" + std::to_string(++count_);

        auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id());

        // 打印线程ID
        RCLCPP_INFO(this->get_logger(), "PubThID: '%s'", std::to_string(hashed).c_str());

        RCLCPP_INFO(this->get_logger(), "Pub: '%s'", msg->data.c_str());

        // 打印消息ID
        RCLCPP_INFO_STREAM(this->get_logger(), "Pub:Addr is:" << reinterpret_cast<std::uintptr_t>(msg.get()));

        std::flush(std::cout);

        pub_->publish(std::move(msg));
    }

}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(ne::ComponentDemoPub)

component_demo_sub.hpp

#ifndef COMPONENT_DEMO__COMPONENT_DEMO_SUB_HPP_
#define COMPONENT_DEMO__COMPONENT_DEMO_SUB_HPP_

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

namespace ne
{
    class ComponentDemoSub : public rclcpp::Node
    {
    private:
        size_t count_;
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;

    public:
        
        explicit ComponentDemoSub(const rclcpp::NodeOptions & options);
        ~ComponentDemoSub() = default;

    protected:
        void on_sub(std_msgs::msg::String::UniquePtr &msg);
    };
}

#endif

component_demo_sub.cpp

#include "component_demo/component_demo_sub.hpp"

#include <chrono>
#include <iostream>
#include <memory>
#include <utility>

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

namespace ne
{
    ComponentDemoSub::ComponentDemoSub(const rclcpp::NodeOptions & options)
        : Node("component_demo_sub", options), count_(0)
    {
        callback_group_sub_ = this->create_callback_group(
                rclcpp::CallbackGroupType::MutuallyExclusive);

        auto sub_option = rclcpp::SubscriptionOptions();
        sub_option.callback_group = callback_group_sub_;

        sub_ = this->create_subscription<std_msgs::msg::String>("demo_component/topic", 10,
            [this](std_msgs::msg::String::UniquePtr msg){this->on_sub(msg);}, sub_option);
        
    }
    
    void ComponentDemoSub::on_sub(std_msgs::msg::String::UniquePtr &msg)
    {
        auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id());

        // 打印线程ID
        RCLCPP_INFO(this->get_logger(), "SubThID: '%s'", std::to_string(hashed).c_str());
        
        RCLCPP_INFO(this->get_logger(), "Sub send:'%s'", msg->data.c_str());

        // 打印消息ID
        RCLCPP_INFO_STREAM(this->get_logger(), "Sub::Addr is: " << reinterpret_cast<std::uintptr_t>(msg.get()));
    }

}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(ne::ComponentDemoSub)

3. 编写 package.xml 文件 和 CmakeLists.txt 文件

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>component_demo</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="ziyedeyuu@163.com">ziyu</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>rclcpp_components</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CmakeList.txt

cmake_minimum_required(VERSION 3.8)
project(component_demo)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# find dependencies
find_package(ament_cmake_auto REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp_components REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
ament_auto_find_build_dependencies()

include_directories(include/)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# -----------------------------------------------------------------------------------------
# pub
# 生成库
ament_auto_add_library(component_demo_pub SHARED
        src/component_demo_pub.cpp
)
# 注册
rclcpp_components_register_node(component_demo_pub
        PLUGIN ne::ComponentDemoPub
        EXECUTABLE component_demo_pub_node
)

# 安装库文件
install(
  TARGETS
  component_demo_pub
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

# -----------------------------------------------------------------------------------------
# sub
# 生成库
ament_auto_add_library(component_demo_sub SHARED
  src/component_demo_sub.cpp
)

# 注册
rclcpp_components_register_nodes(component_demo_sub
        PLUGIN ne::ComponentDemoSub
        EXECUTABLE component_demo_sub_node)

# 安装库文件
install(
  TARGETS
  component_demo_sub
  EXPORT export_${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

#-----------------------------------------------------------------------------------------------

# 安装头文件目录
install(
  DIRECTORY include/
  DESTINATION include/
)

# 安装Launch 文件
install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}
)

ament_package

4. 组件的各种加载方式

ros2 的组件有很多种加载方式。我们可以使用命令行，使用 dlopen_composition 工具，使用launch 文件和使用硬编码的形式。

其中，由于使用命令行方式便于调试，使用 launch 文件方式方便启动。因此，这里主要介绍这两种方式。关于其他加载方式和更多详细内容，可以参考这篇博客：https://www.cnblogs.com/_bob/p/18165425

4.1. 使用 命令行 进行加载

注意：在一切使用命令行使用ros2的操作前，请务必 source。

ros2 component types // 列出所有可以加载的组件

使用上述指令，我们可以查看我们的组件是否编译和安装成功，也可以查看一会我们要用于加载的名字。

ros2 run rclcpp_components component_container // 启动组件容器

因为组件没有 main 函数，不能单独运行，因此需要启动一个空的进程用来作为组件执行的容器。利用这个指令来启动 ros2 提供给我们的默认的容器。

ros2 component load [包名] [组件名] // 该指令将组件挂在至默认的组件容器，注，执行此指令需新开一个命令行

使用上述指令，可以将你想要加载的组件挂载于默认的组件容器，随后，组件将会自动执行。

4.2. 使用 launch 文件进行加载

使用命令行方式虽然方便，但是不利于后续项目的一键启动。为了后续项目的方便，我们编写一个launch 文件来启动我们对的组件。

launch 文件的内容

import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    """Generate launch description with multiple components."""
    container = ComposableNodeContainer(
            name='container_component_demo',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='component_demo',
                    plugin='ne::ComponentDemoPub',
                    name='ComponentDemoPub',
                    extra_arguments=[{'use_intra_process_comms': True}] # 使用进程内通讯
                ),
                ComposableNode(
                    package='component_demo',
                    plugin='ne::ComponentDemoSub',
                    name='ComponentDemoSub',
                    extra_arguments=[{'use_intra_process_comms': True}] # 使用进程内通讯
                )
            ],
            output='both',
    )

    return launch.LaunchDescription([container])

于是，下次需要启动的时候，只需要 launch 此 launch 文件，即可自动启动组件容器并挂载多个组件。

5. 利用组件实现进程内通讯

关注 4 部分的 launch 文件，在 ComposableNode 中配置如下：

extra_arguments=[{'use_intra_process_comms': True}]

可以看见，我们发送消息的内存地址与接受消息的内存地址相同，达到了我们的目的。然而我们发现，发布者组件和接受者组件的线程 ID 也相同，这说明两个节点是以单线程运行的。

6. 实现组件的多线程

一般来说，单独进程运行的节点，一个节点就是一个线程，这样的多进程（工作和多线程类似）方式可以让程序高效的运行。然而编写组件挂载后我们发现，所有节点的线程号是唯一的，也就是他们是单线程运行的，如何做到多线程运行呢。

首先参考 ros2 的官方教程：https://docs.ros.org/en/humble/Concepts/Intermediate/About-Executors.html 关于 Executors 的描述，我们了解到，ros2 的回调函数和节点管理是通过 Executors 实现的，其中 Executors 有三种类型：

因此我们需要我们的组件容器能够将我们的组件以每个组件独立线程的形式加载进入，而不是所有组件公用一个线程。（注：不是使用 MultiThreadedExecutor 而是 SingleThreadedExecutor，因为我们不希望一个消息处理的回调函数可以在多个线程中同时运行，因为那会导致时序难以控制）

我们可以进入 rclcpp 的 github 查看组件容器的具体实现：https://github.com/ros2/rclcpp/tree/9b1e6c9d520f09338b789058571c63d283a5c128/rclcpp_components/src

我们发现，rclcpp 给我门提供了不同类型的三种组件容器

查看component_container_isolated.cpp

翻译：为每个组件提供专用的单线程执行器组件容器。我们选用这个组件容器。

我们可以将 launch 文件修改为

import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    """Generate launch description with multiple components."""
    container = ComposableNodeContainer(
            name='container_component_demo',
            namespace='',
            package='rclcpp_components',
            
            #在这里使用 component_container_isolated 作为容器
            executable='component_container_isolated', 
            
            composable_node_descriptions=[
                ComposableNode(
                    package='component_demo',
                    plugin='ne::ComponentDemoPub',
                    name='ComponentDemoPub',
                    extra_arguments=[{'use_intra_process_comms': True}]
                ),
                ComposableNode(
                    package='component_demo',
                    plugin='ne::ComponentDemoSub',
                    name='ComponentDemoSub',
                    extra_arguments=[{'use_intra_process_comms': True}]
                )
            ],
            output='both',
    )

    return launch.LaunchDescription([container])

修改代码，构建编译运行，我们能够看到，发布者组件和订阅者组件的线程 ID 不同，但却在每次发布和订阅回调函数中保持不变，达到了我们的目的。（注：测试使用component_container_mt.cpp 发现，消息发送和接收的内存地址不同，因此不使用）