ROS 2の紹介 by gbiggs

ROS 2のAPIの基本を理解
- 基本的なコードを読み解く
- ビルド＆実行
ROS 2のノードの書き方を理解
- コンポーネントノードのソースを読み解く
- ビルド＆実行
メッセージ受信ノードの作成
独自メッセージ型の利用方法
ノードのコンポジション
- ソース作成
- ビルド＆実行
サービスの定義と利用
アクションの定義と利用
- アクションの定義
- アクションサーバーノードの作成
アクションの利用するノードの作成
- ビルドとテスト
アクションのキャンセル方法
- ビルドとテスト

本セクションではROS 2上で動くプログラムの書き方とそれに関連する新しいフィーチャーを学習します。

ROS 2のAPIの基本を理解

まずは端末を開き、新しいワークスペースを作成します。

$ mkdir -p ~/ros2_basics/src

そしてワークスペースに用意されたパッケージを入れます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics.git
Cloning into 'rosjp_ros2_basics'...
[省略]
$ cd rosjp_ros2_basics

CまたはC++でノードを作成するとき、以下のステップを行います。

パッケージを作成し、package.xmlにパッケージのメタデータを記述します。
CMakeLists.txtを作成し、パッケージのコンパイル方法を記述します。
C++のソースファイルを作成しノードを実装します。

下記で作成されるファイルの内容を説明します。

基本的なコードを読み解く

パッケージのメタデータ

パッケージのメタデータはパッケージのディレクトリに置くpackage.xmlに記述します。

ソースファイルの編集にはお好みのテキストエディターが利用可能です。Linuxがはじめの方にgeditはおすすめです。

お好みのテキストエディターで~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros1_style/package.xmlを開きます。

package.xmlではパッケージの作成者、連絡先、ウェブサイト等の情報が記述されています。さらに、パッケージのビルド、テスト、利用等のために依存するパッケージも記述されています。依存関係情報はcolconや他のROS 2のツールが利用します。特にcolconはこの情報でワークスペースのビルド順番を決めます。

下記はgreeter_ros1_styleパッケージのpackage.xmlです。（~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros1_style/package.xml）

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeter_ros1_style</name>
  <version>1.2.0</version>
  <description>講習会ようのサンプルソース：ROS1形式のメッセージ送信ノード</description>
  <maintainer email="git@example.com">Geoffrey Biggs</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>std_msgs</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

1行〜2行 XMLのヘッダー
3行　パッケージメタ情報の指定開始
4行　パッケージ名
5行　パッケージのバージョン
6行　パッケージの説明
7行　パッケージを管理する方（複数可）
8行　パッケージのライセンス、READMEやLICENSEファイルに合わせる
10行　ビルドするために必要なツールのパッケージ（複数可）
12行〜１３行　ビルドするために必要なライブラリのパッケージ（複数可）
15行〜１６行　実行するために必要なソフトウェア（複数可）
18行　他パッケージが利用する情報の集まり
19行　colconようのビルドタイプ指定（ament-cmakeを利用するタイプ）

ROS 2は現在のcatkinと同様にpackage.xmlのフォーマットバージョン２を利用しています。ただし、REP-149が指定するフィーチャーによってフォーマットバージョン３が必要です。バージョン３の主な新フィーチャーは依存グループの管理、パッケージレベルでABIバージョンの指定及び同一のパッケージでROS 1とROS 2をサポートすることの３つです。 ROS 1を利用しないパッケージにはバージョン３の利用がおすすめです。

パッケージのコンパイル方法

ament_cmakeビルドタイプを利用するパッケージのコンパイル方法はパッケージのトップディレクトリにあるCMakeLists.txtファイルに記述します。

お好みのテキストエディターで~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros1_style/CMakeLists.txtを開きます。

cmake_minimum_required(VERSION 3.5)
# パッケージ名
project(greeter_ros1_style)

# C++14を利用する
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

# すべてのワーニングを表示する
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 依存するパッケージを探す
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

# ノードの実行ファイルをコンパイルする
add_executable(greeter src/greeter.cpp)
# 実行ファイルのコンパイルターゲットに依存パッケージの情報を追加する
ament_target_dependencies(greeter rclcpp std_msgs)

# ノードの実行ファイルをインストールする（必須）
install(TARGETS
  greeter
  DESTINATION lib/${PROJECT_NAME}
  )

# amentのリソースインデクスにパッケージを登録する
ament_package()

ROS 1のCMakeLists.txtと違って、純粋CMakeのファイルに近いです。 colconようの部分はかなり小さくなりました。 colconはコンパイルするときに環境変数やCMakeの内部変数の設定でコンパイルプロセスを変更します。おかげでROS 2のパッケージの独立性が高いです。

基本の内容は下記のようです。

1行　最低のCMakeバージョンを指定します。
3行　パッケージ名を指定します。 package.xmlと合わせます。
6行〜8行　C++14を利用するソースのでコンパイラ等を合わせます。
11行〜13行　コンパイル中の注意情報を全部表示するように設定します。必要ではないがおすすめです。
16行〜18行　このパッケージがコンパイルするときに利用する他パッケージを指定します。ライブラリパス、ヘッダーパス等の情報を取得します。
21行　ノードの実行ファイルをコンパイルターゲットに追加します。２番目の引数はこのコンパイルターゲットのためにコンパイルするソースファイルです。
23行　他パッケージからのコンパイル情報をコンパイルターゲットに登録します。ライブラリパス等をコンパイラに渡すために必要です。（CMakeが分かる方：TARGET_LINK_LIBRARIES等を呼ぶと同様です。）
26行〜29行　ノードの実行ファイルをインストールします。 ROS 2ではファイルをインストールしないと利用できません。コンパイルだけは足りません。
32行　colconにパッケージを登録します。 colconが管理するリソースインデクスにパッケージ情報、得にパッケージの内容、が登録され、ROS 2のツール（ros2 run等）で利用されます。パッケージを登録しないと他のROS 2のツールはパッケージの存在が分かりません。

ノードの実装

最後にノードを実装します。

エディターで~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros1_style/src/greeter.cppを開きます。このソースはROS 2のAPIとROS 1のスタイル（構造）を利用するノードです。

このコードが実行されたときの流れを確認しましょう。

まず、先頭部分では、必要なヘッダファイルをインクルードしています。 ROS 2のソースには「Include what you use」（利用するものを直接インクルードする）というルールがおすすめです。ソースの再利用性とメンテナンス性が上がります。

#include <rclcpp/rclcpp.hpp>

C++で実装したノードので、ROS 2のC++ようのrclcppAPIを利用します。 Cで実装する場合はrclcをインクルードします。

#include <std_msgs/msg/string.hpp>

std_msgs/Stringを出力するノードです。このメッセージ型のヘッダーをインクルードします。

#include <chrono>

ROS 2はC++14を利用します。 C++14の一つのフィーチャーは、1sや5hのように時間を単位付けで書くことです。このフィーチャーを利用するためにchronoをインクルードします。

#include <memory>

std::shared_ptr、std::make_shared等を利用するためにインクルードします。

続いて、C++のmain関数が定義されています。本ノードは非常に簡単なのですべての機能がmain関数に入れられました。

int main(int argc, char *argv[]) {
  using namespace std::chrono_literals;

  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("greeter");

  auto publisher = node->create_publisher<std_msgs::msg::String>("greeting", 10);

  std_msgs::msg::String greeting;
  greeting.data = "hello world";

  rclcpp::WallRate rate(1s);

  while (rclcpp::ok()) {
    RCLCPP_INFO(node->get_logger(), "Publishing greeting '%s'", greeting.data.c_str());
    publisher->publish(greeting);
    rclcpp::spin_some(node);
    rate.sleep();
  }
  rclcpp::shutdown();
  return 0;
}

（2行目はC++14の時間単位フィーチャーのために必要です。）

main関数はまず4行目ROS 2のインフラストラクチャーをセットアップします。 rclcpp::initはROS 2の初期設定を行います。 ROS 1と違って、ノードの初期化ではありません。 1、2番目の引数には、main関数の引数をそのまま与えます。

