ROS2講座14 mavrosのpluginsを作成する

# 環境

この記事は以下の環境で動いています。

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-8250U |
| Ubuntu | 22.04 |
| ROS2 | Humble |
| Qt | 5.15.3 |
| SQLite | 3.37.2 |

# 概要

前回はmavrosをデフォルトの状態で使用していましたが、`mavros`、`mavros_extras`のプラグラインだけでは足らない機能もあります。それらをプラグインを自作することで実行します。

# mavrosでのpluginの指定
mavrosは必須部分のcoreとpluginの部分に分かれています。launch時のパラメーターで使用するpluginを選択します。以下のように`plugin_denylist`と`plugin_allowlist`のパラメーターがあります。

* 基本的にインストールされているpluginはすべて使用し一部だけ使用しない場合は、`plugin_denylist`に列挙します。
* 指定のpluginだけを使用する場合は、`plugin_denylist`に`"*"`を、`plugin_allowlist`に列挙します。

```yaml:espresso_frame_device_launch/config/mavros.yamlの一部
/device/mavros/mavros:
  ros__parameters:
    plugin_denylist:
    - "*"
    plugin_allowlist:
    - command
    - distance_sensor
    - global_position
    - imu
    - led_control
    - rc_io
    - setpoint_velocity
    - sys_status
    - param
    - velocity_position
    - play_tune_v1
```

# 自作pluginの作成

プラグインはC++で書いて自作することが出来ます。

## led_control plugin

LEDの色を指定するpluginがデフォルトではないので作成します。MAVLINKの[LED_CONTROL](https://mavlink.io/en/messages/ardupilotmega.html#LED_CONTROL)のmsgを送信します。

```c++:srs_mavros_plugins/src/led_control.cpp
#include "mavros/mavros_uas.hpp"
#include "mavros/plugin.hpp"
#include "mavros/plugin_filter.hpp"
#include "std_msgs/msg/color_rgba.hpp"

namespace srs_mavros_plugins {

class LedControlPlugin : public mavros::plugin::Plugin {
 public:
  explicit LedControlPlugin(mavros::plugin::UASPtr uas_)
      : Plugin(uas_, "led_control") {
    color_led_sub_ = node->create_subscription<std_msgs::msg::ColorRGBA>(
        "~/color", 10,
        std::bind(&LedControlPlugin::onColorData, this, std::placeholders::_1));
  }

  Subscriptions get_subscriptions() override { return {}; }

 private:
  void onColorData(const std_msgs::msg::ColorRGBA::SharedPtr color) {
    mavlink::ardupilotmega::msg::LED_CONTROL msg = {};
    uas->msg_set_target(msg);
    msg.instance = 255;
    msg.pattern = 0;
    msg.custom_len = 3;
    msg.custom_bytes[0] = std::min(std::max((int)(color->r * 255), 0), 255);
    msg.custom_bytes[1] = std::min(std::max((int)(color->g * 255), 0), 255);
    msg.custom_bytes[2] = std::min(std::max((int)(color->b * 255), 0), 255);
    uas->send_message(msg);
  }

  rclcpp::Subscription<std_msgs::msg::ColorRGBA>::SharedPtr color_led_sub_{
      nullptr};
};

}  // namespace srs_mavros_plugins

#include <mavros/mavros_plugin_register_macro.hpp>  // NOLINT
MAVROS_PLUGIN_REGISTER(srs_mavros_plugins::LedControlPlugin)
```

* `mavros::plugin::Plugin`を継承したクラスを作成します。
    * `Plugin(uas_, "led_control")`の部分でプラグインの名前を付けています。
* コンストラクターでROSのifの設定をします。
* `get_subscriptions`ではmavlinkから受けた時のcallback関数を設定します。今回はmavlinkに送るだけなので空で設定します。
* `onColorData`はROSのcallback関数で、この中でmavlinkに送信する処理を書きます。
    * `mavlink::ardupilotmega::msg::LED_CONTROL`はLED_CONTROLに対応する構造体です。
    * `uas->msg_set_target(msg);`は構造体の`target_system`、`target_component`を設定する関数です。今回は(1,1)が入ります。
    * `instance`を設定を変更するLEDの対象を指定します。255ではすべてのLEDを指定になります。
    * `pattern`=0、`custom_len`=3で３バイトのRGBの色でLEDの色を指定の意味になります。
* `MAVROS_PLUGIN_REGISTER(srs_mavros_plugins::LedControlPlugin)`はpluginlibのお作法になります。

## mavros_plugins.xmlの記載

pluginをmavrosに認識させるためのファイルです。pluginlibのお作法です。

```xml:srs_mavros_plugins/mavros_plugins.xmlの一部
<?xml version="1.0"?>
<library path="srs_mavros_plugins">
  <class name="led_control" type="mavros::plugin::PluginFactoryTemplate&lt;srs_mavros_plugins::LedControlPlugin&gt;" base_class_type="mavros::plugin::PluginFactory">
    <description>led control plugins</description>
  </class>

</library>
```

* `library`要素の`path`属性はビルドするライブラリのオブジェクトファイルのターゲット名と同じにします。要するに今回の場合は`libsrs_mavros_plugins.so`を読みに行くという設定になります
* `class`要素ではプラグインのクラスを指定します。
    * `name`は名前です。C++の記述で使ったものと同じものにします。
    * `type`はプラグインのクラスの指定
    * `base_class_type`は基底クラスの指定です。

## CMakeList

ライブラリのビルドとインストール、上記のxmlのインストールの記述が必要です。

```cmake:srs_mavros_plugins/CMakeLists.txt
add_library(${PROJECT_NAME} SHARED
  src/led_control.cpp
  src/velocity_position.cpp
  src/play_tune_v1.cpp
)

ament_target_dependencies(${PROJECT_NAME}
  # must for plugin build
  mavros
  mavros_msgs
  pluginlib
  diagnostic_msgs
  tf2_ros
  # use in plugins
  std_msgs
  sensor_msgs
  nav_msgs
  tf2_geometry_msgs
)

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

pluginlib_export_plugin_description_file(mavros mavros_plugins.xml)
```

## package.xmlの記述

mavros本体に上記の`mavros_plugins.xml`の存在を通知するために必要です。

```xml:srs_mavros_plugins/package.xml
  <depend>mavros</depend>

  <export>
    <build_type>ament_cmake</build_type>
    <mavros plugin="${prefix}/mavros_plugins.xml" />
  </export>
```

## ビルド&実行

```shell:ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
```

```shell:実行
 ros2 launch espresso_frame_device_launch mavros.launch.py
```

# 目次ページへのリンク
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)