Zenn
📦

ROS2プログラミング入門 #5 パブリッシャーとサブスクライバー

2022/05/14に公開
C++
Ubuntu
ROS
ROS2
tech

この記事では、ROS2プログラミングでパブリッシャーとサブスクライバーを作ってみます。

はじめに

ROSではメッセージの送り手をパブリッシャー、受け手をサブスクライバーと言うのでした。

パブリッシャーとサブスクライバー
https://docs.ros.org/en/foxy/Tutorials/Topics/Understanding-ROS2-Topics.html より

上図はパブリッシャーからサブスクライバーへトピックを介してメッセージが送られている様子です。今回は、できるだけシンプルなパブリッシャーとサブスクライバーを作って動かしてみます。

全体像

作ろうとしているものの全体像を図示してみると次のようになります。

my_publisher ノードから greeting トピックを介して my_subscriber ノードへメッセージを送る、というのが全体像です。なお greeting トピックで扱うのは文字列ということにします。

パブリッシャー

さっそく my_first_package の中で hello の時と同様にソースファイルを作ってみましょう。

ここでは my_publisher.cpp という名前で、中身は次のように書いてみました。

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("my_publisher");
  auto publisher = node->create_publisher<std_msgs::msg::String>("greeting", 1);

  rclcpp::WallRate loop(1);
  int count = 0;
  while (rclcpp::ok()) {
    auto msg = std_msgs::msg::String();
    msg.data = "Hello, world " + std::to_string(count++);
    publisher->publish(msg);
    loop.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

main() の中はざっくり初期化、メインループ、終了処理に別れています。

初期化とメインループについて詳しく見てみましょう。

パブリッシュ側の初期化

まず init() で rclcpp ライブラリを初期化します。

次に make_shared() でノードを作ります。make_shared() の引数の文字列がノード名です。

さらに create_publisher() でパブリッシュできるようにします。第1引数の文字列はトピック名、第2引数の数値はQoSです。また、テンプレート <std_msgs::msg::String> でトピックのデータ型を指定しています。

メインループ

メインループに入る前に loop と count を宣言します。

メインループに入ると、文字列 Hello, world と count を文字列に変換したものを連結してメッセージのデータを作るとともに count をインクリメントします。そして publish() でメッセージをパブリッシュします。

メインループの最後で sleep() し、1Hz周期でループが回るようにします。

サブスクライバー

パブリッシャーと同様にサブスクライバーも作ります。

ここでは my_subscriber.cpp という名前で、中身は次のように書いてみました。

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

rclcpp::Node::SharedPtr g_node = nullptr;

void callback(const std_msgs::msg::String::SharedPtr msg)
{
  RCLCPP_INFO(g_node->get_logger(), "%s", msg->data.c_str());
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  g_node = rclcpp::Node::make_shared("my_subscriber");
  auto subscriber = g_node->create_subscription<std_msgs::msg::String>("greeting", 1, callback);

  rclcpp::spin(g_node);

  g_node = nullptr;
  rclcpp::shutdown();
  return 0;
}

サブスクライバーにはパブリッシャーと違ってグローバル変数とコールバック関数があります。

こちらは初期化とコールバック関数について詳しく見てみましょう。

サブスクライブ側の初期化

init() による初期化と make_shared() によるノードの作成はパブリッシュ側と同様です。

続く create_subscription() でサブスクライブできるようにします。ここで第1引数と第2引数は create_publisher() と同じですが、さらに第3引数でコールバック関数を指定します。

コールバック関数はメッセージをサブスクライブしたタイミングで呼び出されます。

サブスクライブの準備ができたら spin() でコールバックを待機します。

コールバック関数

コールバック関数は引数にメッセージを受け取って呼び出されます。コールバック関数内では、メッセージのデータ部分を文字列に変換して RCLCPP_INFO() で表示しています。

コールバック関数とメイン関数でノードにアクセスしたかったのでグローバル変数 g_node を宣言しましたが、これはあまり良いやり方ではありません。これは後の回で改善します。

動作確認

ビルド設定ファイル CMakeLists.txt を編集し、ビルドしてノードの動作確認を行います。

ビルド設定ファイルの編集

変更内容としては、基本となるメッセージの型を使えるようにする以下の行と

+ find_package(std_msgs REQUIRED)

パブリッシャーとサブスクライバーをビルドするための以下の行を追加しました。

+ set(NODES my_publisher;my_subscriber)
+ foreach(target IN LISTS NODES)
+   add_executable(${target} src/${target}.cpp)
+   ament_target_dependencies(${target} rclcpp std_msgs)
+   install(TARGETS ${target} DESTINATION lib/${PROJECT_NAME})
+ endforeach()

パブリッシャーもサブスクライバーも同じようなビルドなので、CMakeの set と foreach を使って繰り返す書き方をしています。

ビルドと動作確認

ビルド設定が終わったら (cd ~/dev_ws/ && colcon build) と実行してビルドします。

ビルドに成功したら、ターミナルを2つ起動して、それぞれで次のように実行します。

ros2 run my_first_package my_subscriber

ros2 run my_first_package my_publisher

下記のような感じで表示されれば成功です。

my_subscriberを実行した様子
my_subscriberを実行した様子

my_publisherを実行した様子
my_publisherを実行した様子

おわりに

今回は、ROS2プログラミングでシンプルなパブリッシャーとサブスクライバーを作ってみました。どなたかのお役に立てば幸いです。

Discussion