ROS 2では、ROS 1と違って同一プロセスで複数のノードの作成は可能です。本サンプルでは5行目でノードを作成します。ノードのインスタンスはスマートポインター(C++11以上のフィーチャーであるstd::smart_ptr型）として管理します。 ROS 2のAPIが作成する様々なインスタンスは基本的にスマートポインターとして管理することがベストプラクティスです。ノード作成の引数にこのノードの名前(この例では”Greeter”です)を与えます。ノード作成の結果をnodeという変数で保存します。

（autoはC++11のフィーチャーです。変数のメッセージ型はコンパイラーが自動的に判断するためです。 autoの利用でタイプセフティを保ちながらソースコードをより簡単にします。）

7行目でデータ送信のためのパブリッシャーを初期化します。この変数の作成によりトピックが作成され、このノードからデータの送信が可能になります。 引数としてトピック名を指定します。この例では"greeting"を利用します。テンプレートパラメータは送信するメッセージ型です。パブリッシャーは決まっているメッセージ型しか送信できないので、インスタンス作成の時に指定しないとなりません。 std_msgs/Stringを送信するので、std_msgs::msg::Stringのクラスを示します。

9行目は送信するためのデータを保存する変数の作成です。ここもスマートポインターを利用しています。パブリッシャーと同様のメッセージ型を利用します。データの初期設定は10行目で行います。

セットアップの最後として、rclcpp::WallRate rate(1s)で周期実行のためのクラスを初期化しています。初期化時の引数で実行周波数(この例では1 Hz)を指定します。（1sはC++14にある単位付け時間指定方法のフィーチャーです。「１秒」という意味です。）

while(rclcpp::ok())で、メインの無限ループを回します（すなわちこのノードのメーンプロセッシングループです。）。 rclcpp::ok()をwhileの条件にすることで、ノードの終了指示が与えられたとき(Ctrl+c が押された場合も含む)には、ループを抜けて終了処理などが行えるようになっています。

ループ中では、最初にログ情報を出力します。 ROS 2で情報を画面などに出力する際に用いる、RCLCPP_INFO関数を呼び出してメッセージを表示しています。ほかにも、RCLCPP_DEBUG、RCLCPP_WARN、RCLCPP_ERROR、RCLCPP_FATAL関数が用意されています。

その後、データを送信します。送信するデータがすでに入力しましたが、データが処理中に変更する（または生成される）場合はループ内に設定します。 publisher->publish(greeting)によってデータを送信します。この行でデータはバッファーに入れられ、別のスレッドが自動的にサブスクライバに送信します。

最後に17行目で_rclcpp::spin_some(node)を呼び出して、ノードの内部処理_{: style=”color: red” } といった処理を行います。 spin_someは、その時点で届いているイベント（メッセージの受け取り処理等）を済ませた後、すぐに処理を返します。 18行目のrate.sleep()は、先ほど初期化した実行周波数を維持するようにsleepします。

メーンループが終了するとROS 2をシャットダウンします。

ビルド＆実行

ROS 2上でこのパッケージをビルドするためには、colcon buildコマンドを用います。

$ cd ~/ros2_basics/
$ colcon build --packages-select greeter_ros1_style

端末で実行してみましょう。

ROS 2システムの実行の際にノードがお互いに見つけるために、ROS 1と違ってroscoreというデーモンは要りません。ノードは自動的に他ノードを見つけて接続できます。将来、グローバルパラメータ等の他の機能のためにroscoreは戻る可能性がありますが、現在は存在しません。すなわち、上記のノードを実行するためにroscoreの実行は要らなくて、端末は一つで済みます。

ビルドした端末で下記を実行しノードを起動します。

新しい端末でワークスペースを利用し始めると必ずまずはsource install/local_setup.bashを実行しなければなりません。一つの端末で一回だけ実行すれば十分です。その端末を閉じるまでに有効です。

$ source install/local_setup.bash
$ ros2 run greeter_ros1_style greeter
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'

上述が表示されれば成功です。

端末で Ctrl+c でノードを終了します。

ROS 2のノードの書き方を理解

上記はROS 1のスタイルでノードを実装しました。しかしこのメーン関数で実装するスタイルだとノードの再利用性は低いです。

ノードの開発者が決めた周波以外で振る舞えを実行することや別のノードと同一プロセスに入れることが不可能です。クラスとしてノードを実装する手法もありますが、APIが独自になるのでそれでも再利用性が低いです。 ROS 2では、ノードの再利用性の問題を快活するために新しいノード構造のベストプラクティスが定められました。ここでROS 2の新しいお勧めのノード構造を勉強します。

ROS 2のベストプラクティスを簡潔にすると、共有ライブラリとしてノードを実装するという手法です。 ROS 1のノードレットに近いですが、ノードレットと違ってソースを変更せずに独立ノードとしてもノードレットとしても利用できます。すなわちノードは「ソフトウェアコンポーネント」になります。「コンポーネントノード」との呼び方もあります。下記の図でこの考えを示します。

ROS 2 component-style node

コンポーネントノードのソースを読み解く

ROS 2風のコンポーネントノードのソースはgreeter_ros2_styleというパッケージにあります。ノードの実装はヘッダーファイルとソースファイルの２つのファイルにあります。

~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros2_style/include/greeter_ros2_style/greeter_component.hpp

他ソースファイルに利用するためのコンポーネントノードのクラス定義
~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros2_style/src/greeter_component.cpp

コンポーネントノードの実装

まずはヘッダーファイルの内容を説明します。

ヘッダーファイル

#if !defined GREETER_ROS2_STYLE__GREETER_NODE_HPP_
#define GREETER_ROS2_STYLE__GREETER_NODE_HPP_

#if __cplusplus
extern "C" {
#endif

// The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
// demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define GREETER_EXPORT __attribute__ ((dllexport))
    #define GREETER_IMPORT __attribute__ ((dllimport))
  #else
    #define GREETER_EXPORT __declspec(dllexport)
    #define GREETER_IMPORT __declspec(dllimport)
  #endif
  #ifdef GREETER_BUILDING_DLL
    #define GREETER_PUBLIC GREETER_EXPORT
  #else
    #define GREETER_PUBLIC GREETER_IMPORT
  #endif
  #define GREETER_PUBLIC_TYPE GREETER_PUBLIC
  #define GREETER_LOCAL
#else
  #define GREETER_EXPORT __attribute__ ((visibility("default")))
  #define GREETER_IMPORT
  #if __GNUC__ >= 4
    #define GREETER_PUBLIC __attribute__ ((visibility("default")))
    #define GREETER_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define GREETER_PUBLIC
    #define GREETER_LOCAL
  #endif
  #define GREETER_PUBLIC_TYPE
#endif

#if __cplusplus
} // extern "C"
#endif

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

namespace greeter_ros2_style
{

class Greeter : public rclcpp::Node
{
public:
  GREETER_PUBLIC explicit Greeter(const rclcpp::NodeOptions & options);

private:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;

  void broadcast_greeting();
};

} // namespace greeter_ros2_style

#endif // GREETER_ROS2_STYLE__GREETER_NODE_HPP_

ヘッダーファイルので最初にヘッダーガードがあります。 1行目、2行目及び真下の行はこのためです。

共有ライブラリを作成しているので、ライブラリからエキスポートするシンボル名を指定することが必要です。指定方法はコンパイラーによって変わります。 Visual C++の場合は__declspec(dllexport)等を利用します。 GCCの場合はなし（古いバージョン）または__attribute__を利用します。ソースが複数のコンパイラーを対応するために、4行目から40行目はコンパイラーを判断し正しい方法を利用するためのマクロです。詳しい説明および例は、GNU GCCの説明ページとROS 2のサンプルに参照してください。

#include <rclcpp/rclcpp.hpp>

次はROS 2を利用するためのヘッダーをインクルードします。

#include <std_msgs/msg/string.hpp>

std_msgs/Stringを出力するノードです。このメッセージ型のヘッダーをインクルードします。

namespace greeter_ros2_style
{

ノードの実装をネームスペースに置きます。ネームスペースに置かないと他のノード等とぶつかる可能性があるので、ネームスペースの利用はベストプラクティスです。

class Greeter : public rclcpp::Node
{

コンポーネントノードのクラス定義を始めます。クラス名は自由ですが、ノードのディフォルト名を利用することがいいです。クラスはrclcppに含まれているrclcpp::Nodeクラスから継承します。 rclcpp::NodeはROS 1風のソースで作成したnode変数のメッセージ型と一緒で、ノードを利用するための色々なAPIを提供します。継承するとここで定義しているクラスはROSのインフラストラクチャでノードとして扱えられます。

public:
  GREETER_PUBLIC explicit Greeter(const rclcpp::NodeOptions & options);

クラス定義のパブリックセクションでクラスを共有ライブラリからエキスポートします。（先定義したマクロを利用しています。）そして、クラスのコンストラクタを定義します。コンストラクタの実装はのちほど.cppファイルでします。引数はノード生成のオプションを伝えるためのストラクトです。

private:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;

プライベートセクションでデータを送信するために利用するROS 2のAPIのオブジェクトを定義します。パブリッシャー（ROS 1風のソースの7行目と類似）とタイマーを利用します。

  void broadcast_greeting();

最後にノードの振る舞えを実装するメンバー関数を定義します。後ほど.cppファイルで実装し利用します。

ソースファイル

#include "greeter_ros2_style/greeter_component.hpp"

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <chrono>
#include <memory>

using namespace std::chrono_literals;

namespace greeter_ros2_style
{

Greeter::Greeter(const rclcpp::NodeOptions & options)
: Node("greeter", options)
{
  pub_ = create_publisher<std_msgs::msg::String>("greeting", 10);

  timer_ = create_wall_timer(1s, std::bind(&Greeter::broadcast_greeting, this));
}

void Greeter::broadcast_greeting()
{
  auto greeting = std::make_shared<std_msgs::msg::String>();
  greeting->data = "hello world";
  RCLCPP_INFO(this->get_logger(), "Publishing greeting '%s'", greeting->data.c_str());
  pub_->publish(greeting);
}

} // namespace greeter_ros2_style

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(greeter_ros2_style::Greeter)

これでコンポーネントノードを定義するヘッダーファイルが完成です。次にノードを実装するソースファイルを説明します。

#include "greeter_ros2_style/greeter_component.hpp"

一般C++のソースファイルと同様に、最初にクラス定義のヘッダーファイルをインクルードします。

#include <chrono>
#include <memory>

#include <memory>

ROS 1風のソースと同様の理由でchronoとmemoryをインクルードしネームスペースをusingにします。

namespace greeter_ros2_style
{

ヘッダーファイルで定義したネームスペースに実装も入れます。

Greeter::Greeter(const rclcpp::NodeOptions & options)
: Node("greeter", options)
{
  pub_ = create_publisher<std_msgs::msg::String>("greeting", 10);

  timer_ = create_wall_timer(1s, std::bind(&Greeter::broadcast_greeting, this));
}

まずはコンポーネントノードのクラスのコンストラクタを実装します。 2行目で親クラスであるNodeクラスのコンストラクタを呼びます。引数としてノード名を渡します。 ROS 1と同様に他ノードやツールでこのノードを操るときに利用する名です。

4行目でパブリッシャーを作成します。クラス定義で作った変数に保存します。（保存しないとコンストラクタが終了するとパブリッシャーは削除されます。）この変数の作成によりトピックが作成され、このノードからデータの送信が可能になります。 引数としてトピック名を指定します。この例では"greeting"を利用します。テンプレートパラメータは送信するメッセージ型です。パブリッシャーは決まっているメッセージ型しか送信できないので、インスタンス作成の時に指定しないとなりません。 std_msgs/Stringを送信するので、std_msgs::msg::Stringのクラスを示します。

6行目でタイマーを作成します。 ROS 1風のソースではノードの周期をrcpcpp::WallRateで指定しました。メーン関数はwhileループを利用して永遠に周りました。 ROS 2では実行時間の管理やリアルタイム実装のため等の理由で、そのような実装はおすすめではありません。代わりにタイマーを作成して、ROS 2のインフラストラクチャが管理できるタイマーイベントでノードの振る舞えを実行します。タイマー作成の引数は下記のようです。

1s

タイマーが実行する周期です。１秒ずつにタイマーイベントを生成します。
std::bind(&Greeter::broadcast_greeting, this)

スタンダードライブラリのbind関数でクラスのメンバー関数へのレフランスをファンクションポインターに変更します。１番目の引数はそのメンバー関数です。２番目以後の引数はその関数へ渡す引数です。クラスメンバー関数を利用するときは必ずthisを最初の引数として渡します。今回のメンバー関数は他の引数をいただかないのでこれで終わりです。

void Greeter::broadcast_greeting()
{
  auto greeting = std::make_shared<std_msgs::msg::String>();
  greeting->data = "hello world";
  RCLCPP_INFO(this->get_logger(), "Publishing greeting '%s'", greeting->data.c_str());
  pub_->publish(*greeting);
}

コンストラクタの後、唯一のもうひとつのメンバー関数を実装します。このノードは非常に簡単ので、デストラクタ、ムーブコンストラクタなどは要りません。

このメンバー関数はノードにあるタイマーによって実行されます。ノードの振る舞えを実装します。その振る舞えは単なるstd_msgs/Stringを送信することです。

ROS 1風と同様にメーセージを持つ変数を作成し、変数にデータを入れて、そしてパブリッシャーを利用して出力します。

#include "rclcpp_components/register_node_macro.hpp"

共有クラスとしてノードを実装すると、動的にノードをインスタンス化というフィーチャーが利用可能になります。する方法はここで説明しませんが、このノードは動的にロード可能になるために上記のヘッダーファイルをインクルードします。

RCLCPP_COMPONENTS_REGISTER_NODE(greeter_ros2_style::Greeter)

ファイルの最終行は下記の用です。これは上記で話した動的にノードをロードするフィーチャーを利用可能にするためです。コンポーネントノードの実装クラスを登録しています。

以上でコンポーネントノードの実装が完成です。次にノードをスタンドアローンとして利用するための実行ファイルを実装します。

スタンドアローンノードラッパーのソースファイル

スタンドアローンノードラッパーの実装は~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros2_style/src/greeter.cppにあります。エディターで開きましょう。

#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeter_ros2_style/greeter_component.hpp"

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  auto greeter = std::make_shared<greeter_ros2_style::Greeter>(options);
  rclcpp::spin(greeter);

  rclcpp::shutdown();
  return 0;
}

非常に簡単なソースです。残セクションで実装したコンポーネントノードのクラスをインスタンス化し、永遠のループを実行します。

4行目でコンポーネントノードの定義をインクルードします。

9行目でノードのインスタンスを作成します。 10行目でインスタンスはrclcpp::spinに渡します。 rclcpp::spinは永遠のループを実行しながらノードのタイマーイベント、メッセージ送信・受信等を行います。 Ctrl+cが押されることなどでノードはシャットダウンされるまでループします。

パッケージのメタ情報

パッケージのメタデータはROS 1風のパッケージからほとんど変わりません。 ~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros2_style/package.xmlをエディタで開きます。下記の2行（14行目と18行目）が追加だけです。

  <build_depend>rclcpp_components</build_depend>

  <exec_depend>rclcpp_components</exec_depend>

この行はコンポーネントノードを動的に利用するためです。

パッケージのコンパイル方法

package.xmlファイルと違って、ROS 1風のパッケージからコンパイル方法の変更点は多いです。 ~/ros2_basics/src/rosjp_ros2_basics/style_comparison/greeter_ros2_style/CMakeLists.txtをエディタで開きます。

cmake_minimum_required(VERSION 3.5)
# パッケージ名
project(greeter_ros2_style)

# C++14を利用する
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

# すべてのワーニングを表示する
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 依存するパッケージを探す
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)

include_directories(include)

# コンポーネントノードの共有ライブラリをコンパイルする
add_library(greeter_component SHARED src/greeter_component.cpp)
# コンポーネントノードのヘッダーファイルのマクロを設定する
target_compile_definitions(greeter_component PRIVATE "GREETER_BUILDING_DLL")
ament_target_dependencies(greeter_component
  rclcpp
  std_msgs
  rclcpp_components
  )
# コンポーネントノードをamentのリソースインデクスに登録する
rclcpp_register_node_plugins(greeter_component "greeter_ros2_style::Greeter")

# スタンドアローンノードの実行ファイルをコンパイルする
add_executable(greeter src/greeter.cpp)
# 共有ライブラリに実行ファイルをリンクする
target_link_libraries(greeter greeter_component)
# 実行ファイルのコンパイルターゲットに依存パッケージの情報を追加する
ament_target_dependencies(greeter rclcpp std_msgs)

# 本パッケージで依存するパッケージ情報をエキスポートする
ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rclcpp_components)
ament_export_dependencies(std_msgs)

# 本パッケージが提供するヘッダーファイルディレクトリ情報をエキスポートする
ament_export_include_directories(include)

# 本パッケージが提供するライブラリ情報をエキスポートする
ament_export_libraries(greeter_component)

# コンポーネントノードのヘッダーファイルをインストールする
install(DIRECTORY
  include/greeter_ros2_style
  DESTINATION include
  )

# コンポーネントノードの共有ライブラリをインストールする
install(TARGETS
  greeter_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

# スタンドアローンの実行ファイルをインストールする
install(TARGETS
  greeter
  DESTINATION lib/${PROJECT_NAME}
  )

# amentのリソースインデクスにパッケージを登録する
ament_package()

下記は変更点です。

18行　rclcpp_componentsをビルドタイムで利用するので、パッケージ情報を獲得します。
21行　コンポーネントノードの定義をインクルードするためにincludeディレクトリをインクルードパスに追加します。
24行　コンポーネントノードの共有ライブラリをコンパイルターゲットに追加します。
26行　GREETER_PUBLICマクロの内容を正しく設定するために、GREETER_BUILDING_DLLをコンパイルフラグに追加します。
27行〜31行　共有ライブラリが利用する他パッケージのライブラリパス、ヘッダーパス等をコンパイルターゲットに登録します。
33行　コンポーネントノードが利用できるようにcolconのリソースインデクスに登録します。
38行　ノードを利用する実行ファイルのコンパイルターゲットをノードの実装が入っている共有ライブラリにリンクします。
43行〜52行　他パッケージでコンポーネントノードが利用できるようにCMakeのターゲット情報等をエキスポートします。
55行〜58行　他パッケージでコンポーネントノードが利用できるようにノードを定義するヘッダーファイルをインストールします。
61行〜66行　他パッケージでコンポーネントノードが利用できるようにノードの実装が入っている共有ライブラリをインストールします。
69行〜72行　ros2 run などでパッケージから実行するためにノードの実行ファイルをインストールします。

上記の変更でこのパッケージは共有ライブラリもコンパイルし、パッケージにあるノードの再利用がより簡単になりました。

ビルド＆実行

ノードをコンパイルし、ノードのスタンドアローンラッパーで実行します。端末で下記を実行しパッケージをビルドします。

$ cd ~/ros2_basics/
$ colcon build --packages-select greeter_ros2_style

端末で下記を実行しノードを起動します。

$ source install/local_setup.bash
$ ros2 run greeter_ros2_style greeter
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'

上述が表示されれば成功です。

端末で Ctrl+c でノードを終了します。

メッセージ受信ノードの作成

本セクションでは、上記で実装したノードが出力するメッセージを受信するノードを実装します。

上記で記述したように、CまたはC++でノードを作成するとき、以下のステップを行います。

パッケージを作成し、package.xmlにパッケージのメタデータを記述します。
CMakeLists.txtを作成し、パッケージのコンパイル方法を記述します。
C++のソースファイルを作成しノードを実装します。

パッケージ作成

パッケージを持つためにワークスペース下のsrcディレクトリに新しいディレクトリを作成します。

$ cd ~/ros2_basics/src
$ mkdir displayer

パッケージディレクトリ内のpackage.xmlファイルを作成し、エディターで開いて下記の内容を入力します。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>displayer</name>
  <version>1.2.0</version>
  <description>ボブのメッセージ表示ノード</description>
  <maintainer email="bob@example.com">Geoffrey Biggs</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>rclcpp_components</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>rclcpp_components</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

コンパイル方法の指定

パッケージディレクトリ内のCMakeLists.txtファイルを作成し、エディターで開いて下記の内容を入力します。

cmake_minimum_required(VERSION 3.5)
project(displayer)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(class_loader REQUIRED)
find_package(std_msgs REQUIRED)

include_directories(include)

add_library(displayer_component SHARED src/displayer_component.cpp)
target_compile_definitions(displayer_component PRIVATE "DISPLAYER_BUILDING_DLL")
ament_target_dependencies(displayer_component
  rclcpp
  std_msgs
  rclcpp_components
  )
rclcpp_components_register_nodes(displayer_component "displayer::Displayer")

add_executable(displayer src/displayer.cpp)
target_link_libraries(displayer displayer_component)
ament_target_dependencies(displayer rclcpp std_msgs)

ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rclcpp_components)
ament_export_dependencies(greeting_msg)

ament_export_include_directories(include)

ament_export_libraries(displayer_component)

install(DIRECTORY
  include/displayer
  DESTINATION include
  )

install(TARGETS
  displayer_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

install(TARGETS
  displayer
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

ノードの実装

パッケージディレクトリでincludeとsrcディレクトリを作成します。 include内にdisplayerディレクトリを作成します。

$ cd ~/ros2_basics/src/displayer
$ mkdir src
$ mkdir -p include/displayer

include/displayer/内にdisplayer_component.hppファイルを作成し、エディターで開きます。内容は以下のようです。

#if !defined DISPLAYER__DISPLAYER_COMPONENT_HPP_
#define DISPLAYER__DISPLAYER_COMPONENT_HPP_

#if __cplusplus
extern "C" {
#endif

// The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
// demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define DISPLAYER_EXPORT __attribute__ ((dllexport))
    #define DISPLAYER_IMPORT __attribute__ ((dllimport))
  #else
    #define DISPLAYER_EXPORT __declspec(dllexport)
    #define DISPLAYER_IMPORT __declspec(dllimport)
  #endif
  #ifdef DISPLAYER_BUILDING_DLL
    #define DISPLAYER_PUBLIC DISPLAYER_EXPORT
  #else
    #define DISPLAYER_PUBLIC DISPLAYER_IMPORT
  #endif
  #define DISPLAYER_PUBLIC_TYPE DISPLAYER_PUBLIC
  #define DISPLAYER_LOCAL
#else
  #define DISPLAYER_EXPORT __attribute__ ((visibility("default")))
  #define DISPLAYER_IMPORT
  #if __GNUC__ >= 4
    #define DISPLAYER_PUBLIC __attribute__ ((visibility("default")))
    #define DISPLAYER_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define DISPLAYER_PUBLIC
    #define DISPLAYER_LOCAL
  #endif
  #define DISPLAYER_PUBLIC_TYPE
#endif

#if __cplusplus
} // extern "C"
#endif

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

namespace displayer_sample_version
{

class Displayer : public rclcpp::Node
{
public:
  DISPLAYER_PUBLIC explicit Displayer(const rclcpp::NodeOptions & options);

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;

  void display_greeting(const std_msgs::msg::String::SharedPtr msg);
};

} // namespace displayer_sample_version

#endif // DISPLAYER__DISPLAYER_COMPONENT_HPP_

次にパッケージのsrcディレクトリにdisplayer_component.cppファイルを作成し、内容を下記のようにします。

#include "displayer/displayer_component.hpp"

#include <chrono>
#include <memory>

using namespace std::chrono_literals;
using std::placeholders::_1;

namespace displayer_sample_version
{

Displayer::Displayer(const rclcpp::NodeOptions & options)
: Node("displayer", options)
{
  sub_ = this->create_subscription<std_msgs::msg::String>(
    "greeting", 10, std::bind(&Displayer::display_greeting, this, _1));
}

void Displayer::display_greeting(const std_msgs::msg::String::SharedPtr msg)
{
  RCLCPP_INFO(this->get_logger(), "Received greeting '%s'", msg->data.c_str());
}

} // namespace displayer_sample_version

#include "rclcpp_components/register_node_macro.hpp"

// 動的にコンポーネントノードをロードできるために登録する
RCLCPP_COMPONENTS_REGISTER_NODE(displayer_sample_version::Displayer)

前セクションのgreeter_ros2_styleノードの差は、タイマーがないこと及びパブリッシャーの変わりにサブスクライバーを利用しています。ノードの振る舞えはdisplay_greetingメンバー関数に実装され、トピックにメッセージが届くときに実行されます。

最後に、パッケージのsrcディレクトリにdisplayer.cppファイルを作成し、内容を下記のようにします。

#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "displayer/displayer_component.hpp"

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  rclcpp::spin(std::make_shared<displayer_sample_version::Displayer>(options));
  rclcpp::shutdown();
  return 0;
}

ビルド＆実行

新しいパッケージをコンパイルし、greeter_ros2_styleノードと一緒に利用します。

端末で下記を実行し作成したパッケージをコンパイルします。

$ cd ~/ros2_basics/
$ colcon build --packages-select displayer

または、下記の実行で全ワークスペースをコンパイルします。

$ cd ~/ros2_basics/
$ colcon build

端末で下記を実行しgreeter_ros2_styleノードを起動します。

$ source install/local_setup.bash
$ ros2 run greeter_ros2_style greeter
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'
[INFO] [greeter]: Publishing greeting 'hello world'

もう一つの端末を起動して、下記で自分で作成したメッセージ表示ノードを起動します。

$ cd ~/ros2_basics
$ source install/local_setup.bash
$ ros2 run displayer displayer
[INFO] [displayer]: Received greeting 'hello world'
[INFO] [displayer]: Received greeting 'hello world'
[INFO] [displayer]: Received greeting 'hello world'
[INFO] [displayer]: Received greeting 'hello world'

上述が表示されれば成功です。

端末で Ctrl+c でノードを終了します。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/receiving_messages/displayer_basic_version

独自メッセージ型の利用方法

上記のセクションでROS 2に含まれているメッセージ型を送信・受信するノードを作成しました。本セクションでは、独自のメッセージ型の定義方法及びそのメッセージ型の送信方法と受信方法を説明します。

独自メッセージ型の定義

ROS ２のメッセージ型定義フォーマットは主にROS 1と同様です。しかし、変更点と新フィーチャーはあります。詳細はROS 2のドキュメンテーションに参照してください。

メッセージ型を定義するパッケージを作成します。 ROS 2ではメッセージ型定義を別のパッケージに置くことがおすすめです。再利用製とメンテナンス性の向上になります。

まずはパッケージを作成しpackage.xmlを作成します。

$ cd ~/ros2_basics/src
$ mkdir greeting_msg

~/ros2_basics/src/greeting_msg/package.xmlを作成し下記の内容にします。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeting_msg</name>
  <version>1.1.0</version>
  <description>挨拶メッセージ型</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

本パッケージはrclcpp等利用しません。ビルドするときにメッセージを生成するだけです。

15行目はメッセージを定義するすべてのパッケージ（ノード等の他の内容があるパッケージも）がかならず含まないとなりません。パッケージフォーマットバージョン３の依存関係グループ定義という新しいフィーチャーであり、指定しないとcolconはパッケージをビルドしません。

次にパッケージのビルド方法を記述するために~/ros2_basics/src/greeting_msg/CMakeLists.txtを作成し下記の内容を入力します。

cmake_minimum_required(VERSION 3.5)
project(greeting_msg)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED) # メッセージ生成に必要

# メッセージ型定義ファイルからヘッダーファイル等を生成する
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Greeting.msg"
  )

