多节点单进程

ROS通信是基于Node(节点)的，Node使用方便、易于扩展，可以满足ROS中大多数应用场景，但是也存在一些局限性，由于一个Node启动之后独占一根进程，不同Node之间数据交互其实是不同进程之间的数据交互，当传输类似于图片、点云的大容量数据时，会出现延时与阻塞的情况。

在ROS1中，nodelet软件包旨在提供在同一进程中运行多个算法(节点)的方式，不同算法之间通过传递指向数据的指针来代替了数据本身的传输(类似于编程传值与传址的区别)，从而实现零成本的数据拷贝。在ROS2中，nodelet升级为更强大的Components。

nodelet和Components都是基于class_loader实现的。nodelet是对pluginlib的进一步封装，Components本质是有ROS2 接口的plugins。

Nodelet

nodelet能够实现以下功能：

• 不同算法被封装进插件类，可以像单独的节点一样运行；
• 在该功能包中提供插件类实现的基类nodelet:Nodelet，它将用于动态加载。所有的节点都将继承这个基类，并且可以使用pluginlib动态加载
• 它将自动提供命名空间、重新映射arguments和parameters，就像它们是一个节点一样
• 提供nodelet_manager进程，可以使用加载插件类的类加载器nodelet:NodeletLoader加载一个或多个节点。它们之间的任何通信都可以使用boost共享指针实现零拷贝roscpp发布调用，如果希望no-copy pub/sub工作，必须将消息发布为shared_ptr。

ros::Publisher pub = nh.advertise<std_msgs::String>("topic_name", 5);
std_msgs::StringPtr str(new std_msgs::String);
str->data = "hello world";
pub.publish(str);
1
2
3
4

注意，当用这种方式发布，你和roscpp之间的隐性契约：你不可以修改你发送后的消息，因为指针会直接传递到用户的任何进程内。如果你想发送另一条消息，你必须分配一个新的。

bug清单

• nodelet派生类的onInit()函数中使用std::shared_ptr而非boost::shared_ptr导致

  [FATAL] [1673255629.474497531]: Failed to load nodelet '/fastlio_plugin_loader` of type `fast_lio/fastlio_plugin` to manager `nodelet_manager'
  [nodelet_manager-1] process has died [pid 1170639, exit code -6, cmd /opt/ros/noetic/lib/nodelet/nodelet manager __name:=nodelet_manager __log:=/home/long/.ros/log/1496afea-8ffa-11ed-913a-07d0641c7442/nodelet_manager-1.log].
  1
  2

• plugins.xml中没有加name属性导致

  [ERROR] [1673261540.209231901]: Failed to load nodelet [/fastlio_plugin_loader] of type [fast_lio/fastlio_plugin] even after refreshing the cache: According to the loaded plugin descriptions the class fast_lio/fastlio_plugin with base class type nodelet::Nodelet does not exist.
  1

命令行工具

# Launch a nodelet manager node
nodelet manager               
# Launch a nodelet of type pkg/Type on m_name manager
nodelet load pkg/Type m_name 
# Launch a nodelet of type pkg/Type in a standalone node
nodelet standalone pkg/Type
# Unload a nodelet a nodelet by name from manager
nodelet unload name manager   
1
2
3
4
5
6
7
8

nodelet调用案例

rosrun nodelet nodelet manager __name:=mymanager
1

__name:= 用于设置管理器名称。

添加第一个节点

rosrun nodelet nodelet load nodelet_tutorial_math/Plus mymanager __name:=n1 _value:=100
1

• nodelet 的节点名称是: /n1

• 添加的参数名称是: /n1/value。

添加第二个节点

rosrun nodelet nodelet load nodelet_tutorial_math/Plus mymanager __name:=n2 _value:=-50 /n2/in:=/n1/out
1

• /n2/in 重映射为 /n1/out

将独立节点改造为nodelet节点

源代码编写

原始节点程序改造

原始独立节点的main函数在main.cpp文件中，将其main函数更改名称(除main外的任意名称)，比如mainFunction，同时还需新建头文件main.h，main.h中声明mainFunction函数

//main.h
#ifndef MAIN_H
#define MAIN_H
int mainFunction();
#endif 
//main.h
1
2
3
4
5
6

//main.cpp
#define NODELET //切换独立节点与Nodelet节点的宏定义
#ifdef NODELET
#include "main.h"
#endif

#endif

