创建和使用插件

前言

在 ROS 2 中，插件（plugin） 是一种动态加载和扩展功能的机制。插件的主要目的是通过抽象和封装，提供模块化、可扩展和可重用的功能，以支持不同的硬件接口、算法、通信协议等。插件机制通常是基于某些接口或抽象类，通过动态加载共享库（如 .so 文件）来实现的。ROS 2 插件通常使用 pluginlib 库来管理插件的加载和卸载，从而扩展 ROS 2 系统的功能。在本章中，我们将创建两个新的功能包，一个定义滤波器的基类，另一个提供插件，即不同的滤波器类型，比如互补滤波或者卡尔曼滤波。

图 1: 插件工作原理

图 1 展示了插件的工作原理，首先需要定义一个基类（接口类），本例中为 FilterInterface。在基类中定义了一组通用的函数接口，如： initialize()，update()，get_state()，这些函数不实现具体功能，仅规定“插件必须具备什么”，在基类中使用virtual关键字描述。然后定义派生类，派生类继承基类，此基类中定义的函数会在派生类中继承，本例子中的派生类为ComplementaryFilter和KalmanFilter，但是他们的函数实现的功能不同ComplementaryFilter实现互补滤波，KalmanFilter实现Kalman滤波。

在使用插件时，先使用基类实例化对象，如果赋值给该对象的类型为 ComplementaryFilter，就会实现互补滤波的功能。如果赋值给该对象类型为 KalmanFilter，则会实现Kalman滤波的功能。这就是插件的原理。

编写插件

创建基类功能包

在ros2_ws/src文件夹中使用以下命令创建一个新的功能包：

ros2 pkg create --build-type ament_cmake filter_base --dependencies pluginlib --node-name filter_node 

编辑 ros2_ws/src/filter_base/include/filter_base/filter_interface.hpp，内容如下：

#ifndef FILTER_INTERFACE_HPP
#define FILTER_INTERFACE_HPP

class FilterInterface
{
    public:
        virtual void initialize() = 0;
        virtual void update(double sensor_data) = 0;
        virtual double get_state() = 0;
        virtual ~FilterInterface() {}
    protected:
        FilterInterface() {}
};

#endif  // FILTER_BASE_FILTER_INTERFACE_HPP

上述代码创建了一个名为 FilterInterface 的抽象类。使用 pluginlib 时，需要一个无参数的构造函数，因此如果类需要给参数赋值，我们使用类似 initialize 方法将它们传递给对象。

为了使这个头文件对其他类可用，打开 ros2_ws/src/filter_base/CMakeLists.txt 进行编辑。在ament_target_dependencies 命令后添加以下行：

install(
  DIRECTORY include/
  DESTINATION include
)

这个命令会把与 CMakeLists.txt 同级 include 文件夹复制到工作空间下面的 install/include 目录下。在 ament_package 命令前添加下面的命令，此命令会将 include 路径添加到功能包的信息中，让其他依赖该功能包的功能包可以找到头文件

ament_export_include_directories(
  include
)

创建插件功能包

现在使用上述抽象类编写两个非虚拟实现。在 ros2_ws/src 文件夹中使用以下命令创建第二个功能包filter_plugins,同时使用 --library-name 参数创建了 filter_plugins_library 库文件：

ros2 pkg create --build-type ament_cmake --dependencies filter_base pluginlib --library-name filter_plugins_library filter_plugins

1. 编写插件源代码

打开 ros2_ws/src/filter_plugins/src/filter_plugins_library.cpp 进行编辑，并粘贴以下内容：

#include "filter_base/filter_interface.hpp"
#include <pluginlib/class_list_macros.hpp>

class ComplementaryFilter : public FilterInterface
{
    public:
        void initialize() override
        {
            // 初始化互补滤波器
            filter_state_ = 0.0;
        }

        void update(double sensor_data) override
        {
            // 简单的互补滤波更新公式
            filter_state_ = 0.98 * (filter_state_ + sensor_data) + 0.02 *     sensor_data;
        }

        double get_state() override
        {
            return filter_state_;
        }

    private:
        double filter_state_;
    
};


class KalmanFilter : public FilterInterface
{
    public:
        void initialize() override
        {
            // 初始化卡尔曼滤波器
            filter_state_ = 0.0;
            error_covariance_ = 1.0;
        }

        void update(double sensor_data) override
        {
            // 卡尔曼滤波更新公式
            double kalman_gain = error_covariance_ / (error_covariance_ + 1.0);
            filter_state_ += kalman_gain * (sensor_data - filter_state_);
            error_covariance_ = (1.0 - kalman_gain) * error_covariance_;
        }

        double get_state() override
        {
            return filter_state_;
        }

    private:
        double filter_state_;
        double error_covariance_;
};

// 注册插件
PLUGINLIB_EXPORT_CLASS(ComplementaryFilter, FilterInterface)
PLUGINLIB_EXPORT_CLASS(KalmanFilter, FilterInterface)