ament_package()

前回から変わった行は下記のようです。

13行　メッセージ型のソースファイル等を生成するためにrosidl_default_generatorsというパッケージからのCMakeマクロとツールを利用します。
16行〜18行　メッセージ型定義ファイルからヘッダー等を生成するマクロをコールします。
20行　本パッケージが依存するパッケージのコンパイル情報をエキスポートします。

最後にメッセージ型定義ファイルを作成します。パッケージのトップディレクトリにmsgというディレクトリを作成し、その中にGreeting.msgというファイルを作成します。

$ cd ~/ros2_basics/src/greeting_msg
$ mkdir msg

~/ros2_basics/src/greeting_msg/msg/Greeting.msgの内容は下記にします。

string hello_text
string world_name
int16 count

greeting_msgパッケージはビルドできるか確認します。

$ cd ~/ros2_basics
$ colcon build --packages-select greeting_msg

これで新しいメッセージ型の定義ができました。次のセクションでdisplayerノードが利用するように変更します。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/custom_message_types/greeting_msg

独自メッセージ型の受信

独自のメッセージ型を利用するために、下記のステップが必要です。

package.xmlでメッセージ型を定義するパッケージに依存を追加します。
CMakeLists.txtでメッセージ型の依存を追加し、ノード等のコンパイルターゲットにリンクします。
メッセージ型のデータ型を利用するようにノードのソースを編集します。