#ifdef NODELET
int mainFunction()
{
    int argc; char** argv;
    ros::init(argc, argv, "laserMapping");
#else
int main(int argc, char** argv)
{
    ros::init(argc, argv, "laserMapping");
#endif
//main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

插件类编写

新建文件plugin.h与plugin.cpp

//plugin.h
#ifndef PLUGIN_H
#define PLUGIN_H
#include  #继承Nodelet基类
namespace custom
{
	class Plugin: public nodelet::Nodelet
	{
	public:
	  Plugin();
	  virtual void onInit();
	};
}
#endif 
//plugin.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

//plugin.cpp
#include "plugin.h"
#include "main.h"
#include 
#include 
namespace custom
{
	Plugin::Plugin(){
	}
	
	void Plugin::onInit(){
	  NODELET_INFO("plugin - %s", __FUNCTION__);
	  boost::shared_ptr<boost::thread> spinThread;
	  spinThread = boost::make_shared<boost::thread>(&mainFunction);
	}
}
PLUGINLIB_EXPORT_CLASS(custom::Plugin,nodelet::Nodelet);
//plugin.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

编译插件库

在CMakeLists.txt文件中添加库

# CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(sample_nodelet)
 
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  nodelet
)
 
catkin_package(
 INCLUDE_DIRS include
 LIBRARIES ${PROJECT_NAME}
 CATKIN_DEPENDS nodelet roscpp rospy
 # DEPENDS system_lib
)
 
include_directories(
    include
    ${catkin_INCLUDE_DIRS}
)

add_library(main_function src/main.cpp)
target_link_libraries(main_funtion ${catkin_LIBRARIES})

add_library(custom_plugin
  src/plugin.cpp
)
target_link_libraries(custom_plugin main_function ${catkin_LIBRARIES})
if(catkin_EXPORTED_LIBRARIES)
  add_dependencies(custom_plugin ${catkin_EXPORTED_LIBRARIES})
endif()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

插件描述文件

插件描述文件是存储插件所有重要信息的机器可读文件，包含插件所在库的名字，插件的名字，插件的类型及其继承的基类名。

ROS1中功能包生成的库保存在devel/lib目录下，ROS2中功能包生存的库保存在install//lib目录下。

创建plugins.xml文件

<class_libraries>
	<library path="lib/libcustom_plugin">
	  
	  <class name="custom/Plugin" type="custom::Plugin" base_class_type="nodelet::Nodelet">
	    <description>This is a nodelet plugin.description>
	  class>
	library>
class_libraries>
1
2
3
4
5
6
7
8

class name必须与nodelet load 相同，否则会报错。

在ROS包系统中注册插件

为了让pluginlib能够找到ROS包中的所有可用的插件，每个包必须显式指定其导出的插件和包含这些插件的库。

ROS1是通过package.xml导出的。

在package.xml中添加以下语句，其中polygon_base是基类的包名，plugin= xml描述文件

<export>
    <nodelet plugin="${prefix}/plugins.xml">
export>
1
2
3

为了使上述导出命令正常工作，提供插件的包必须直接依赖于包含插件基类的包，还要在package.xml文件中加油

<build_depend>nodeletbuild_depend>
<run_depend>nodeletrun_depend>
或
<depend>nodeletdepend>
1
2
3
4

launch文件运行

<launch>
  <node pkg="nodelet" type="nodelet" name="nodelet_manager"  args="manager" output="screen">
  	<param name="num_worker_threads" value="4"/>
  node>
      
  <node pkg="nodelet" type="nodelet" name="SampleNodelet" args="load custom/Plugin nodelet_manager" output="screen">
  node>
launch>
1
2
3
4
5
6
7
8

load 后跟的名称必须与class name相同，否则会报错。

Components

ROS2 Wiki
Components可以实现以下功能：

• 在不同的进程中运行多个节点。这样可以使不同的进程独立开。一个崩溃其他可以正常运行。也更方便调试各个节点。
• 在同一个进程中运行多个节点。这样可以使得通信更加高效。

对于传统插件，基类可以是用户定义的任何东西。唯一的约束是派生类必须可以默认构造（因此，根据C++定律，基类也必须可以默认构造）。通常对于Components，插件类通常会继承rclcpp::Node类，但这不是必须的。实际上，将类作为Components导出的要求是：

• 具有一个构造函数，该构造函数将单个参数是rclcpp::NodeOptions实例。
• 具有方法：

  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(void)
  1

满足要求的有rclcpp::Node和rclcpp_lifecycle::LifecycleNode等。这意味着Components可以从满足这些要求的任何类中继承，但它也可以自身满足要求并完全跳过继承。

传统上，插件在注册到class_loader factory scheme时必须声明他们的基类。这允许工厂将派生对象实例化为基类的智能指针

#include 
// Registering a plugin
PLUGINLIB_EXPORT_CLASS(BaseClass, DerivedClass)
//
...
// Instantiating a plugin
// We have access to all of BaseClass API
std::shared_ptr<BaseClass> base_ptr = plugin_loader.createSharedInstance("DerivedClass");
1
2
3
4
5
6
7
8

Components基类注册依赖于void(智能)指针的类型擦除，这样就可以将一个简单的包装器类作为所有组件的公共基类。这种设计意味着开发人员不需要自己为新实例查询工厂。

#include 
// Registering a component
RCLCPP_COMPONENTS_REGISTER_NODE(DerivedNode)
//
...
// NodeInstanceWrapper is a wrapper around both a
// std::shared_ptr and a
// rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
// Not much to do with those!
rclcpp_components::NodeInstanceWrapper component_wrapper = component_loader->create_node_instance(options);
1
2
3
4
5
6
7
8
9
10

在大多数情况下，Component继承自rclcpp::Node，因为它是满足上述需求的最简单方法。因此，我们假设Component是一个ROS 2节点，包含它所涉及的一切，可能包括parameters, listeners/publishers, services, action等等。

在CMakeLists.txt

命令行工具

查看已经注册过的components

ros2 component types
1

启动component container，重映射容器名和命名空间

ros2 run rclcpp_components component_container --ros_args --remap __node:=MyContainer --remap __ns:=/ns
1

component container的名字将会是/ns/MyContainer
容器的命名空间不会影响加载的components

加载component到容器

# ros2 component load [component_container_name] [package_name] [class_name]
ros2 component load /ns/MyContainer composition composition::Listener
1
2

return the unique ID of the loaded component as well as the node name:

终端输出

Loaded component 1 into '/ns/MyContainer' container node as '/listener'`
1

重映射component的节点名和命名空间

ros2 component load /ns/MyContainer composition composition::Talker --node-name talker3 --node-namespace /ns1
1

终端输出

Loaded component 2 into '/ns/MyContainer' container node as '/ns1/talker3'`
1

使用component id卸载组件

ros2 component unload /ns/MyContainer 1 2
1

终端输出

Unloaded component 1 from '/ns/MyContainer' container
Unloaded component 2 from '/ns/MyContainer' container
1
2

编译插件库

与ROS1 nodelet相似，将节点编译成共享库，与ROS1在package.xml中导出插件不同，ROS2在CMakeLists.txt中使用rclcpp_components_register_nodes注册组件节点。

find_package(rclcpp_components REQUIRED)
add_library([shared_lib_name] SHARED [cppFile...])
rclcpp_components_register_nodes([shared_lib_name] "component_class_name")
1
2
3

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='my_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='composition',
                    plugin='composition::Talker',
                    name='talker'),
                ComposableNode(
                    package='composition',
                    plugin='composition::Listener',
                    name='listener')
            ],
            output='screen',
    )

    return launch.LaunchDescription([container])
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

