Day22 ROS2 Plugins

15th鐵人賽 ros2 ros2 porting ros ros plugins

leochien1110

2023-10-08 00:51:17

289 瀏覽

分享至

Plugin

ROS中有個特別的package叫做pluginlib，這個package提供了一個plugin的架構，讓我們可以在不修改原始碼的情況下，動態的載入plugin，類似於C++的多型(polymorphism)。這樣可以讓程式更有彈性，也更容易擴充。

舉例來說，在自駕車中避障的功能可能同時有不同的演算法，我們會需要在不同的情境下啟用不同的避障功能，像是白天就可以使用LiDAR+相機的plugin，但夜間可能相機感光度不夠只能依賴LiDAR的plugin。

範例

這邊拿官方的多邊形為例子來說明plugin的使用方法。首先我們會先定義一個多邊形的base class，接著會有三角形和長方形的plugin連結到base class。

這邊借用官方的流程圖:
ploygon plugin

安裝pluginlib package

sudo apt-get install ros-foxy-pluginlib

建立一個polygon_base package

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake polygon_base --dependencies pluginlib --node-name area_node

在ros2_ws/src/polygon_base/include/polygon_base/regular_polygon.hpp中定義一個多邊形的base class

#ifndef POLYGON_BASE_REGULAR_POLYGON_HPP
#define POLYGON_BASE_REGULAR_POLYGON_HPP

namespace polygon_base
{
class RegularPolygon
{
    public:
    virtual void initialize(double side_length) = 0;
    virtual double area() = 0;
    virtual ~RegularPolygon(){}

    protected:
    RegularPolygon(){}
};
}  // namespace polygon_base

#endif  // POLYGON_BASE_REGULAR_POLYGON_HPP

這些基本上都是C++的pure virtual function，沒有辦法單獨運作，需要被繼承才能使用。

接著編輯CMakeLists.txt讓這個header可以被其他Package的Class繼承:

install(
DIRECTORY include/
DESTINATION include
)
...
ament_export_include_directories(
include
)
# before ament_package

創建polygon_plugins的package

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake polygon_plugins --dependencies polygon_base pluginlib --library-name polygon_plugins

編輯ros2_ws/src/polygon_plugins/src/polygon_plugins.cpp:

#include <polygon_base/regular_polygon.hpp>
#include <cmath>

namespace polygon_plugins
{
class Square : public polygon_base::RegularPolygon
{
    public:
    void initialize(double side_length) override
    {
        side_length_ = side_length;
    }

    double area() override
    {
        return side_length_ * side_length_;
    }

    protected:
    double side_length_;
};

class Triangle : public polygon_base::RegularPolygon
{
    public:
    void initialize(double side_length) override
    {
        side_length_ = side_length;
    }

    double area() override
    {
        return 0.5 * side_length_ * getHeight();
    }

    double getHeight()
    {
        return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
    }

    protected:
    double side_length_;
};
}

#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)

這邊我們定義了兩個plugin，一個是正方形，一個是三角形，裡面定義了上面pure virtual function的實作。
Classes後面接著才是重點，這邊我們使用了pluginlib提供的PLUGINLIB_EXPORT_CLASS，這個macro會幫將這些classes註冊到plugin中。第一個參數是class的名稱，第二個參數是base class的名稱。

Plugin loader還會需要一個plugin.xml來表達資訊。創建一個ros2_ws/src/polygon_plugins/plugins.xml:
```
<library path="polygon_plugins">
<class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
    <description>This is a square plugin.</description>
</class>
<class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
    <description>This is a triangle plugin.</description>
</class>
</library>
```
- library的path是指定plugin的library。與ROS不同的是ROS會需要lib/的prefix，這邊的話會變成lib/polygon_plugins。但這點在ROS2中已經被修改，不需要加上lib/。
- type是plugin的名稱。
- base_class_type是指定base class的名稱。
- description是plugin的描述。

在ROS中註冊plugin是寫在package.xml而ROS2則是寫在CMakeLists.txt，不過也變的冗長許多從三行變成好幾行。編輯CMakeLists.txt，將下面的程式碼加到ros2_ws/src/polygon_plugins/CMakeLists.txt的find_package(pluginlib REQUIRED)下面:

add_library(polygon_plugins src/polygon_plugins.cpp)
target_include_directories(polygon_plugins PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
polygon_plugins
polygon_base
pluginlib
)

pluginlib_export_plugin_description_file(polygon_base plugins.xml)

install(
TARGETS polygon_plugins
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)

pluginlib_export_plugin_description_file的第一個參數是指定base class的package，第二個參數是指定plugin.xml的路徑。

最後在ament_package前面加上:

ament_export_libraries(
polygon_plugins
)
ament_export_targets(
export_${PROJECT_NAME}
)

這樣就大功告成寫好了plugin，接著我們要來寫一個node來使用這些plugin。

使用plugin

理論上這些plugin可以在任何一個package中使用，這邊我們寫在base package裡。

新增ros2_ws/src/polygon_base/src/area_node.cpp:

#include <pluginlib/class_loader.hpp>
#include <polygon_base/regular_polygon.hpp>

int main(int argc, char** argv)
{
    // To avoid unused parameter warnings
    (void) argc;
    (void) argv;

    pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("polygon_base", "polygon_base::RegularPolygon");

    try
    {
        std::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createSharedInstance("polygon_plugins::Triangle");
        triangle->initialize(10.0);

        std::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createSharedInstance("polygon_plugins::Square");
        square->initialize(10.0);

        printf("Triangle area: %.2f\n", triangle->area());
        printf("Square area: %.2f\n", square->area());
    }
    catch(pluginlib::PluginlibException& ex)
    {
        printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
    }

    return 0;
}

ClassLoader是pluginlib提供的class，可以用來載入plugin。
* template內是放base class和plugin的名稱。
* 第一個參數是指定base class的package。
* 第二個參數是指定base class的名稱。

:warning: 注意這邊ploygon_base並沒有指出polygon_plugins的dependency而是直接使用，所以在使用時須自行確認是否有安裝polygon_plugins。

Build:

colcon build --packages-select polygon_base polygon_plugins

source install/setup.bash

接著執行:
```
ros2 run polygon_base area_node
```
會得到:
```
Triangle area: 43.30
Square area: 100.00
```
這邊我們可以看到，我們在area_node.cpp中並沒有直接使用到polygon_plugins，但是在執行時卻會載入polygon_plugins的plugin。