まずは~/ros2_basics/src/displayer/package.xmlをエディターで開いて、下記の編集を行います。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>displayer</name>
  <version>1.1.0</version>
  <description>ボブのメッセージ表示ノード</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
<!---- ここから削除 ---->
  <build_depend>std_msgs</build_depend>
<!---- ここまで削除 ---->
  <build_depend>rclcpp_components</build_depend>
<!---- ここから追加 ---->
  <build_depend>greeting_msg</build_depend>
<!---- ここまで追加 ---->

  <exec_depend>rclcpp</exec_depend>
<!---- ここから削除 ---->
  <exec_depend>std_msgs</exec_depend>
<!---- ここまで削除 ---->
  <exec_depend>rclcpp_components</exec_depend>
<!---- ここから追加 ---->
  <exec_depend>greeting_msg</exec_depend>
<!---- ここまで追加 ---->

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

13行目あたりと17行目あたり、std_msgsへの依存を削除し、greeting_msgへの依存を追加します。

次に~/ros2_basics/src/displayer/CMakeLists.txtを開いて、下記の編集を行います。

cmake_minimum_required(VERSION 3.5)
project(displayer)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
####### ここから削除 #######
find_package(std_msgs REQUIRED)
####### ここまで削除 #######
####### ここから追加 #######
find_package(greeting_msg REQUIRED)
####### ここまで追加 #######

include_directories(include)

add_library(displayer_component SHARED src/displayer_component.cpp)
target_compile_definitions(displayer_component PRIVATE "DISPLAYER_BUILDING_DLL")
ament_target_dependencies(displayer_component
  rclcpp
  rclcpp_components
####### ここから削除 #######
  std_msgs
####### ここまで削除 #######
####### ここから追加 #######
  greeting_msg
####### ここまで追加 #######
  )
rclcpp_components_register_nodes(displayer_component "displayer::Displayer")

add_executable(displayer src/displayer.cpp)
target_link_libraries(displayer displayer_component)
####### ここから削除 #######
ament_target_dependencies(displayer rclcpp std_msgs)
####### ここまで削除 #######
####### ここから追加 #######
ament_target_dependencies(displayer rclcpp greeting_msg)
####### ここまで追加 #######

ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rclcpp_components)
ament_export_dependencies(greeting_msg)

ament_export_include_directories(include)

ament_export_libraries(displayer_component)

install(DIRECTORY
  include/displayer
  DESTINATION include
  )

install(TARGETS
  displayer_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

install(TARGETS
  displayer
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

４箇所にstd_msgsの利用を削除し、greeting_msgの利用を追加します。

これから新しいメッセージ型を利用するためにソースを編集します。

~/ros2_basics/src/displayer/include/displayer/displayer_component.hppというヘッダーファイルを開いて下記の変更を行います。

///// 省略 /////

#include <rclcpp/rclcpp.hpp>
/**** ここから削除 ****/
#include <std_msgs/msg/string.hpp>
/**** ここまで削除 ****/

/**** ここから追加 ****/
#include <greeting_msg/msg/greeting.hpp>
/**** ここまで追加 ****/

namespace displayer
{

class Displayer : public rclcpp::Node
{
public:
  DISPLAYER_PUBLIC
  Displayer();

private:
/**** ここから削除 ****/
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;

  void display_greeting(const std_msgs::msg::String::SharedPtr msg);
/**** ここまで削除 ****/

/**** ここから追加 ****/
  rclcpp::Subscription<greeting_msg::msg::Greeting>::SharedPtr sub_;

  void display_greeting(const greeting_msg::msg::Greeting::SharedPtr msg);
/**** ここまで追加 ****/
};

} // namespace displayer

#endif // DISPLAYER__DISPLAYER_COMPONENT_HPP_

43行でインクルードするファイルをstd_msgs/Stringのヘッダーからgreeting_msg/Greetingのヘッダーへ変更します。
54行からテンプレートパラメータが利用するデータ型を変更します。（２箇所）

~/ros2_basics/src/displayer/src/displayer_component.cppの変更点は下記のようです。

#include "displayer/displayer_component.hpp"

#include <chrono>
#include <memory>

using namespace std::chrono_literals;
using std::placeholders::_1;

namespace displayer
{

Displayer::Displayer()
: Node("displayer")
{
/**** ここから削除 ****/
  sub_ = this->create_subscription<std_msgs::msg::String>(
    "greeting", 10, std::bind(&Displayer::display_greeting, this, _1));
/**** ここまで削除 ****/

/**** ここから追加 ****/
  sub_ = this->create_subscription<greeting_msg::msg::Greeting>(
    "greeting", 10, std::bind(&Displayer::display_greeting, this, _1));
/**** ここまで追加 ****/
}

/**** ここから削除 ****/
void Displayer::display_greeting(const std_msgs::msg::String::SharedPtr msg)
{
  RCLCPP_INFO(this->get_logger(), "Received greeting '%s'", msg->data.c_str());
}
/**** ここまで削除 ****/

/**** ここから追加 ****/
void Displayer::display_greeting(const greeting_msg::msg::Greeting::SharedPtr msg)
{
  RCLCPP_INFO(
    this->get_logger(), "Received greeting '%s %s %d'",
    msg->hello_text.c_str(), msg->world_name.c_str(), msg->count);
}
/**** ここまで追加 ****/

} // namespace displayer

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(displayer::Displayer)

15行でサブスクライバーが利用するデータ型を変更します。
19行からのコールバック関数で、引数のデータ型を変更します。受信したデータの利用するソースも変更します。

~/ros2_basics/src/displayer/src/displayer.cppのノードラッパー実装には変更はありません。

確認するために、displayerパッケージはビルドできるか確認します。

$ cd ~/ros2_basics
$ colcon build --packages-select displayer

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/custom_message_types/displayer

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select displayer_custom_msg
$ ros2 run displayer displayer_custom_msg

独自メッセージ型の送信

最後に、独自データ型を利用して挨拶を送るノードを作成します。

greeterというパッケージを作成し、下記のファイルを入れます。

$ cd ~/ros2_basics/src
$ mkdir -p greeter/include/greeter greeter/src

~/ros2_basics/src/greeter/package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeter</name>
  <version>1.2.0</version>
  <description>アイサツくん</description>
  <maintainer email="bob@example.com">Geoffrey Biggs</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>rclcpp_components</build_depend>
  <build_depend>greeting_msg</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclcpp_components</exec_depend>
  <exec_depend>greeting_msg</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

~/ros2_basics/src/greeter/CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(greeter)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(greeting_msg REQUIRED)

include_directories(include)

add_library(greeter_component SHARED src/greeter_component.cpp)
target_compile_definitions(greeter_component PRIVATE "GREETER_BUILDING_DLL")
ament_target_dependencies(greeter_component
  rclcpp
  rclcpp_components
  greeting_msg
  )
rclcpp_components_register_nodes(greeter_component "greeter::Greeter")

add_executable(greeter src/greeter.cpp)
target_link_libraries(greeter greeter_component)
ament_target_dependencies(greeter rclcpp rclcpp_components greeting_msg)

ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rclcpp_components)
ament_export_dependencies(greeting_msg)

ament_export_include_directories(include)

ament_export_libraries(greeter_component)

install(DIRECTORY
  include/greeter
  DESTINATION include
  )

install(TARGETS
  greeter_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

install(TARGETS
  greeter
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

~/ros2_basics/src/greeter/include/greeter/greeter_component.hpp:

#if !defined GREETER__GREETER_COMPONENT_HPP_
#define GREETER__GREETER_COMPONENT_HPP_

#if __cplusplus
extern "C" {
#endif

// The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
// demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define GREETER_EXPORT __attribute__ ((dllexport))
    #define GREETER_IMPORT __attribute__ ((dllimport))
  #else
    #define GREETER_EXPORT __declspec(dllexport)
    #define GREETER_IMPORT __declspec(dllimport)
  #endif
  #ifdef GREETER_BUILDING_DLL
    #define GREETER_PUBLIC GREETER_EXPORT
  #else
    #define GREETER_PUBLIC GREETER_IMPORT
  #endif
  #define GREETER_PUBLIC_TYPE GREETER_PUBLIC
  #define GREETER_LOCAL
#else
  #define GREETER_EXPORT __attribute__ ((visibility("default")))
  #define GREETER_IMPORT
  #if __GNUC__ >= 4
    #define GREETER_PUBLIC __attribute__ ((visibility("default")))
    #define GREETER_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define GREETER_PUBLIC
    #define GREETER_LOCAL
  #endif
  #define GREETER_PUBLIC_TYPE
#endif

#if __cplusplus
} // extern "C"
#endif

#include <rclcpp/rclcpp.hpp>
#include <greeting_msg/msg/greeting.hpp>

namespace greeter
{

class Greeter : public rclcpp::Node
{
public:
  GREETER_PUBLIC explicit Greeter(const rclcpp::NodeOptions & options);

private:
  rclcpp::Publisher<greeting_msg::msg::Greeting>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  greeting_msg::msg::Greeting::SharedPtr greeting_;

  void broadcast_greeting();
};

} // namespace greeter

#endif // GREETER__GREETER_COMPONENT_HPP_

~/ros2_basics/src/greeter/src/greeter_component.cpp:

#include "greeter/greeter_component.hpp"

#include <class_loader/register_macro.hpp>
#include <rosidl_generator_cpp/message_initialization.hpp>
#include <chrono>
#include <memory>

using namespace std::chrono_literals;

namespace greeter
{

Greeter::Greeter(const rclcpp::NodeOptions & options)
: Node("greeter", options)
{
  pub_ = create_publisher<greeting_msg::msg::Greeting>("greeting", 10);

  timer_ = create_wall_timer(1s, std::bind(&Greeter::broadcast_greeting, this));

  greeting_ = std::make_shared<greeting_msg::msg::Greeting>(rosidl_generator_cpp::MessageInitialization::ZERO);
  greeting_->hello_text = "hello";
  greeting_->world_name = "world";
}

void Greeter::broadcast_greeting()
{
  RCLCPP_INFO(
      this->get_logger(),
      "Publishing greeting '%s %s %d'",
      greeting_->hello_text.c_str(),
      greeting_->world_name.c_str(),
      greeting_->count);
  pub_->publish(*greeting_);
  ++greeting_->count;
}

} // namespace greeter

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(greeter::Greeter)

メッセージを保存するための変数の作成（20行目）で利用するMessageInitialization::ZEROは、変数作成時にデータをゼロに設定すると指定します。 ALL、SKIPとDEFAULTS_ONLYという選択もあります。

~/ros2_basics/src/greeter/src/greeter.cpp:

#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeter/greeter_component.hpp"

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  rclcpp::spin(std::make_shared<greeter::Greeter>(options));
  rclcpp::shutdown();
}

上記のように、独自メッセージ型をそのためのパッケージに入れるとstd_msgsと同様に簡単に利用できます。

ビルドとテスト

端末で下記を実行し全ワークスペースをコンパイルします。

$ cd ~/ros2_basics/
$ colcon build

端末で下記を実行しgreeterノードを起動します。

$ source install/local_setup.bash
$ ros2 run greeter greeter
[INFO] [greeter]: Publishing greeting 'hello world 0'
[INFO] [greeter]: Publishing greeting 'hello world 1'
[INFO] [greeter]: Publishing greeting 'hello world 2'
[INFO] [greeter]: Publishing greeting 'hello world 3'
[INFO] [greeter]: Publishing greeting 'hello world 4'
[INFO] [greeter]: Publishing greeting 'hello world 5'

もう一つの端末を起動して、下記でdisplayerノードを起動します。

$ cd ~/ros2_basics
$ source install/local_setup.bash
$ ros2 run displayer displayer
[INFO] [displayer]: Received greeting 'hello world 2'
[INFO] [displayer]: Received greeting 'hello world 3'
[INFO] [displayer]: Received greeting 'hello world 4'
[INFO] [displayer]: Received greeting 'hello world 5'

上述が表示されれば成功です。

端末で Ctrl+c でノードを終了します。

他のメッセージ定義の例は、std_msgsパッケージやsensor_msgsパッケージに見えます。 Linuxの場合にstd_msgsは/opt/ros/ardent/share/std_msgsにあります。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/custom_message_types/greeter

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greeter_custom_msg
$ ros2 run greeter_custom_msg greeter

ノードのコンポジション

「ROS 2のノードの書き方を理解」で説明したように、ノードを共有ライブラリとして実装するとROS 1のノードレットと同様に複数のノードを同一プロセスに入れられます。

ROS 2 node composition

ノードコンポジションの利用には主に２つの利点があります。

ノードの実行順番を厳密に守ることができます。特にリアルタイム制御ループを複数のノードで実装するとこの機能は役に立ちますが、一般的なセンサーデータ処理チェーンにも役に立ちます。
ノードとノードの間のデータ通信をゼロコピーにすることは可能になります。

本セクションで、greeterノードとdisplayerノードを一つのプロセスに入れる方法を説明します。

コンポーネントノードを利用する方法は下記を含めて複数あります。

新しい実行ファイルを作成し、複数のコンポーネントノードを主導で集めること
このサンプルのような特別なコンテナノードを作成し、load_nodeサービスを利用別のノードを作成し、そのノードからコンテナノードにほしいコンポーネントノードをロードすること
上記のコンテナノードとこのサンプルのようなコマンドラインツールを利用して端末またはスクリプトからノードをロードすること

本セクションはオプション１を説明します。

コンポジットノードを持つパッケージを作成します。

$ cd ~/ros2_basics/src
$ mkdir -p greet_and_displayer/src

ソース作成

/ros2_basics/src/greet_and_displayer/package.xmlに下記のXMLを入力します。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greet_and_displayer</name>
  <version>1.1.0</version>
  <description>自己アイサツくん</description>
  <maintainer email="bob@example.com">Geoffrey Biggs</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>greeter</build_depend>
  <build_depend>displayer</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>greeter</exec_depend>
  <exec_depend>displayer</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

依存関係パッケージに統合するつもりのgreeterとdisplayerノードのパッケージを指定します。

/ros2_basics/src/greet_and_displayer/CMakeLists.txtに下記のソースを入力します。

cmake_minimum_required(VERSION 3.5)
project(greet_and_displayer)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(greeter REQUIRED)
find_package(displayer REQUIRED)

# 他のノードを統合するノードをコンパイルする
add_executable(greet_and_displayer src/greet_and_displayer.cpp)
ament_target_dependencies(greet_and_displayer
  rclcpp
  greeter
  displayer
  )

install(TARGETS
  greet_and_displayer
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

単なるノードの実行ファイルをコンパイルしますが、統合するノードを利用します。

そして最後に、/ros2_basics/src/greet_and_displayer/src/greet_and_displayer.cppを下記のようにします。

#include <rclcpp/rclcpp.hpp>
#include <memory>

#include <greeter/greeter_component.hpp>
#include <displayer/displayer_component.hpp>

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;

  // タイマーコールバック、トピックコールバック等を行うexecutor
  rclcpp::executors::SingleThreadedExecutor exec;

  // Greeterコンポーネントノードのインスタンスを作成しexecutorに登録する
  auto greeter = std::make_shared<greeter::Greeter>(options);
  exec.add_node(greeter);
  // Displayerコンポーネントノードのインスタンスを作成しexecutorに登録する
  auto displayer = std::make_shared<displayer::Displayer>(options);
  exec.add_node(displayer);

  // Ctrl-Cが押されるまでexecutorを実行する
  exec.spin();

  // Shut down ROS
  rclcpp::shutdown();
  return 0;
}

新しいソースを説明します。

  rclcpp::executors::SingleThreadedExecutor exec;

この行はexecutorを作成します。

ROS 2では、すべてのノードはexecutorというオブジェクトの中で実行します。タイマーイベント、データ受信コールバック、サービス等の実行は必ずexecutorが管理します。今まで見たソースにはexecutorというオブジェクトは見えなかったが、rclcpp::spin等の後ろで利用されていました。

Executorは直接利用することが可能です。むしろROS 2をより柔軟的に利用したければ、利用するべきです。

シングルスレッドやマルチスレッド等、複数のexecutorタイプがすでにあります。下記はシングルスレッドexecutorがノードのイベントを実行する手法です。

ROS 2 single-threaded executor

マルチスレッドexecutorを利用するとノードのイベントを下記のように実行します。（２スレッドを利用する例です。）

ROS 2 multi-threaded executor

ユーザにより新しいexecutorの種類は実装可能です。何かの実行方法が必要であれば、executorの実装によって実現できます。

  auto greeter = std::make_shared<greeter::Greeter>(options);

greeterコンポーネントノードのインスタンスを作成します。

  exec.add_node(greeter);

Executorにgreeterノードインスタンスを登録します。これでgreeterはexecから実行時間をもらい、タイマーイベントが実行されます。

  auto displayer = std::make_shared<displayer::Displayer>(options);
  exec.add_node(displayer);

displayerコンポーネントノードを同様に作成しexecutorに登録します。

  exec.spin();

execを実行し、executorがループを回し始まるようにします。 rclcpp::spin()ど類似です。

ビルド＆実行

下記でノードをコンパイルします。

$ cd ~/ros2_basics
$ colcon build --packages-select greet_and_displayer

実行すると下記のような出力があります。

$ ros2 run greet_and_displayer greet_and_displayer
[INFO] [greeter]: Publishing greeting 'hello world 0'
[INFO] [displayer]: Received greeting 'hello world 0'
[INFO] [greeter]: Publishing greeting 'hello world 1'
[INFO] [displayer]: Received greeting 'hello world 1'
[INFO] [greeter]: Publishing greeting 'hello world 2'
[INFO] [displayer]: Received greeting 'hello world 2'
[INFO] [greeter]: Publishing greeting 'hello world 3'
[INFO] [displayer]: Received greeting 'hello world 3'

端末で Ctrl+c でノードを終了します。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/custom_message_types/greet_and_displayer

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greet_and_displayer_custom_msg
$ ros2 run greet_and_displayer_custom_msg greet_and_displayer

サービスの定義と利用

ROS 2にはROS 1と同様にサービスという概念があります。 ROS ２のサービス型定義フォーマットはROS 1と同様にメッセージ定義フォーマットを利用します。詳細はROS 2のドキュメンテーションに参照してください。

本セクションにサービスの定義方法と利用方法を説明します。

サービスの定義

独自メッセージ型と同様に、専用パッケージの作成がおすすめです。

$ cd ~/ros2_basics/src
$ mkdir request_greeting_service

~/ros2_basics/src/request_greeting_service/package.xmlにパッケージのメタデータを記述します。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>request_greeting_service</name>
  <version>1.1.0</version>
  <description>挨拶サービス型</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

メッセージ型を定義するパッケージと一緒です。

~/ros2_basics/src/request_greeting_service/CMakeLists.txtにパッケージのビルド方法を記述します。

cmake_minimum_required(VERSION 3.5)
project(request_greeting_service)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)

# サービス定義からヘッダーファイル等を生成する
rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/RequestGreeting.srv"
  )

