ROS 2のAPIの使い方

Updated on: 2019-04-16

トップに戻る

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

ROS 2のAPIの基本を理解

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

1
$ mkdir -p ~/ros2_basics/src

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

1
2
3
4
5
6
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics.git
Cloning into 'rosjp_ros2_basics'...
[省略]
$ cd rosjp_ros2_basics
$ git checkout style_comparison

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

  1. パッケージを作成し、package.xmlにパッケージのメタデータを記述します。

  2. CMakeLists.txtを作成し、パッケージのコンパイル方法を記述します。

  3. C++のソースファイルを作成しノードを実装します。

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

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

パッケージのメタデータ

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

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
<?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.0.0</version>
  <description>ROSJPのROS 2講習会ようのサンプルソース: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>

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
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のパッケージの独立性が高いです。

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

ノードの実装

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

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

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

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

1
#include <rclcpp/rclcpp.hpp>

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

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

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

1
#include <chrono>

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

1
#include <memory>

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
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");

  auto greeting = std::make_shared<std_msgs::msg::String>();
  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にある単位付け時間指定方法のフィーチャーです。 「1秒」という意味です。)

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コマンドを用います。

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

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

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

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

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

1
2
3
4
5
6
$ 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というパッケージにあります。 ノードの実装はヘッダーファイルとソースファイルの2つのファイルにあります。

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

ヘッダーファイル

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#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 Greeter();

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のサンプルに参照してください。

1
#include <rclcpp/rclcpp.hpp>

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

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

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

