Zenn
🎡

ROS2プログラミング入門 #4 メインループ

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

この記事では、作成した hello.cpp を見直しつつ、ROS2プログラミングでのメインループについて考えます。

メインループのないノード

まずは作成した hello.cpp を見直すところから始めましょう。

初期化と終了処理

hello.cpp を見て、はじめにパッと分かるのは main() の最初と最後は初期化と終了処理だろう、ということではないでしょうか。

#include "rclcpp/rclcpp.hpp"

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("hello");
  RCLCPP_INFO(node->get_logger(), "Hello, ROS2 world!");

  rclcpp::shutdown();
  return 0;
}

初期化の init() と終了処理の shutdown() の前にある rclcpp はROS2のアプリケーションに必要なライブラリです。ROS2におけるアプリケーションとライブラリの関係は About internal ROS 2 interfaces の図が参考になります。

ROS2の内部インターフェース
https://docs.ros.org/en/foxy/Concepts/About-Internal-Interfaces.html より

図から察せられるように、C++でプログラミングするときは rclcpp を、Pythonでプログラミングするときは rclpy を使うことになります。

出力処理

直接的に出力処理と関係しているのは次の行です。

  auto node = rclcpp::Node::make_shared("hello");
  RCLCPP_INFO(node->get_logger(), "Hello, ROS2 world!");

はじめに make_shared() でノードを作成して node に代入しています。

ROSでは複数のノードを協調させることによってアプリケーションを実現します。なので、基本的にプログラムの最初の方でノードを作成する必要があるわけですね。

そして RCLCPP_INFO() で Hello, ROS2 world! と標準出力に表示しています。

すぐ終わってしまう

ところで、このコードにはメインループがないのですぐ終わってしまいます。

ロボットのアプリケーションには例えば次のような繰り返しが必要です。というわけで、メインループを書きましょう。

メインループのあるノード

早速 hello.cpp にメインループを追加して Ctrl-C を押すまで終わらないようにしてみます。

と言っても簡単で、次の行を rclcpp::shutdown(); の前に追加すればOKです。これで Ctrl-C を押すまでプログラムは終了しなくなります。

  rclcpp::WallRate loop(1);
  while (rclcpp::ok()) {
    loop.sleep();
  }

while の条件である rclcpp::ok() は、終了シグナル Ctrl-C を受け取っていないなら true を返しますから、すなわち Ctrl-C を押すまでは無限にループします。

また loop.sleep() は loop の宣言時に与えた引数が繰り返し周期になるようにスリープします。したがって、この例では1秒に1回の周期で終了判定していることになります。

改めてソースファイル全体を見てみましょう。

#include "rclcpp/rclcpp.hpp"

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("hello");
  RCLCPP_INFO(node->get_logger(), "Hello, ROS2 world!");

  rclcpp::WallRate loop(1);
  while (rclcpp::ok()) {
    loop.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

動作確認

先に ros2 run my_first_package hello で hello ノードを起動しておきます。

別ターミナルで次のように実行すると、ノードが存在していることを確認できます。

ros2 node list

また、より詳しい情報を次のようにして確認することもできます。

ros2 node info /hello

実行の様子は次のような感じです。

コマンドを実行した様子

おわりに

今回は、作成した hello.cpp を見直しつつ、ROS2プログラミングでのメインループについて考えてみました。どなたかのお役に立てば幸いです。

Discussion