ament_export_dependencies(rosidl_default_runtime)

ament_package()

これもメッセージ型を定義するパッケージと一緒です。

最後にサービス型定義ファイルを作成します。パッケージのトップディレクトリにsrvというディレクトリを作成し、その中にRequestGreeting.srvというファイルを作成します。

$ cd ~/ros2_basics/src/request_greeting_service
$ mkdir srv

~/ros2_basics/src/request_greeting_service/srv/RequestGreeting.srvの内容は下記にします。

string name
---
string greeting

request_greeting_serviceパッケージはビルドできるか確認します。

$ cd ~/ros2_basics
$ colcon build --packages-select request_greeting_service

これで新しいサービス型の定義ができました。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/custom_message_type/request_greeting_service

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select request_greeting_service

サービスの提供するノードの作成

サービス型を利用してサービスを提供するノードを作成します。

新しいパッケージを作成し、下記のファイルを入れます。

$ cd ~/ros2_basics/src
$ mkdir -p greeting_server/include/greeting_server greeting_server/src

~/ros2_basics/src/greeting_server/package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeting_server</name>
  <version>1.1.0</version>
  <description>アイサツくんサービス</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>rclcpp_components</build_depend>
  <build_depend>request_greeting_service</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclcpp_components</exec_depend>
  <exec_depend>request_greeting_service</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

~/ros2_basics/src/greeting_server/CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(greeting_server)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(request_greeting_service REQUIRED)

include_directories(include)

add_library(greeting_server_component SHARED src/greeting_server_component.cpp)
target_compile_definitions(greeting_server_component PRIVATE "GREETING_SERVER_BUILDING_DLL")
ament_target_dependencies(greeting_server_component
  rclcpp
  rclcpp_components
  request_greeting_service
  )
rclcpp_components_register_nodes(greeting_server_component "greeting_server::GreetingServer")

add_executable(greeting_server src/greeting_server.cpp)
target_link_libraries(greeting_server greeting_server_component)
ament_target_dependencies(greeting_server rclcpp request_greeting_service)

ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rclcpp_components)
ament_export_dependencies(request_greeting_service)

ament_export_include_directories(include)

ament_export_libraries(greeting_server_component)

install(DIRECTORY
  include/greeting_server
  DESTINATION include
  )