1
2
namespace greeter_ros2_style
{

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

class Greeter : public rclcpp::Node
{

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

1
2
public:
  GREETER_PUBLIC Greeter();

クラス定義のパブリックセクションでクラスを共有ライブラリからエキスポートします。 (先定義したマクロを利用しています。) そして、クラスのコンストラクタを定義します。 コンストラクタの実装はのちほど.cppファイルでします。

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

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

1
  void broadcast_greeting();

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

ソースファイル

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
#include "greeter_ros2_style/greeter_component.hpp"

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

using namespace std::chrono_literals;

namespace greeter_ros2_style
{

Greeter::Greeter()
: Node("greeter")
{
  pub_ = create_publisher<std_msgs::msg::String>("greeting");

  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

CLASS_LOADER_REGISTER_CLASS(greeter_ros2_style::Greeter, rclcpp::Node)

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

1
#include "greeter_ros2_style/greeter_component.hpp"

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

1
#include <class_loader/register_macro.hpp>

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

1
2
3
4
#include <chrono>
#include <memory>

#include <memory>

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

1
2
namespace greeter_ros2_style
{

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

1
2
3
4
5
6
7
Greeter::Greeter()
: Node("greeter")
{
  pub_ = create_publisher<std_msgs::msg::String>("greeting");

  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のインフラストラクチャが管理できるタイマーイベントでノードの振る舞えを実行します。 タイマー作成の引数は下記のようです。

1
2
3
4
5
6
7
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風と同様にメーセージを持つ変数を作成し、変数にデータを入れて、そしてパブリッシャーを利用して出力します。

1
CLASS_LOADER_REGISTER_CLASS(greeter_ros2_style::Greeter, rclcpp::Node)

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeter_ros2_style/greeter_component.hpp"

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

  rclcpp::shutdown();
  return 0;
}

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

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

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

パッケージのメタ情報

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

1
2
3
  <build_depend>class_loader</build_depend>

  <exec_depend>class_loader</exec_depend>

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
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(class_loader 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
  class_loader
  )
# コンポーネントノードを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(class_loader)
ament_export_dependencies(std_msgs)

ament_export_include_directories(include)

# 本パッケージが提供する再利用可能なコンパイルターゲットの情報をエクスポートする
ament_export_libraries(displayer_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()

下記は変更点です。

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

ビルド&実行

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

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

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

1
2
3
4
5
6
$ 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++でノードを作成するとき、以下のステップを行います。

  1. パッケージを作成し、package.xmlにパッケージのメタデータを記述します。

  2. CMakeLists.txtを作成し、パッケージのコンパイル方法を記述します。

  3. C++のソースファイルを作成しノードを実装します。

パッケージ作成

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

1
2
$ cd ~/ros2_basics/src
$ mkdir displayer

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?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>class_loader</build_depend>

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

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

コンパイル方法の指定

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
cmake_minimum_required(VERSION 3.5)
project(displayer)

# Must use C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

# Display all warnings
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Find the packages we need to build this node
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(class_loader REQUIRED)
find_package(std_msgs REQUIRED)

include_directories(include)

# Build a shared library containing the composable node (the "component")
add_library(displayer_component SHARED src/displayer_component.cpp)
# Set the definition needed to expose the component class in the shared library
target_compile_definitions(displayer_component PRIVATE "DISPLAYER_BUILDING_DLL")
ament_target_dependencies(displayer_component
  rclcpp
  class_loader
  std_msgs
  )
# Register the component plugin with the ament resource index
rclcpp_register_node_plugins(displayer_component "displayer::Displayer")

# Build a standalone version of the node
add_executable(displayer src/displayer.cpp)
# Link in the shared library containing the component
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(class_loader)
ament_export_dependencies(std_msgs)

ament_export_include_directories(include)

ament_export_libraries(displayer_component)

# Install the component header file
install(DIRECTORY
  include/displayer
  DESTINATION include
  )

# Install the component library
install(TARGETS
  displayer_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  )

# Install the standalone version of the node
install(TARGETS
  displayer
  DESTINATION lib/${PROJECT_NAME}
  )

ament_package()

ノードの実装

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

1
2
3
$ cd ~/ros2_basics/src/displayer
$ mkdir src
$ mkdir -p include/displayer

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
#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
{

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);
};

} // namespace displayer

#endif // DISPLAYER__DISPLAYER_COMPONENT_HPP_

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
#include "displayer/displayer_component.hpp"

#include <class_loader/register_macro.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", 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

CLASS_LOADER_REGISTER_CLASS(displayer::Displayer, rclcpp::Node)

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

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

1
2
3
4
5
6
7
8
9
10
11
#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "displayer/displayer_component.hpp"

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

ビルド&実行

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

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

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

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

1
2
$ cd ~/ros2_basics/
$ colcon build

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

1
2
3
4
5
6
$ 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'

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

1
2
3
4
5
6
7
$ 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/string_displayer/displayer

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

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

独自メッセージ型の定義

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

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

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

1
2
$ cd ~/ros2_basics/src
$ mkdir greeting_msg

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
<?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.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>

  <member_of_group>rosidl_interface_packages</member_of_group>

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
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_export_dependencies(rosidl_default_runtime)

ament_package()

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

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

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

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

1
2
3
string hello_text
string world_name
int16 count

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

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

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

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

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

独自メッセージ型の受信

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

  1. package.xmlでメッセージ型を定義するパッケージに依存を追加します。

  2. CMakeLists.txtでメッセージ型の依存を追加し、ノード等のコンパイルターゲットにリンクします。

  3. メッセージ型のデータ型を利用するようにノードのソースを編集します。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
<?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.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>std_msgs</build_depend>
<!---- ここまで削除 ---->
  <build_depend>class_loader</build_depend>
<!---- ここから追加 ---->
  <build_depend>greeting_msg</build_depend>
<!---- ここまで追加 ---->

  <exec_depend>rclcpp</exec_depend>
<!---- ここから削除 ---->
  <exec_depend>std_msgs</exec_depend>
<!---- ここまで削除 ---->
  <exec_depend>class_loader</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を開いて、下記の編集を行います。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
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)
####### ここまで削除 #######
####### ここから追加 #######
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
  class_loader
####### ここから削除 #######
  std_msgs
####### ここまで削除 #######
####### ここから追加 #######
  greeting_msg
####### ここまで追加 #######
  )
rclcpp_register_node_plugins(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(class_loader)
####### ここから削除 #######
ament_export_dependencies(std_msg)
####### ここまで削除 #######
####### ここから追加 #######
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()

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
///// 省略 /////

#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_

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
#include "displayer/displayer_component.hpp"

#include <class_loader/register_macro.h>
#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", std::bind(&Displayer::display_greeting, this, _1));
/**** ここまで削除 ****/

/**** ここから追加 ****/
  sub_ = this->create_subscription<greeting_msg::msg::Greeting>(
    "greeting", 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

CLASS_LOADER_REGISTER_CLASS(displayer::Displayer, rclcpp::Node)

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

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

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

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

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

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout custom_message_type
$ cd ~/ros2_basics
$ colcon build --packages-select displayer
$ ros2 run displayer displayer

独自メッセージ型の送信

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?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.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>class_loader</build_depend>
  <build_depend>greeting_msg</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>class_loader</exec_depend>
  <exec_depend>greeting_msg</exec_depend>

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
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(class_loader 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
  class_loader
  greeting_msg
  )
rclcpp_register_node_plugins(greeter_component "greeter::Greeter")

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

ament_export_dependencies(ament_cmake)
ament_export_dependencies(rclcpp)
ament_export_dependencies(class_loader)
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:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#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 Greeter();

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

  void broadcast_greeting();
};

} // namespace greeting

#endif // GREETER__GREETER_COMPONENT_HPP_

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
#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()
: Node("greeter")
{
  pub_ = create_publisher<greeting_msg::msg::Greeting>("greeting");

  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

CLASS_LOADER_REGISTER_CLASS(greeter::Greeter, rclcpp::Node)

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

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

1
2
3
4
5
6
7
8
9
10
#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeter/greeter_component.hpp"

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

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

ビルドとテスト

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

1
2
$ cd ~/ros2_basics/
$ colcon build

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

1
2
3
4
5
6
7
8
$ 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ノードを起動します。

1
2
3
4
5
6
7
$ 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/custom_message_type/greeter

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout custom_message_type
$ cd ~/ros2_basics
$ colcon build --packages-select greeter
$ ros2 run greeter greeter

ノードのコンポジション

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

ROS 2 node composition

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

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

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

  1. 新しい実行ファイルを作成し、複数のコンポーネントノードを主導で集めること

  2. このサンプルのような特別なコンテナノードを作成し、load_nodeサービスを利用別のノードを作成し、そのノードからコンテナノードにほしいコンポーネントノードをロードすること

  3. 上記のコンテナノードとこのサンプルのようなコマンドラインツールを利用して端末またはスクリプトからノードをロードすること

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

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

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

ソース作成

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?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.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>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>

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
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を下記のようにします。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
#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);

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

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

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

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

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

1
  rclcpp::executors::SingleThreadedExecutor exec;

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

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

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

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

ROS 2 single-threaded executor

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

ROS 2 multi-threaded executor

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

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

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

1
  exec.add_node(greeter);

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

1
2
  auto displayer = std::make_shared<displayer_custom_msg::Displayer>();
  exec.add_node(displayer);

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

1
  exec.spin();

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

ビルド&実行

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

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

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

1
2
3
4
5
6
7
8
9
$ 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/custom_message_type/greet_and_displayer

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout custom_message_type
$ cd ~/ros2_basics
$ colcon build --packages-select greet_and_displayer
$ ros2 run greet_and_displayer greet_and_displayer

サービスの定義と利用

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

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

サービスの定義

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

1
2
$ cd ~/ros2_basics/src
$ mkdir request_greeting_service

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
<?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.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>

  <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にパッケージのビルド方法を記述します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
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というファイルを作成します。

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

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

1
2
3
string name
---
string greeting

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

1
2
$ 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

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

1
2
3
4
5
6
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout custom_message_type
$ cd ~/ros2_basics
$ colcon build --packages-select request_greeting_service

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

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?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.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>class_loader</build_depend>
  <build_depend>request_greeting_service</build_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>class_loader</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:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
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(class_loader 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
  class_loader
  request_greeting_service
  )
rclcpp_register_node_plugins(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(class_loader)
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:

1
2
3
4
5
6
7
8
9
10
#include <rclcpp/rclcpp.hpp>
#include <memory>

#include "greeting_server/greeting_server_component.hpp"

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
#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 GreetingServer();

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_

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#include "greeting_server/greeting_server_component.hpp"

#include <class_loader/register_macro.hpp>

namespace greeting_server
{

GreetingServer::GreetingServer()
: Node("greeting_server")
{
  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)
{
  response->greeting = "Hello, " + request->name;
  RCLCPP_INFO(
    this->get_logger(),
    "Responding to greeting request with '%s'",
    response->greeting.c_str());
}

} // namespace greeting_server

CLASS_LOADER_REGISTER_CLASS(greeting_server::GreetingServer, rclcpp::Node)

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

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

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

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

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

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout custom_message_type
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_server
$ ros2 run greeting_server greeting_server

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
<?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.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>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:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
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をエディターで開いて下記の内容を入力します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
#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行で作成します。

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

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

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

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

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

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

1
  request->name = "Bob";

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

1
  auto result = client->async_send_request(request);

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

1
2
3
4
5
6
7
  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です。 ここではサーバーから結果がくるまで待つという意味になります。

1
    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/custom_message_type/greeting_client

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout custom_message_type
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_client
$ ros2 run greeting_client greeting_client

ビルドとテスト

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

1
2
$ cd ~/ros2_basics/
$ colcon build

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

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

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

1
2
3
4
5
$ 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/crystal/share/std_srvsにインストールされています。

アクションの定義と利用

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

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

Action state machine

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

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

アクションの定義

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

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

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

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

1
2
$ cd ~/ros2_basics/src
$ mkdir greeting_actions

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
<?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にパッケージのビルド方法を記述します。 これもサービス型を定義するパッケージと一緒です。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
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というファイルを作成します。

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

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

1
2
3
4
5
6
7
8
# Goal
string name
---
# Result
string greeting
---
# Feedback
uint8 percent_complete

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

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

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

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

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

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

1
2
3
4
5
6
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout master
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_actions

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

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
<?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>class_loader</build_depend>
  <build_depend>greeting_actions</build_depend>

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
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(class_loader 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
  class_loader
  greeting_actions
  )
rclcpp_register_node_plugins(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(class_loader)
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:

1
2
3
4
5
6
7
8
9
10
11
12
#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を開いて下記を入れます。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
#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 GreetingProcessor();

  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_

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
#include "greeting_processor/greeting_processor_component.hpp"

#include <class_loader/register_macro.hpp>

namespace greeting_processor
{

GreetingProcessor::GreetingProcessor()
: Node("greeting_processor")
{
}

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 std::array<uint8_t, 16> & 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());
  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");
  // 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->set_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->set_succeeded(result);
    RCLCPP_INFO(rclcpp::get_logger("server"), "Goal succeeded");
  }
}

} // namespace greeting_processor

CLASS_LOADER_REGISTER_CLASS(greeting_processor::GreetingProcessor, rclcpp::Node)

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

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

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

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

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

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout master
$ cd ~/ros2_basics
$ colcon build --packages-select greeting_processor
$ ros2 run greeting_processor greeting_processor

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

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

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

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?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:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
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をエディターで開いて下記の内容を入力します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
#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 goal_handle_future = client->async_send_goal(
    goal,
    std::bind(&feedback_callback, node, _1, _2));

