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に列挙します。

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のmsgを送信します。

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のお作法です。

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のインストールの記述が必要です。

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の存在を通知するために必要です。

srs_mavros_plugins/package.xml

  <depend>mavros</depend>

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

ビルド&実行

ビルド

source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build

実行

 ros2 launch espresso_frame_device_launch mavros.launch.py

目次ページへのリンク

ROS2講座の目次へのリンク