install(TARGETS
  greeting_server_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

install(TARGETS
  greeting_server
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

~/ros2_basics/src/greeting_server/src/greeting_server.cpp:

#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeting_server/greeting_server_component.hpp"

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  rclcpp::spin(std::make_shared<greeting_server::GreetingServer>(options));
  rclcpp::shutdown();
}

これからサービスを提供するコンポーネントノードを実装します。まずはヘッダーファイルを作成します。エディターで~/ros2_basics/src/greeting_server/include/greeting_server/greeting_server_component.hppを開いて下記を入れます。

#if !defined GREETING_SERVER__GREETING_SERVER_COMPONENT_HPP_
#define GREETING_SERVER__GREETING_SERVER_COMPONENT_HPP_

#if __cplusplus
extern "C" {
#endif

// The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
// demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define GREETING_SERVER_EXPORT __attribute__ ((dllexport))
    #define GREETING_SERVER_IMPORT __attribute__ ((dllimport))
  #else
    #define GREETING_SERVER_EXPORT __declspec(dllexport)
    #define GREETING_SERVER_IMPORT __declspec(dllimport)
  #endif
  #ifdef GREETING_SERVER_BUILDING_DLL
    #define GREETING_SERVER_PUBLIC GREETING_SERVER_EXPORT
  #else
    #define GREETING_SERVER_PUBLIC GREETING_SERVER_IMPORT
  #endif
  #define GREETING_SERVER_PUBLIC_TYPE GREETING_SERVER_PUBLIC
  #define GREETING_SERVER_LOCAL
#else
  #define GREETING_SERVER_EXPORT __attribute__ ((visibility("default")))
  #define GREETING_SERVER_IMPORT
  #if __GNUC__ >= 4
    #define GREETING_SERVER_PUBLIC __attribute__ ((visibility("default")))
    #define GREETING_SERVER_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define GREETING_SERVER_PUBLIC
    #define GREETING_SERVER_LOCAL
  #endif
  #define GREETING_SERVER_PUBLIC_TYPE
#endif

#if __cplusplus
} // extern "C"
#endif

#include <rclcpp/rclcpp.hpp>
#include <request_greeting_service/srv/request_greeting.hpp>
#include <memory>

namespace greeting_server
{

using RequestGreeting = request_greeting_service::srv::RequestGreeting;

class GreetingServer : public rclcpp::Node
{
public:
  GREETING_SERVER_PUBLIC explicit GreetingServer(const rclcpp::NodeOptions & options);

private:
  rclcpp::Service<RequestGreeting>::SharedPtr server_;

  void send_greeting(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<RequestGreeting::Request> request,
    const std::shared_ptr<RequestGreeting::Response> response);
};

} // namespace greeting_server

#endif // GREETING_SERVER__GREETING_SERVER_COMPONENT_HPP_

ヘッダーの内容は主に他の作成したコンポーネントノードと同様です。変更点は下記のようです。

43行　サービスのデータ型のヘッダーファイルをインクルードします。
49行　サービスのデータ型のタイプ名は長いので、usingでアリアスを定義します。
57行　サービスを提供するサーバオブジェクトを保存する変数です。
59行〜62行　サービスコールバックを定義します。 1番目の引数はサービスリクエストを区別するためのへっだーです。 2番目の引数はクライアントからのリクエストデータです。 3番目の引数はクライアントに送る返事の置く場所です。

そして、サービスを提供するコンポーネントノードを実装します。エディターで~/ros2_basics/src/greeting_server/src/greeting_server_component.cppを開いて下記を入れます。

#include "greeting_server/greeting_server_component.hpp"

namespace greeting_server
{

GreetingServer::GreetingServer(const rclcpp::NodeOptions & options)
: Node("greeting_server", options)
{
  using std::placeholders::_1;
  using std::placeholders::_2;
  using std::placeholders::_3;

  server_ = this->create_service<RequestGreeting>(
    "request_greeting",
    std::bind(&GreetingServer::send_greeting, this, _1, _2, _3));
}

void GreetingServer::send_greeting(
  const std::shared_ptr<rmw_request_id_t> request_header,
  const std::shared_ptr<RequestGreeting::Request> request,
  const std::shared_ptr<RequestGreeting::Response> response)
{
  (void)request_header;
  response->greeting = "Hello, " + request->name;
  RCLCPP_INFO(
    this->get_logger(),
    "Responding to greeting request with '%s'",
    response->greeting.c_str());
}

} // namespace greeting_server

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(greeting_server::GreetingServer)

他の実装したコンポーネントノードの実装からの変更点は下記のようです。

9行〜11行　std::bindを利用するときに渡すメンバー関数は３つの引数を使うので、それを指定するためのオブジェクトタイプをusingします。
13行〜15行　サーバーインスタンスを作成します。サービス名をrequest_greetingにします。リクエストが届くときに呼ぶサービスコールバック関数はGreetingServer::send_greetingです。クラスポインター以外３つの引数を渡すことが必要です。
18行〜29行　サービスコーロバックの実装です。
24行　サービスリクエストにあるデータを利用して返事を作成します。
25行〜18行　ノードの振る舞えが明確になるようにログを出力します。

これでサービスを提供するノードを作成しました。ビルドできるかどうか確認します。

$ cd ~/ros2_basics
$ colcon build --packages-select greeting_server

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/services/greeting_server

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_server
$ ros2 run greeting_server greeting_server

サービスの利用するノードの作成

サービスを利用するノードも作成します。新しいパッケージを作成し、下記のファイルを入れます。

$ cd ~/ros2_basics/src
$ mkdir -p greeting_client/src

~/ros2_basics/src/greeting_client/package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeting_client</name>
  <version>1.1.0</version>
  <description>アイサツくんクライアント</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>request_greeting_service</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>request_greeting_service</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

~/ros2_basics/src/greeting_client/CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(greeting_client)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(request_greeting_service REQUIRED)

add_executable(greeting_client src/greeting_client.cpp)
ament_target_dependencies(greeting_client rclcpp request_greeting_service)

install(TARGETS
  greeting_client
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

サービスクライアントノードを実装します。 ~/ros2_basics/src/greeting_client/src/greeting_client.cppをエディターで開いて下記の内容を入力します。

#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <memory>
#include <request_greeting_service/srv/request_greeting.hpp>

using namespace std::chrono_literals;
using RequestGreeting = request_greeting_service::srv::RequestGreeting;

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("greeting_client");

  // サービスクライントを作成する
  auto client = node->create_client<RequestGreeting>("request_greeting");

  // サービスが提供されているまでに待つ
  while (!client->wait_for_service(1s)) {
    // シャットダウンされたかどうか確認する
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service.");
      rclcpp::shutdown();
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "Waiting for service...");
  }

  // サーバーに送るリクエストを作成する
  auto request = std::make_shared<RequestGreeting::Request>();
  request->name = "Bob";
  // サーバーにリクエストを送る（非同期）
  auto result = client->async_send_request(request);
  // 結果を待つ
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(node->get_logger(), "Received greeting: '%s'", result.get()->greeting.c_str());
  } else {
    RCLCPP_ERROR(node->get_logger(), "Problem while waiting for response.");
  }

  rclcpp::shutdown();
  return 0;
}

上記のノードはサービスを一回利用してシャットダウンしますします。

サービスクライアントは14行で作成します。

  auto client = node->create_client<RequestGreeting>("request_greeting");

クライント作成のテンプレートパラメータはサービスのデータ型です。引数はサービス名です。この例ではサービスは/request_greetingに提供されているとの前提で実装しています。

17行から25行でサービスが提供されるまで待ちます。タイムアウトは１秒ので、１秒づつにノードがシャットダウンされたか確認します。

サーバーにリクエストを送ることは27行から39行です。

  auto request = std::make_shared<RequestGreeting::Request>();

リクエストオブジェクトを作成します。サービス定義からRequestとResponseという２つのデータ型が生成されます。 Requestはクライントがサーバーに送ります。サービス定義の前半（---の前の部分）です。サーバーからの返事はResponseで帰ってきます。サービス定義の後半（---の後の部分）です。

  request->name = "Bob";

リクエストにデータを入れます。サービスによってデータが必要かどうか変わります。

  auto result = client->async_send_request(request);

サーバーにリクエストを送ります。サービスコールは非同期のでこの関数はすぐに終了します。返事を持つためのオブジェクトを保存します。

  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(node->get_logger(), "Received greeting: '%s'", result.get()->greeting.c_str());
  } else {
    RCLCPP_ERROR(node->get_logger(), "Problem while waiting for response.");
  }

サービスコールは非同期ので、結果が来るまでspinします。 async_send_requestが出した結果オブジェクトのデータ型はサービスのResponseではなくて、C++11のFutureという新しいデータ型です。非同期の何かの終わることを持つために利用します。 ROS 2で広く利用されています。 rclcpp::spin_until_future_completeは、Futureオブジェクトが示す活動の終了を待ちながらノードをspinするためのAPIです。ここではサーバーから結果がくるまで待つという意味になります。

    RCLCPP_INFO(node->get_logger(), "Received greeting: '%s'", result.get()->greeting.c_str());

ここで結果を利用します。 Responseオブジェクトはresult.get()でアクセスできます。

結果またはエラーが出たら、ノードは終了します。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/services/greeting_client

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_client
$ ros2 run greeting_client greeting_client

ビルドとテスト

端末で下記を実行し全ワークスペースをコンパイルします。

$ cd ~/ros2_basics/
$ colcon build

端末で下記を実行しgreeting_serverノードを起動します。

$ source install/local_setup.bash
$ ros2 run greeting_server greeting_server

もう一つの端末を起動して、下記でgreeting_clientを実行しサービスを利用します。

$ cd ~/ros2_basics
$ source install/local_setup.bash
$ ros2 run greeting_client greeting_client
[INFO] [greeting_client]: Received greeting: 'Hello, Bob'
$

他のサービス定義はstd_srvsパッケージに見えます。 Linuxの場合に、std_srvsは/opt/ros/dashing/share/std_srvsにインストールされています。

アクションの定義と利用

アクションはROSの３つの通信方法の３番目です。 ROS 1と同様にアクションは時間がかかるタスクを実行・管理するための通信方法です。サービスと同様にサーバーとクライアントが必要ですが、サービスと違って、アクションは必ず非同期に実行されます。そしてサービスと違って、実行中にステータス情報の取得や実行のキャンセルは可能です。

アクションは、クライアントからのリクエストに対してサーバーが何らかのタスクを実行し始めます。実行中、サーバーはクライアントにそのタスクの実行ステータスについて定期的に連絡します。そしてタスクが完成されたらクライアントに実行の結果を伝えます。結果は成功、失敗、エラー等に加えて地図等のような作成した情報も伝えることはあります。もちろん、タスクの結果は何かの情報ではなくて、冷蔵庫からビールを取ること等のような実世界でのタスクも可能です。実行中、クライントはもうそのタスクの結果がいらないと判断したら、実行のキャンセルも可能です。アクションのステートマシンは下記の図に表示されます。

Action state machine

アクションの詳細はROS 2の設計文書に参照してください。

本セクションでは、アクションの定義方法、アクションサーバーの作成方法、及びアクションクライアントの作成方法を説明します。

アクションの定義

アクションの定義は、メッセージとサービスと同様なフォーマットを利用します。

アクションの定義は下記の３セクションで構造されています。

Goal: アクションが果たすことです。実世界で場所に移動することなどのタスクも、データから地図を作成してクライントへ返すような時間がかかる処理タスクも可能です。
Result: 成功でも失敗でも、アクションの実行が完成した後、要求したクライントへ返す結果データです。作成した地図や、移動タスク後のロボットがいる位置等です。
Feedback: アクションが実行中に実行を要求したクライントへ伝える状況の情報です。データ処理の何割が完成や、目的地まで後何キロメートル等のような進捗情報が一般です。

アクションを定義します。メッセージとサービスと同様に専用のパッケージの作成がおすすめです。

$ cd ~/ros2_basics/src
$ mkdir greeting_actions

~/ros2_basics/src/greeting_actions/package.xmlにパッケージのメタデータを記述します。サービス型を定義するパッケージと一緒です。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeting_actions</name>
  <version>1.0.0</version>
  <description>挨拶アクション型</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <depend>action_msgs</depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

~/ros2_basics/src/greeting_actions/CMakeLists.txtにパッケージのビルド方法を記述します。これもサービス型を定義するパッケージと一緒です。

cmake_minimum_required(VERSION 3.5)
project(greeting_actions)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(action_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

# アクション定義からヘッダーファイル等を生成する
rosidl_generate_interfaces(${PROJECT_NAME}
  "action/ProcessGreeting.action"
  DEPENDENCIES action_msgs
  )

ament_export_dependencies(rosidl_default_runtime)

ament_package()

最後にアクション型定義ファイルを作成します。パッケージのトップディレクトリにactionというディレクトリを作成し、その中にProcessGreeting.actionというファイルを作成します。

$ cd ~/ros2_basics/src/greeting_actions
$ mkdir action

~/ros2_basics/src/greeting_actions/action/ProcessGreeting.actionの内容は下記にします。

# Goal
string name
---
# Result
string greeting
---
# Feedback
uint8 percent_complete

greeting_actionsパッケージはビルドできるか確認します。

$ cd ~/ros2_basics
$ colcon build --packages-select greeting_actions

これで新しいアクション型の定義ができました。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/greeting_actions

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_actions

アクションサーバーノードの作成

定義したアクション型を利用してアクションを提供するノードを作成します。

新しいパッケージを作成し、下記のファイルを入れます。

$ cd ~/ros2_basics/src
$ mkdir -p greeting_processor/include/greeting_processor greeting_processor/src

~/ros2_basics/src/greeting_processor/package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greeting_processor</name>
  <version>1.0.0</version>
  <description>アイサツくんアクション</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>rclcpp_action</build_depend>
  <build_depend>rclcpp_components</build_depend>
  <build_depend>greeting_actions</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclcpp_action</exec_depend>
  <exec_depend>rclcpp_components</exec_depend>
  <exec_depend>greeting_actions</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

アクション機能を提供するライブラリはrclcppと別ので、13行目と１８行目でアクション機能を提供するパッケージに依存します。

~/ros2_basics/src/greeting_processor/CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(greeting_processor)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(greeting_actions REQUIRED)

include_directories(include)

add_library(greeting_processor_component SHARED src/greeting_processor_component.cpp)
target_compile_definitions(greeting_processor_component PRIVATE "GREETING_PROCESSOR_BUILDING_DLL")
ament_target_dependencies(greeting_processor_component
  rclcpp
  rclcpp_action
  rclcpp_components
  greeting_actions
  )
rclcpp_components_register_nodes(greeting_processor_component "greeting_processor::GreetingProcessor")

add_executable(greeting_processor src/greeting_processor.cpp)
target_link_libraries(greeting_processor greeting_processor_component)
ament_target_dependencies(greeting_processor rclcpp rclcpp_action greeting_actions)

ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rclcpp_action)
ament_export_dependencies(rclcpp_components)
ament_export_dependencies(greeting_actions)