  // 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 = goal_handle->async_result();
  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>::Result 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.response->greeting.c_str());

  rclcpp::shutdown();
  return 0;
}

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

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

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

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout master
$ cd ~/ros2_basics
$ colcon build --packages-select greet_me
$ ros2 run greet_me greet_me

ビルドとテスト

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

1
2
$ cd ~/ros2_basics/
$ colcon build

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
$ 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を実行しサービスを利用します。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
$ 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をエディターで開いて、下記の編集を行います。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#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 goal_handle_future = client->async_send_goal(
    goal,
    std::bind(&feedback_callback, node, _1, _2));

  // 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 = goal_handle->async_result();
  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 = goal_handle->async_result();
  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>::Result 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.response->greeting.c_str());

  rclcpp::shutdown();
  return 0;
}

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

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

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

https://github.com/gbiggs/rosjp_ros2_basics/tree/cancelling_actions/greet_me

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

1
2
3
4
5
6
7
$ cd ~/ros2_basics/src
$ git clone https://github.com/gbiggs/rosjp_ros2_basics
$ cd rosjp_ros2_basics
$ git checkout cancelling_actions
$ cd ~/ros2_basics
$ colcon build --packages-select greet_me 
$ ros2 run greet_me greet_me

ビルドとテスト

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

1
2
$ cd ~/ros2_basics/
$ colcon build

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

1
2
3
4
5
6
7
8
9
10
11
12
13
$ 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を実行しサービスを利用します。

1
2
3
4
5
6
7
8
9
10
11
12
13
$ 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