在C++代码中运行时加载组件

#include "class_loader/class_loader.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/node_factory.hpp"
int main(int argc, char * argv[])
{
  if (argc < 2) {
    fprintf(stderr, "Requires at least one argument to be passed with the library to load\n");
    return 1;
  }
  rclcpp::init(argc, argv);
  rclcpp::Logger logger = rclcpp::get_logger("composition");
  rclcpp::executors::SingleThreadedExecutor exec;
  rclcpp::NodeOptions options;
  std::vector<std::unique_ptr<class_loader::ClassLoader>> loaders;
  std::vector<rclcpp_components::NodeInstanceWrapper> node_wrappers;

  std::vector<std::string> libraries;
  for (int i = 1; i < argc; ++i) {
    libraries.push_back(argv[i]);
  }
  for (auto library : libraries) {
    RCLCPP_INFO(logger, "Load library %s", library.c_str());
    std::unique_ptr<class_loader::ClassLoader> loader = std::make_unique<class_loader::ClassLoader>(library);
    auto classes = loader->getAvailableClasses<rclcpp_components::NodeFactory>();
    for (auto clazz : classes) {
      RCLCPP_INFO(logger, "Instantiate class %s", clazz.c_str());
      rclcpp_components::NodeFactory::SharedPtr node_factory = loader->createInstance<rclcpp_components::NodeFactory>(clazz);
      rclcpp_components::NodeInstanceWrapper wrapper = node_factory->create_node_instance(options);
      node_wrappers.push_back(wrapper);
      // 获取节点基类指针
      rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node = wrapper.get_node_base_interface();
      // 将component节点加到执行器中
      exec.add_node(node);
    }
    loaders.push_back(std::move(loader));
  }

  exec.spin();
  // 卸载components节点
  for (auto wrapper : node_wrappers) {
    exec.remove_node(wrapper.get_node_base_interface());
  }
  node_wrappers.clear();

  rclcpp::shutdown();

  return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49