ament_export_include_directories(include)

ament_export_libraries(greeting_processor_component)

install(DIRECTORY
  include/greeting_processor
  DESTINATION include
  )

install(TARGETS
  greeting_processor_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

install(TARGETS
  greeting_processor
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

~/ros2_basics/src/greeting_processor/src/greeting_processor.cpp:

#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeting_processor/greeting_processor_component.hpp"

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<greeting_processor::GreetingProcessor>();
  node->make_action_server();
  rclcpp::spin(node);
  rclcpp::shutdown();
}

これからアクションを提供するコンポーネントノードを実装します。まずはヘッダーファイルを作成します。エディターで~/ros2_basics/src/greeting_processor/include/greeting_processor/greeting_processor_component.hppを開いて下記を入れます。

#if !defined GREETING_PROCESSOR__GREETING_PROCESSOR_COMPONENT_HPP_
#define GREETING_PROCESSOR__GREETING_PROCESSOR_COMPONENT_HPP_

#if __cplusplus
extern "C" {
#endif

// The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
// demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define GREETING_PROCESSOR_EXPORT __attribute__ ((dllexport))
    #define GREETING_PROCESSOR_IMPORT __attribute__ ((dllimport))
  #else
    #define GREETING_PROCESSOR_EXPORT __declspec(dllexport)
    #define GREETING_PROCESSOR_IMPORT __declspec(dllimport)
  #endif
  #ifdef GREETING_PROCESSOR_BUILDING_DLL
    #define GREETING_PROCESSOR_PUBLIC GREETING_PROCESSOR_EXPORT
  #else
    #define GREETING_PROCESSOR_PUBLIC GREETING_PROCESSOR_IMPORT
  #endif
  #define GREETING_PROCESSOR_PUBLIC_TYPE GREETING_PROCESSOR_PUBLIC
  #define GREETING_PROCESSOR_LOCAL
#else
  #define GREETING_PROCESSOR_EXPORT __attribute__ ((visibility("default")))
  #define GREETING_PROCESSOR_IMPORT
  #if __GNUC__ >= 4
    #define GREETING_PROCESSOR_PUBLIC __attribute__ ((visibility("default")))
    #define GREETING_PROCESSOR_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define GREETING_PROCESSOR_PUBLIC
    #define GREETING_PROCESSOR_LOCAL
  #endif
  #define GREETING_PROCESSOR_PUBLIC_TYPE
#endif

#if __cplusplus
} // extern "C"
#endif

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <greeting_actions/action/process_greeting.hpp>
#include <memory>

namespace greeting_processor
{

using ProcessGreeting = greeting_actions::action::ProcessGreeting;

class GreetingProcessor : public rclcpp::Node
{
public:
  GREETING_PROCESSOR_PUBLIC explicit GreetingProcessor(const rclcpp::NodeOptions & options);

  void make_action_server();

private:
  rclcpp_action::Server<ProcessGreeting>::SharedPtr server_;

  rclcpp_action::GoalResponse handle_goal(
    const std::array<uint8_t, 16> & uuid,
    std::shared_ptr<const ProcessGreeting::Goal> goal);

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ProcessGreeting>> goal_handle);

  void handle_accepted(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ProcessGreeting>> goal_handle);

  void execute(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ProcessGreeting>> goal_handle);
};

} // namespace greeting_processor

#endif // GREETING_PROCESSOR__GREETING_PROCESSOR_COMPONENT_HPP_

ヘッダーの内容は主に他の作成したコンポーネントノードと同様です。変更点は下記のようです。

44行　アクションのデータ型のヘッダーファイルをインクルードします。
50行　アクションのデータ型のタイプ名は長いので、usingでアリアスを定義します。
60行　アクションを提供するサーバオブジェクトを保存する変数です。
62行〜64行　アクションが要求される時のコールバックを定義します。 uuidはアクションのユニークIDです。 goalはクライントからのGoal情報です。アクション定義のセクション１で定義されたデータです。
66行〜67行　アクションがキャンセルされる時のコールバックを定義します。 goal_handleはサーバー内で保存されるGoalオブジェクトのハンドルです。アクションステートマシン上の状況を管理するために利用します。
69行〜70行　アクションをアクセプトする時のコールバックを定義します。ここでアクションの実行方法を実装します。この関数が完了するまでに新たなアクション実行要求は対応できないので、同アクションの並行実行が可能であればここでスレッドを立ち上げてすぐにリターンすることがおすすめです。このサンプルはその手法を利用します。
72行〜73行　アクションの実装はここに置きます。 handle_acceptedコールバックから呼ばれるようにします。

最後にアクションを提供するコンポーネントノードを実装します。エディターで~/ros2_basics/src/greeting_processor/src/greeting_processor_component.cppを開いて下記を入れます。

#include "greeting_processor/greeting_processor_component.hpp"

namespace greeting_processor
{

GreetingProcessor::GreetingProcessor(const rclcpp::NodeOptions & options)
: Node("greeting_processor", options)
{
}

void GreetingProcessor::make_action_server()
{
  using std::placeholders::_1;
  using std::placeholders::_2;

  server_ = rclcpp_action::create_server<ProcessGreeting>(
    this->shared_from_this(),
    "process_greeting",
    std::bind(&GreetingProcessor::handle_goal, this, _1, _2),
    std::bind(&GreetingProcessor::handle_cancel, this, _1),
    std::bind(&GreetingProcessor::handle_accepted, this, _1));
}

rclcpp_action::GoalResponse GreetingProcessor::handle_goal(
  const rclcpp_action::GoalUUID & uuid,
  std::shared_ptr<const ProcessGreeting::Goal> goal)
{
  RCLCPP_INFO(
    rclcpp::get_logger("server"),
    "Got request to process greeting from human '%s'",
    goal->name.c_str());
  (void)uuid;
  if (goal->name == "bob" || goal->name == "Bob") {
    // Reject goals asking to greet Bob
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse GreetingProcessor::handle_cancel(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<ProcessGreeting>> goal_handle)
{
  RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal");
  (void)goal_handle;
  // Accept all cancel requests
  return rclcpp_action::CancelResponse::ACCEPT;
}

void GreetingProcessor::handle_accepted(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<ProcessGreeting>> goal_handle)
{
  using std::placeholders::_1;
  RCLCPP_INFO(rclcpp::get_logger("server"), "Beginning execution of goal");
  // Create a thread to handle the actual goal execution so we remain responsive to new goals
  std::thread{std::bind(&GreetingProcessor::execute, this, _1), goal_handle}.detach();
}

void GreetingProcessor::execute(
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<ProcessGreeting>> goal_handle)
{
  rclcpp::Rate rate(1);
  const auto goal = goal_handle->get_goal();
  RCLCPP_INFO(
    rclcpp::get_logger("server"),
    "Executing goal for human '%s'",
    goal->name.c_str());
  auto feedback = std::make_shared<ProcessGreeting::Feedback>();
  auto result = std::make_shared<ProcessGreeting::Result>();

  // Simulate long processing time while remaining cancellable
  for (int i=0; i < 101 && rclcpp::ok(); i += 10) {
    if (goal_handle->is_canceling()) {
      result->greeting = "Cancelled greeting";
      goal_handle->canceled(result);
      RCLCPP_INFO(rclcpp::get_logger("server"), "Goal cancelled");
      return;
    }

    // Publish feedback on how processing is going
    feedback->percent_complete = i;
    goal_handle->publish_feedback(feedback);
    RCLCPP_INFO(rclcpp::get_logger("server"), "%d complete", i);

    rate.sleep();
  }

  if (rclcpp::ok()) {
    result->greeting = "Hello, " + goal->name;
    goal_handle->succeed(result);
    RCLCPP_INFO(rclcpp::get_logger("server"), "Goal succeeded");
  }
}

} // namespace greeting_processor

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(greeting_processor::GreetingProcessor)

実装の重要点を下記で説明します。

13行〜14行　std::bindを利用するときに渡すメンバー関数は２つの引数を使うので、それを指定するためのオブジェクトタイプをusingします。
16行〜21行　サーバーインスタンスを作成します。サーバーはこのノードに登録します。アクション名はprocess_greetingにします。アクションに必要な３つのコールバックを渡します。クラスのメンバー関数のでstd::bindで渡します。
24行〜38行　アクションの実行が要求されるときのコールバックです。
33行　要求を却下することは可能です。ここでは「bob」という名前が入るGoalを却下します。それ以外をアクセプトします。
40行〜47行　アクションの実行がキャンセルされるときのコールバックです。すべての実行をキャンセル可能にしたのでただACCEPTを返します。キャンセルを却下することも可能です。
49行〜56行　アクションの実行がアクセプトされた時のコールバックです。 handle_goal()がACCEPT_AND_EXECUTEを返すとこのコールバックが実行されます。
55行　アクションの実行は平行でできるためにスレッドで実行します。
58行〜92行　アクションの実行自体です。
62行　GoalハンドルからGoalの情報を取得します。
67行〜68行　Feedback（進捗報告）とResult（結果報告）を持つ変数です。
71行　デモのために長い処理のフリをするループです。ノードがシャットダウンされることも確認します。
72行　Goalはキャンセルされたかを確認します。キャンセルされた場合はそこまでの処理を結果として返します。 Goalをキャンセルに設定します。要求したクライントにこれが伝えられます。
81行　処理の進捗を報告します。
87行~91行　処理が終わったら、ノードはシャットダウンされていないと確認して、そして最後の結果を要求したクライントに返します。 GoalのIDが分かれば他の興味があるクライント（デバッギングツール等）もこの結果が見えます。

これでアクションを提供するノードを作成しました。ビルドできるかどうか確認します。

$ cd ~/ros2_basics
$ colcon build --packages-select greeting_processor

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/actions/greeting_processor

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_processor
$ ros2 run greeting_processor greeting_processor

アクションの利用するノードの作成

ここでアクションを利用するクライアントを作成します。新しいパッケージを作成し、下記のファイルを入れます。

$ cd ~/ros2_basics/src
$ mkdir -p greet_me/src

~/ros2_basics/src/greet_me/package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>greet_me</name>
  <version>1.0.0</version>
  <description>アイサツを要求する</description>
  <maintainer email="bob@example.com">ボブ</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rclcpp</build_depend>
  <build_depend>rclcpp_action</build_depend>
  <build_depend>greeting_actions</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclcpp_action</exec_depend>
  <exec_depend>greeting_actions</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

~/ros2_basics/src/greet_me/CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(greet_me)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(greeting_actions REQUIRED)

add_executable(greet_me src/greet_me.cpp)
ament_target_dependencies(greet_me rclcpp rclcpp_action greeting_actions)

install(TARGETS
  greet_me
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

アクションクライアントノードを実装します。サービスクライアントと同様にターミナルのツールとして利用します。 ~/ros2_basics/src/greet_me/src/greet_me.cppをエディターで開いて下記の内容を入力します。

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <chrono>
#include <memory>
#include <greeting_actions/action/process_greeting.hpp>

using namespace std::chrono_literals;
using ProcessGreeting = greeting_actions::action::ProcessGreeting;

void feedback_callback(
  std::shared_ptr<rclcpp::Node> node,
  rclcpp_action::ClientGoalHandle<ProcessGreeting>::SharedPtr,
  const std::shared_ptr<const ProcessGreeting::Feedback> feedback)
{
  RCLCPP_INFO(node->get_logger(), "Greeting creation %d%% complete", feedback->percent_complete);
}

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("greet_me");

  // アクションクライントを作成する
  auto client = rclcpp_action::create_client<ProcessGreeting>(node, "process_greeting");

  // アクションが提供されているまでに待つ
  while (!client->wait_for_action_server(1s)) {
    // シャットダウンされたかどうか確認する
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for action server.");
      rclcpp::shutdown();
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "Waiting for action server...");
  }

  // アクションGoalを作成する
  auto goal = ProcessGreeting::Goal();
  if (argc > 1) {
    goal.name = std::string(argv[1]);
  } else {
    goal.name = std::string("Bill");
  }

  RCLCPP_INFO(node->get_logger(), "Sending goal: '%s'", goal.name.c_str());
  using std::placeholders::_1;
  using std::placeholders::_2;
  auto send_goal_options = rclcpp_action::Client<ProcessGreeting>::SendGoalOptions();
  send_goal_options.feedback_callback = std::bind(&feedback_callback, node, _1, _2);
  auto goal_handle_future = client->async_send_goal(goal, send_goal_options);

  // Goalがサーバーでアクセプトされるまでに待つ
  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
      rclcpp::executor::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Send goal call failed");
    rclcpp::shutdown();
    return 1;
  }