这段代码定义了两个类分别为 ComplementaryFilter 和 KalmanFilter，这两个类都继承了基类FilterInterface 并对基类中的函数进行了重新定义。最后使用 PLUGINLIB_EXPORT_CLASS 宏将 ComplementaryFilter 和 KalmanFilter 类注册为FilterInterface 类的一个插件。这意味着 ComplementaryFilter 和KalmanFilter类实现了 FilterInterface 接口，并可以被 pluginlib 的类加载器识别和实例化。

插件类，在本例中是 ComplementaryFilter 和 KalmanFilter。
基类，在本例中是 FilterInterface。

2. 声明插件XML

上述步骤使得功能包被加载时可以创建插件实例，但插件加载器仍然需要一个途径来找到库文件，并且要知道引用库文件的哪些内容。为此，我们还将创建一个XML文件，让插件的所有必要信息可供ROS工具链使用。

创建 ros2_ws/src/filter_plugins/plugins.xml，并输入以下代码：

<library path="filter_plugins_library">
  <class type="ComplementaryFilter" base_class_type="FilterInterface">
    <description>This is a complementary filter plugin.</description>
  </class>
  <class type="KalmanFilter" base_class_type="FilterInterface">
    <description>This is a kalman filter plugin.</description>
  </class>
</library>

path="filter_plugins_library"：指定了插件库的名称。在ROS 2中，通常 path 就是动态库文件的名称，不包括前缀 lib 和扩展名 .so。

type="ComplementaryFilter" ：指定了插件的完全限定名。
base_class_type="FilterInterface"：指定了插件基类名，就是插件继承的类。
<description>This is a complementary plugin.</description>：提供了关于插件的描述。这个描述可以是任何文本，用于说明插件的功能或用途。

3 声明插件的CMake

最后一步是通过 CMakeLists.txt 导出插件（与ROS 1不同的地方，在 ROS1 中的导出是通过package.xml完成的）。在 ros2_ws/src/filter_plugins/CMakeLists.txt 中，在读取find_package(pluginlib REQUIRED) 的行后添加以下行：

pluginlib_export_plugin_description_file(filter_base plugins.xml)

pluginlib_export_plugin_description_file 命令的参数：

具有基类的功能包，即 filter_base。
插件声明xml的相对 CMakeLists.txt 路径，即 plugins.xml。

使用插件

现在可以使用插件了，该插件可以在任何功能包中使用，这里选择在基类功能包中使用。编辑 ros2_ws/src/filter_base/src/filter_node.cpp 如下内容：

#include <pluginlib/class_loader.hpp>
#include <filter_base/filter_interface.hpp>

int main(int argc, char** argv)
{
  // 为了避免未使用参数警告
  (void) argc;
  (void) argv;

  pluginlib::ClassLoader<FilterInterface> filter_loader("filter_base", "FilterInterface");

  try
  {
    std::shared_ptr<FilterInterface> complementaryfilter = filter_loader.createSharedInstance("ComplementaryFilter");
    complementaryfilter->initialize();
    double sensor_data=3;
    double update_complementaryfilter_data;
    complementaryfilter->update(sensor_data);
    update_complementaryfilter_data=complementaryfilter->get_state();

    std::shared_ptr<FilterInterface> kalmanfilter = filter_loader.createSharedInstance("KalmanFilter");
    kalmanfilter->initialize();
    double update_kalmanfilter_data;
    kalmanfilter->update(sensor_data);
    update_kalmanfilter_data=kalmanfilter->get_state();

    printf("complementaryfilter: %.2f\n", update_complementaryfilter_data);
    printf("kalmanfilter: %.2f\n", update_kalmanfilter_data);
  }
  catch(pluginlib::PluginlibException& ex)
  {
    printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
  }

  return 0;
}

这里的关键就是明白定义在 class_loader.hpp 头文件中 ClassLoader 类：

它使用基类模板进行初始化，即 FilterInterface。
第一个参数是基类功能包的名称，即 filter_base。
第二个参数是插件基类中类的名称，即 FilterInterface，是字符串的形式。

实例化类的实例有多种方法，这个例子使用了共享指针。我们只需要调用 createSharedInstance，并传入插件类的完全限定类型，在这个例子中是 ComplementaryFilter 和 KalmanFilter。

重要说明：定义此节点的 filter_base 功能包不依赖于 ComplementaryFilter 和KalmanFilter 类。插件将被动态加载，无需声明任何依赖关系。此外，我们使用插件名称来实例化类，但也可以用参数等方式动态实现。

构建和运行

返回到工作空间根目录 ros2_ws，并构建新的功能包：

colcon build --packages-select filter_base filter_plugins

从 ros2_ws 目录，确保加载设置文件：

source install/setup.bash

现在运行节点：

ros2 run filter_base filter_node

它应该打印：

complementaryfilter: 3.0
kalmanfilter: 1.5

总结

本章简单介绍了ROS2插件编写的步骤，一般在编写插件时会给插件一个命名空间，多个插件可能有同名的类或函数，命名空间可以防止这些名字互相冲突，另一个详细编写示例请参考官方教程https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Pluginlib.html

前言​

编写插件​

创建基类功能包​

创建插件功能包​

使用插件​

总结​

前言

编写插件

创建基类功能包

创建插件功能包

使用插件

总结