  rclcpp_action::ClientGoalHandle<ProcessGreeting>::SharedPtr goal_handle = goal_handle_future.get();
  if (!goal_handle) {
    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
    rclcpp::shutdown();
    return 1;
  }

  // サーバーでアクションの実行が終わるまでに待つ
  RCLCPP_INFO(node->get_logger(), "Waiting for result");
  auto result_future = client->async_get_result(goal_handle);
  if (rclcpp::spin_until_future_complete(node, result_future) !=
      rclcpp::executor::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Failed to get action result");
    rclcpp::shutdown();
    return 1;
  }

  rclcpp_action::ClientGoalHandle<ProcessGreeting>::WrappedResult result = result_future.get();
  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(node->get_logger(), "Goal was aborted");
      rclcpp::shutdown();
      return 1;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(node->get_logger(), "Goal was cancelled");
      rclcpp::shutdown();
      return 1;
    default:
      RCLCPP_ERROR(node->get_logger(), "Unknown action result code");
      rclcpp::shutdown();
      return 1;
  }

  RCLCPP_INFO(node->get_logger(), "Received action result: '%s'", result.result->greeting.c_str());

  rclcpp::shutdown();
  return 0;
}

実装の重要点を下記で説明します。

10行〜16行　アクションが実行中、進捗情報が来る次第呼ばれるコールバックです。
24行　アクションサーバーと連絡するクライアントオブジェクトを作成します。
27行〜35行　アクションサーバーが立ち上がることを確認し、必要におおじて待ちます。
38行〜43行　Goalを作成してサーバーに送るデータを入れます。
48行〜50行　Goalをサーバーに送ります。進捗情報のコールバック関数も指定します。
53行〜58行　サーバーがGoalをアクセプトするか却下するかまでに待ちます。
60行〜65行　キャンセルしたり結果を受け取ったりするために、送ったGoalを管理するためのGoalハンドルを取得します。
69行　サービスクライアントと同様に、Goalハンドルを利用して実行完了を待ちます。完了したら結果は受信されてresult_futureに保存されます。
77行〜93行　result_futureから実行の結果を取得し、確認します。
95行　実行が成功した場合、結果を表示します。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/tree/master/actions/greet_me

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greet_me
$ ros2 run greet_me greet_me

ビルドとテスト

端末で下記を実行し全ワークスペースをコンパイルします。

$ cd ~/ros2_basics/
$ colcon build

端末で下記を実行しgreeting_processorノードを起動します。

$ source install/local_setup.bash
$ ros2 run greeting_processor greeting_processor
[INFO] [server]: Got request to process greeting from human 'Bill'
[INFO] [server]: Beginning execution of goal
[INFO] [server]: Executing goal for human 'Bill'
[INFO] [server]: 0 complete
[INFO] [server]: 10 complete
[INFO] [server]: 20 complete
[INFO] [server]: 30 complete
[INFO] [server]: 40 complete
[INFO] [server]: 50 complete
[INFO] [server]: 60 complete
[INFO] [server]: 70 complete
[INFO] [server]: 80 complete
[INFO] [server]: 90 complete
[INFO] [server]: 100 complete
[INFO] [server]: Goal succeeded
[INFO] [server]: Got request to process greeting from human 'bob'

もう一つの端末を起動して、下記でgreet_meを実行しサービスを利用します。

$ cd ~/ros2_basics
$ source install/local_setup.bash
$ ros2 run greet_me greet_me
[INFO] [greet_me]: Waiting for action server...
[INFO] [greet_me]: Waiting for action server...
[INFO] [greet_me]: Sending goal: 'Bill'
[INFO] [greet_me]: Waiting for result
[INFO] [greet_me]: Greeting creation 0% complete
[INFO] [greet_me]: Greeting creation 10% complete
[INFO] [greet_me]: Greeting creation 20% complete
[INFO] [greet_me]: Greeting creation 30% complete
[INFO] [greet_me]: Greeting creation 40% complete
[INFO] [greet_me]: Greeting creation 50% complete
[INFO] [greet_me]: Greeting creation 60% complete
[INFO] [greet_me]: Greeting creation 70% complete
[INFO] [greet_me]: Greeting creation 80% complete
[INFO] [greet_me]: Greeting creation 90% complete
[INFO] [greet_me]: Greeting creation 100% complete
[INFO] [greet_me]: Received action result: 'Hello, Bill'
$ ros2 run greet_me greet_me bob
[INFO] [greet_me]: Sending goal: 'bob'
[ERROR] [greet_me]: Goal was rejected by server

アクションのキャンセル方法

アクションとサービスの一つの大きな差は、アクションが実行中にキャンセルすることは可能です。上記で作成したアクションサーバーはキャンセル対応の機能がありますが、クライアントは利用していません。このセクションで上記のクライアントを編集して、実行は時間がかかりすぎなGoalをキャンセル要求するようにします。

まずは~/ros2_basics/src/greet_me/src/greet_me.cppをエディターで開いて、下記の編集を行います。

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <chrono>
#include <memory>
#include <greeting_actions/action/process_greeting.hpp>

using namespace std::chrono_literals;
using ProcessGreeting = greeting_actions::action::ProcessGreeting;

void feedback_callback(
  std::shared_ptr<rclcpp::Node> node,
  rclcpp_action::ClientGoalHandle<ProcessGreeting>::SharedPtr,
  const std::shared_ptr<const ProcessGreeting::Feedback> feedback)
{
  RCLCPP_INFO(node->get_logger(), "Greeting creation %d%% complete", feedback->percent_complete);
}

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("greet_me");

  // アクションクライントを作成する
  auto client = rclcpp_action::create_client<ProcessGreeting>(node, "process_greeting");

  // アクションが提供されているまでに待つ
  while (!client->wait_for_action_server(1s)) {
    // シャットダウンされたかどうか確認する
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for action server.");
      rclcpp::shutdown();
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "Waiting for action server...");
  }

  // アクションGoalを作成する
  auto goal = ProcessGreeting::Goal();
  if (argc > 1) {
    goal.name = std::string(argv[1]);
  } else {
    goal.name = std::string("Bill");
  }

  RCLCPP_INFO(node->get_logger(), "Sending goal: '%s'", goal.name.c_str());
  using std::placeholders::_1;
  using std::placeholders::_2;
  auto send_goal_options = rclcpp_action::Client<ProcessGreeting>::SendGoalOptions();
  send_goal_options.feedback_callback = std::bind(&feedback_callback, node, _1, _2);
  auto goal_handle_future = client->async_send_goal(goal, send_goal_options);

  // Goalがサーバーでアクセプトされるまでに待つ
  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
      rclcpp::executor::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Send goal call failed");
    rclcpp::shutdown();
    return 1;
  }

  rclcpp_action::ClientGoalHandle<ProcessGreeting>::SharedPtr goal_handle = goal_handle_future.get();
  if (!goal_handle) {
    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
    rclcpp::shutdown();
    return 1;
  }

/**** ここから削除 ****/
  // サーバーでアクションの実行が終わるまでに待つ
  RCLCPP_INFO(node->get_logger(), "Waiting for result");
  auto result_future = client->async_get_result(goal_handle);
  if (rclcpp::spin_until_future_complete(node, result_future) !=
      rclcpp::executor::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Failed to get action result");
    rclcpp::shutdown();
    return 1;
  }
/**** ここまで削除 ****/
/**** ここから追加 ****/
  RCLCPP_INFO(node->get_logger(), "Waiting for result");
  auto result_future = client->async_get_result(goal_handle);
  auto wait_result = rclcpp::spin_until_future_complete(node, result_future, 5s);
  if (wait_result == rclcpp::executor::FutureReturnCode::TIMEOUT) {
    RCLCPP_INFO(node->get_logger(), "Cancelling goal");
    auto cancel_result_future = client->async_cancel_goal(goal_handle);
    if (rclcpp::spin_until_future_complete(node, cancel_result_future) !=
        rclcpp::executor::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(node->get_logger(), "Failed to cancel goal");
      rclcpp::shutdown();
      return 1;
    }
  } else if (wait_result != rclcpp::executor::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Failed to get action result");
    rclcpp::shutdown();
    return 1;
  }

  RCLCPP_INFO(node->get_logger(), "Waiting for result again");
  if (rclcpp::spin_until_future_complete(node, result_future) !=
      rclcpp::executor::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Failed to get action result");
    rclcpp::shutdown();
    return 1;
  }
/**** ここまで追加 ****/

  rclcpp_action::ClientGoalHandle<ProcessGreeting>::WrappedResult result = result_future.get();
  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(node->get_logger(), "Goal was aborted");
      rclcpp::shutdown();
      return 1;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(node->get_logger(), "Goal was cancelled");
      rclcpp::shutdown();
      return 1;
    default:
      RCLCPP_ERROR(node->get_logger(), "Unknown action result code");
      rclcpp::shutdown();
      return 1;
  }

  RCLCPP_INFO(node->get_logger(), "Received action result: '%s'", result.result->greeting.c_str());

  rclcpp::shutdown();
  return 0;
}

変更点は下記に説明します。

81行　タイムアウト着きで実行の結果を待ちます。
82行~84行　結果の待つことがタイムアウトするとき、キャンセルリクエストをサーバーに送信します。
85行~90行　キャンセルできるかどうかがアクションの実装によって決められるので、キャンセルリクエストの結果を待ちます。

変更点後、通常通り結果を待ちます。実行をキャンセルしたのですぐにくることもありますが、キャンセルは時間がかかるアクションもあるのですぐではないときもあります。このため、キャンセルした場合でも結果を待って確認することがよく重要です。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rosjp_ros2_basics/master/actions/greet_me_with_cancel

下記のように自分のワークスペースに入れて利用できます。

$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd ~/ros2_basics
$ colcon build --packages-select greet_me_with_cancel
$ ros2 run greet_me_with_cancel greet_me

ビルドとテスト

端末で下記を実行し全ワークスペースをコンパイルします。

$ cd ~/ros2_basics/
$ colcon build

端末で下記を実行しgreeting_processorノードを起動します。

$ source install/local_setup.bash
$ ros2 run greeting_processor greeting_processor
[INFO] [server]: Got request to process greeting from human 'ben'
[INFO] [server]: Beginning execution of goal
[INFO] [server]: Executing goal for human 'ben'
[INFO] [server]: 0 complete
[INFO] [server]: 10 complete
[INFO] [server]: 20 complete
[INFO] [server]: 30 complete
[INFO] [server]: 40 complete
[INFO] [server]: 50 complete
[INFO] [server]: Got request to cancel goal
[INFO] [server]: Goal cancelled

もう一つの端末を起動して、下記でgreet_meを実行しサービスを利用します。

$ cd ~/ros2_basics
$ source install/local_setup.bash
$ ros2 run greet_me greet_me ben
[INFO] [greet_me]: Sending goal: 'ben'
[INFO] [greet_me]: Waiting for result
[INFO] [greet_me]: Greeting creation 10% complete
[INFO] [greet_me]: Greeting creation 20% complete
[INFO] [greet_me]: Greeting creation 30% complete
[INFO] [greet_me]: Greeting creation 40% complete
[INFO] [greet_me]: Greeting creation 50% complete
[INFO] [greet_me]: Cancelling goal
[INFO] [greet_me]: Waiting for result again
[ERROR] [greet_me]: Goal was cancelled