初版「ROS2ではじめよう次世代ロボットプログラミング」ROS 2移行章公開

Posted Sep 13, 2024 Updated Sep 16, 2024

By Yutaka Kondo

57 min read

実に5年ぶりの改訂となる改訂新版「ROS2ではじめよう次世代ロボットプログラミング」が9/19に発売されます。それを記念して、9/11からROS2本の紹介記事を連続投稿しています。

今回も初版ではかなり好評をいただいていたRommbaドライバーを題材にROS 1パッケージをROS 2パッケージに移行する作業を実践する章をブログフォーマットに直して、無償公開します。前回同様、2025年にROS 1はディスコンとなるため、あえて改訂新版から削除しました。 2019年当時の内容のままなので、少し古くなっている部分はご了承ください。

RoombaとROSの歴史

iRobotが製造、販売しているロボット掃除機Roombaを知らない人は、今やほとんどいないでしょう。2002年に販売が開始され、世界累計販売台数2000万台（2017年9月当時）を販売するに至っています¹。また、日本だけに限っても累計販売台数300万台（2018年9月当時）を販売しています²。Roombaは世界で一番販売されているロボット家電です。

そして、Roombaは単なるロボット家電ではありません。その実、移動ロボットの開発プラットフォームとしても利用できるのです。Roomba 800シリーズ以前のRoombaは表面のカバーを取り外すとシリアル通信のための接続口が用意されています。この接続口にシリアルUSB接続アダプターを介してPCと接続すると、シリアル通信が行え、Roombaの全機能にアクセスできます。シリアル通信のプロトコルは、iRobot Roomba Open Interface Specification³というiRobot公式の仕様書が公開されており、誰でも閲覧することができます。

残念ながら、最新のRoombaモデルはシリアル通信の接続口がなくなり、無線LAN通信だけでしか接続できなくなってしまいました。その通信プロトコルは安全上、公開されていないために、開発プラットフォームにはできません。もし、掃除機としてだけではなく、移動ロボットとしても使いたいのであれば、旧式のRoombaを購入されることをお勧めします。

このOpen Interfaceで通信できるRoombaから掃除機機能を取り除き、廉価な移動ロボットの開発プラットフォームに転用した製品がiRobot Create 2⁴です。Create 2は200ドルで販売されており、前代のCreate 1はROSナビゲーションプログラミング入門のためのロボットである初代TurtleBotにも採用されました。なお、二代目にあたるTurtleBot2⁵にもOpen Interface相当で動作するYujin RobotのKobuki⁶が採用されています。

Roombaは歴としたROS対応移動ロボットプラットフォームです。Roombaを購入して、日中は掃除機として、夜中はROS開発プラットフォームとして活かし切りましょう！

ただし、残念ながら、RoombaのROS1ドライバはまだROS2には対応していません。そこで、本章では筆者がこのパッケージをROS2対応に移行する過程で得られた実践的なノウハウを共有します。読者の皆様が、自身のロボットをROS2対応させる際にも役立つでしょう。

著者は2019年初頭に価格改定されて、非常にお値頃価格となったRoomba 643⁷をこの機会に購入しました。また、iRobot Create 2も試用する機会があり、両機体を使ってROS1ドライバと拙作ROS2ドライバ両方の動作確認を行っています。日本で購入する場合には、Create 2は輸入代理店を経由する必要があるため、200ドル相当では購入できず、倍額ほどします。3万円前後で購入できるようになったRoomba 643の購入をお勧めします。

シリアル通信ケーブルの入手

Roombaと接続するシリアル通信ケーブルは

Create 2に付属する純正品を用いる
電子工作を行って自作する
Amazonで同等のケーブル⁸を購入する

のどれかで入手する必要があります。Create 2を購入して純正品を用いるのが一番推奨されますが、前述のように日本からでは輸入代行を行う必要があり、200ドル相当では購入できません。 2のやり方を詳細に紹介しているブログ記事は日本語でもたくさんありますので、そちらを参照いただき、挑戦してみてください。

3で購入すると5,000円以上かかりますが、2で自作すると2,000円程度で収まります。著者は時間を節約するために、3のケーブルを購入してしまいましたが、全く問題なく動作しています。

Roomba／CreateのROS1ドライバ `create_autonomy`

RoombaおよびCreateのROS1ドライバには、カナダのサイモンフレーザー大学Autonomy研究室⁹が公開した create_autonomy があります。このパッケージは同研究室が公開しているC++言語ライブラリ libcreate を、薄くROS1のメッセージ通信で覆ったラッパーです。 libcreate だけを使えば、ROSを介さず純粋なC++言語ライブラリとしても利用できる、よく設計されたライブラリです。

ROS2移行する際も、 libcreate に修正を加える必要はありません。 create_autonomy に対してのみROS2移行に伴う修正を行っていきます。

create_autonomy レポジトリには以下の5つのパッケージが含まれています。

create_autonomy: メタパッケージ¹⁰
ca_driver: libcreate を使ったRoombaとのシリアル接続・通信
ca_description: Roomba / CreateのURDFなどの保存場所
ca_msgs: create_autonomy パッケージ専用のメッセージ定義
ca_tools: 遠隔操作スクリプトの保存場所

特に大きな変更が必要なのは、 ca_driver です。その他のパッケージは設定ファイルの修正などで済みます。

Roombaとの接続を開始すると、多くのトピック送受信、パラメータ設定が行われます。どのようなトピック、パラメータがあるか一覧をご覧ください。モーターやセンサーだけでなく、バッテリーやロボット上部のボタン、LED、効果音なども制御できるようになっています。Roombaの持つ機能すべてにアクセスできます。

パラメータ一覧

表 1にはRoombaドライバの起動時に必要となるパラメータが並んでいます。

表1: パラメータ一覧
パラメータ名	内容	デフォルト値
`dev`	ロボットのデバイスパス	`/dev/ttyUSB0`
`base_frame`	ロボットのベースとなる座標系名	`base_footprint`
`odom_frame`	ロボットのオドメトリー¹¹の座標系名	`odom`
`latch_cmd_duration`	もし設定値以上、速度入力がなければ停止するまでの時間	`0.2`
`loop_hz`	制御ループの周期	`10.0`
`publish_tf`	`odom_frame` から `base_frame` へのTFを発行するなら `true`	`true`
`robot_model`	ロボットのモデル名（ `ROOMBA_400`, `CREATE_1`, `CREATE_2` のどれか）	`CREATE_2`
`baud`	シリアル通信のボーレート	`robot_model` に応じて自動設定

送信トピック一覧

表 2はRoombaドライバの起動後に送信されるトピックの一覧です。車輪の回転量に基づくオドメトリーも得られるようになっています。また、バッテリーの詳細な情報がわかるため、バッテリーの現在状態を正しく計測することができます。ロボット上部のボタンや赤外線センサーの受信状態などもメッセージ通信で受信することができます。

実はRoomba全面のバンパーには、接触したか、していないかの true/false で変化するタッチセンサーだけでなく、距離に応じて連続的に変化する1次元の距離センサーも複数個内蔵しており、その値もRoombaドライバで取得できます。これを使えば、至近距離専用のLiDARのように振舞うこともできます。

表2: 送信トピック一覧
トピック名	内容	メッセージ型
`battery/capacity`	バッテリーの最大充電量 (Ah)	std_msgs/Float32
`battery/charge`	バッテリーの現在充電量 (Ah)	std_msgs/Float32
`battery/charge_ratio`	charge / capacity	std_msgs/Float32
`battery/charging_state`	バッテリーの充電状態	ca_msg/ChargingState
`battery/current`	バッテリーの現在電流消費量 (A) プラスなら充電中	std_msgs/Float32
`battery/temperature`	バッテリーの温度 (℃)	std_msgs/Int16
`battery/voltage`	バッテリーの電圧 (V)	std_msgs/Float32
`bumper`	バンパーの状態（距離センサーの値含む）	ca_msg/Bumper
`clean_button`	cleanボタンの押下	std_msgs/Empty
`day_button`	dayボタンの押下	std_msgs/Empty
`hour_button`	hourボタンの押下	std_msgs/Empty
`minute_button`	minuteボタンの押下	std_msgs/Empty
`dock_button`	dockボタンの押下	std_msgs/Empty
`spot_button`	spotボタンの押下	std_msgs/Empty
`ir_omni`	全方位赤外線センサーが受信した値（0なら受信なし）	std_msgs/UInt16
`joint_states`	車輪の位置、速度の現在状態	sensor_msgs/JointState
`mode`	ロボットの現在状態（値の意味はOpen Interface参照）	ca_msg/Mode
`odom`	車輪の回転量に基づくロボットのオドメトリー	nav_msgs/Odometry
`wheeldrop`	どちらかの車輪の脱輪	std_msgs/Empty
`/tf`	`odom` から `base_footprint` へのTF	tf2_msgs/TFMessage

受信トピック一覧

表 3はRoombaを外部から制御するために受信するトピックの一覧です。一般的なROS対応移動ロボットと同じく、 cmd_vel を受け取って、2次元平面の移動を行うことができます。それ以外に、ロボット上部のLEDや4文字のアスキー文字が表示できる液晶ディスプレイに表示する文字列の指定、ロボット本体から流れるMIDI音源の定義、再生が行えます。

表3: 受信トピック一覧
トピック名	内容	メッセージ型
`cmd_vel`	ロボットの目標速度	geometry_msgs/Twist
`debris_led`	debrisを表すLEDの有効・無効	std_msgs/Bool
`spot_led`	spotを表すLEDの有効・無効	std_msgs/Bool
`dock_led`	dockを表すLEDの有効・無効	std_msgs/Bool
`check_led`	check robotを表すLEDの有効・無効	std_msgs/Bool
`power_led`	powerのLEDの色と輝度	std_msgs/UInt8MultiArray
	最初の1バイトで緑(0)から赤(255)を表し、次の1バイトで輝度を表す
`set_ascii`	最大4文字のASCII文字	std_msgs/UInt8MultiArray
`dock`	給電動作の開始	std_msgs/Empty
`undock`	給電状態からの離脱動作の開始	std_msgs/Empty
`define_song`	最大16種類のMIDI効果音の定義（詳細はOpen Interface参照）	ca_msg/DefineSong
`play_song`	16種類の中からMIDI効果音の再生	ca_msg/PlaySong

拙作ROS2版 `create_autonomy`

create_autonomy は残念ながら、まだ公式にはROS1にしか対応していませんが、 libcreate 自体はiRobot Roomba Open Interfaceが提供するほぼすべての機能に対応しており、 create_autonomy 自体はほとんどその関数を呼び出すだけの構造になっています。そこで、この create_autonomy をROS1からROS2対応に移行してみましょう。実践すること以上に効率的かつ効果的にROS2を学ぶ手段はありません。

パッケージ全体のソースコードは著者のGitHubレポジトリ https://github.com/youtalk/create_autonomy¹²の各ブランチに保存されています。

dashing-devel ブランチ¹³: 一番修正箇所が少なくなるように、必要最低限のROS2移行を行なったブランチ
reformat-code ブランチ¹⁴: 上記に加えて、ROS2のC++言語ソースコード用Lintツール¹⁵ ament_lint をパスするように修正したブランチ
lifecycle-node ブランチ¹⁶: 上記に加えて、ライフサイクルに対応させたブランチ
ros2-textbook ブランチ¹⁷: 上記に加えて、日本語コメントを追加したブランチ（以降で使用）

拙作ROS2版 create_autonomy はROS2にまだ正式対応していない診断機能以外は全てROS2対応しています。さらに新機能としてライフサイクルにも対応しています。

ROS1パッケージのROS2移行は公式サイトに推奨手順が記載されています。¹⁸その手順は大まかには以下の通りです。

package.xml の更新
メッセージ、サービス、アクション定義の更新
ビルドシステムの変更
ソースコードの更新

前述の通り、 create_autonomy はメタパッケージになっており、一番変更が必要なのは libcreate を使ったRoombaとのシリアル接続・通信を行う ca_driver パッケージです。このパッケージとメッセージ定義のパッケージに対する移行手順を一つずつコメント付きのソースコードで確認しながら進んでいきましょう。

`package.xml` の更新

package.xml はROS1からROS2に移行するに伴い、フォーマットのバージョンが2から3に更新されました。

http://www.ros.org/reps/rep-0149.html

フォーマットの修正箇所は上記ドキュメントの赤字部分を見るとわかります。多岐に渡りますが、実際にROS2パッケージ開発でよく目にするのは、 ROS_VERSION という属性です。

  
<depend condition="$ROS_VERSION == 1">roscpp</depend>
<depend condition="$ROS_VERSION == 2">rclcpp</depend>

のように、ROSのバージョンの1か2のどちらに依存するかどうかを指定できるようになりました。その他の更新内容は随時必要になったタイミングで説明します。

~/ros2/src/create_autonomy/ca_driver/package.xml:

診断機能を提供する diagnostic_updater パッケージはまだROS2移行が完了していないため、コメントアウトしています。 ca_driver のソースコードからも一時的に取り除いています。 diagnostic_*** パッケージはROSノードの様々なエラーを管理するために用いる便利パッケージなので、ROS2移行が済めば復活させましょう。

  
<?xml version="1.0"?>
<!-- format属性を2から3に更新 -->
<package format="3">
  <name>ca_driver</name>
  <version>2.0.0</version>
  <description>ROS 2 driver for iRobot's Create and Roomba platforms, based on libcreate</description>

  <maintainer email="jperron@sfu.ca">Jacob Perron</maintainer>
  <maintainer email="yutaka.kondo@youtalk.jp">Yutaka Kondo</maintainer>

  <!-- licenseはApache License 2.0が好ましいが、本パッケージは元々BSDなため、変更せず -->
  <license>BSD</license>

  <url type="website">http://wiki.ros.org/ca_driver</url>

  <author email="jperron@sfu.ca">Jacob Perron</author>

  <!-- パッケージビルドツールをament_cmakeに変更 -->
  <buildtool_depend>ament_cmake</buildtool_depend>

  <!-- 依存パッケージを列挙 -->
  <depend>ca_msgs</depend>
  <depend>diagnostic_msgs</depend>
  <!-- <depend>diagnostic_updater</depend> ROS2未対応なため、コメントアウト-->
  <depend>geometry_msgs</depend>
  <depend>libcreate</depend>
  <depend>nav_msgs</depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_lifecycle</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>

  <!-- 実行時のみの依存パッケージを列挙 -->
  <exec_depend>ca_description</exec_depend>

  <!-- テスト時のみの依存パッケージを列挙 -->
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <!-- パッケージの追加情報 -->
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

ROS2パッケージはApache License 2.0を用いることが推奨されています。ROS1パッケージではBSDライセンス¹⁹が用いられてきましたが、Apache License 2.0はBSDライセンスでは考えられていない特許などの補足事項にも対応しています。ですが、元々BSDライセンスで開発が進んでいた create_autonomy パッケージを独断でライセンス変更することはライセンス違反ですので、ここではそのままBSDライセンスを継続適用しています。

メッセージ、サービス、アクション定義の更新

ROS2パッケージでは、各種メッセージ、サービス、アクション定義ファイルの設置場所を下記ディレクトリ名にする必要があります。

msg: メッセージ定義ファイルの設置ディレクトリ
srv: サービス定義ファイルの設置ディレクトリ
action: アクション定義ファイルの設置ディレクトリ

また、3章や7章でも説明していますが、メッセージ、サービス、アクション定義は他のソースコードとは別のパッケージにしておくことをお勧めします。今回の場合、こちらはすでに ca_msgs として別パッケージになっており、ディレクトリ名も正しかったため、ほとんど修正が必要ありません。定義ファイルに関する部分だけ抜き出して、 package.xml と CMakeLists.txt の変更を載せます。

~/ros2/src/create_autonomy/ca_msgs/package.xml:

メッセージ、サービス、アクション定義ファイルを設置しているパッケージの場合、 rosidl_default_generators を buildtool_depend タグに追加しましょう。これにより、メッセージ、サービス、アクションを呼び出すためのスタブコードが自動生成されます。

member_of_group タグ²⁰は package.xml バージョン3で追加されたタグの一つです。メタパッケージのような親子関係に似たグループ関係を表現します。これにより、 member_of_group の指定先がビルドされると、本パッケージも再ビルドされるようになります。

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

<depend>std_msgs</depend>
<depend>common_interfaces</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

~/ros2/src/create_autonomy/ca_msgs/CMakelists.txt:

メッセージ、サービス、アクション定義ファイルを rosidl_generate_interfaces マクロの引数に渡します。 DEPENDENCIES 以下には定義ファイルで用いた依存パッケージを列挙します。 rosidl_generate_interfaces マクロを用いるには find_package(rosidl_default_generators REQUIRED) を呼んでおく必要があります。

  
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(common_interfaces REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Bumper.msg"
  "msg/ChargingState.msg"
  "msg/DefineSong.msg"
  "msg/Mode.msg"
  "msg/PlaySong.msg"
  DEPENDENCIES std_msgs
)

ビルドシステムの変更

ビルドシステムはROS1の catkin から colcon に変更されました。 colcon はクライアントプログラミング言語に依らないビルドツールの総称です。C++言語パッケージの場合には ament_cmake などのツールが呼び出されます。

ビルドツールにはソースコードの静的解析ツール²¹もテストツールの一つとして統合されています。それが ament_lint_auto です。 ament_lint_auto はパッケージごとにテストに必要になる静的解析ツールを自動的に検出し、呼び出すようにできています。例えば、C++言語パッケージではPython3言語のための静的解析ツールは呼び出されません。

~/ros2/src/create_autonomy/ca_driver/CMakelists.txt:

テストに関する記述は if(BUILD_TESTING) ... endif() で囲う必要があります。 BUILD_TESTING 変数はROS2独自の変数ではなく、 CMake プロジェクトのテストを行う標準機能 CTest²² で用いられているものです。

  
add_executable(${PROJECT_NAME} src/create_driver.cpp)
ament_target_dependencies(${PROJECT_NAME}
  "ca_msgs"
  "diagnostic_msgs"
  # "diagnostic_updater"
  "geometry_msgs"
  "libcreate"
  "nav_msgs"
  "rclcpp"
  "rclcpp_lifecycle"
  "sensor_msgs"
  "std_msgs"
  "tf2"
  "tf2_ros"
)

# テスト設定
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

# libディレクトリのインストール
install(TARGETS ${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME}
)

# includeディレクトリのインストール
install(DIRECTORY include/${PROJECT_NAME}
  DESTINATION include/${PROJECT_NAME}
  FILES_MATCHING PATTERN "*.hpp"
)

# configディレクトリのインストール
install(DIRECTORY config
  DESTINATION config
)

ament_package()

package.xml と CMakeLists.txt が揃えば、ビルドシステムを実行することができます。ROS2移行後のソースコードに対して、ビルドとテストを実行してみましょう。

  
$ cd ~/ros2
$ colcon build
$ colcon test
$ colcon test-result --all
build/ca_driver/test_results/ca_driver/copyright.xunit.xml: 2 tests, 0 errors, 2 failures, 0 skipped
build/ca_driver/test_results/ca_driver/cppcheck.xunit.xml: 0 tests, 0 errors, 0 failures, 0 skipped
build/ca_driver/test_results/ca_driver/cpplint.xunit.xml: 2 tests, 0 errors, 0 failures, 0 skipped
build/ca_driver/test_results/ca_driver/lint_cmake.xunit.xml: 1 test, 0 errors, 0 failures, 0 skipped
build/ca_driver/test_results/ca_driver/uncrustify.xunit.xml: 2 tests, 0 errors, 0 failure, 0 skipped
build/ca_driver/test_results/ca_driver/xmllint.xunit.xml: 1 test, 0 errors, 0 failures, 0 skipped

Summary: 8 tests, 0 errors, 2 failures, 0 skipped

colcon test は全てのテストを実行するコマンド、 colcon test-result --all はそのテスト結果を集計するコマンドです。各テストツールのコマンドを colcon test コマンド経由ではなく、単体のコマンドとして呼び出すこともできます。 ament_*** という名前で定義されています。

$ ament_  # tabボタンを押下
AMENT_PREFIX_PATH           ament_pclint
AMENT_SHELL                 ament_pep257
ament_append_value          ament_pep8
ament_clang_format          ament_prepend_unique_value
ament_copyright             ament_pyflakes
ament_cppcheck              ament_uncrustify
ament_cpplint               ament_xmllint
ament_flake8                ament_zsh_to_array
ament_lint_cmake

代表的ないくつかのテストツールを個別に実行してみましょう。

  
$ cd ~/ros2/src/create_autonomy/ca_driver
$ ament_cpplint
Using '--root=/home/youtalk/ros2/src/create_autonomy/ca_driver/include' argument
Done processing /home/youtalk/ros2/src/create_autonomy/ca_driver/include/create_driver/create_driver.hpp
Using '--root=/home/youtalk/ros2/src/create_autonomy/ca_driver/src' argument
Done processing /home/youtalk/ros2/src/create_autonomy/ca_driver/src/create_driver.cpp
No errors found

$ ament_cppcheck
No errors

$ ament_uncrustify
No code style divergence in file 'include/create_driver/create_driver.hpp'
No code style divergence in file 'src/create_driver.cpp'
No errors

$ ament_lint_cmake
No errors

$ ament_copyright
include/create_driver/create_driver.hpp: copyright=Jacob Perron (Autonomy Lab, Simon Fraser University) (2015), license=<unknown>
src/create_driver.cpp: copyright=Jacob Perron (Autonomy Lab, Simon Fraser University) (2015), license=<unknown>
2 errors, checked 2 files

実行したコマンドの役割は以下の通りです。

ament_cpplint: C++言語のソースコードがGoogle C++スタイルガイド²³に従っているかどうかを判別するコマンドラインツール cpplint²⁴のROS2構成版。スタイルガイドに違反しているとエラーになります。
ament_cppcheck: C++言語のソースコードの未定義の振る舞いや危険なコーディングを検出する静的解析を行うコマンドラインツール cppcheck²⁵のROS2構成版。検出されるとエラーになります。
ament_uncrustify: C++言語の自動フォーマットを行うコマンドラインツール uncrustify²⁶のROS2構成版。フォーマット前後でソースコードの差分がある場合、エラーとなります。
ament_lint_cmake: cmakeファイルのためのLintコマンドラインツール cmakelint²⁷ のROS2構成版。違反があるとエラーになります。
ament_copyright: .c, .cc, .cpp, .cxx, .h, .hh, .hpp, .hxx, .cmake, .py の各スクリプトファイルの先頭にライセンス条項が記載されているかを確認するコマンドラインツール。ライセンスが不明の場合、エラーを返す。

先ほどの ament_copyright の実行結果では、ライセンスが不明のため、2個のエラーが報告されています。この部分は、著者が勝手に書き換えることはライセンス違反であると考え、エラーを解消することができていません。

ソースコードの更新

ソースコードの更新は多岐に渡りますが、要所要所を抜き出してROS1版とROS2版で比較しながら、移行の勘所を説明していきます。

ヘッダーファイルの更新

ROS2の名前空間では、メッセージ、サービス、アクションに対して、 msg, srv, action という別個の名前空間が与えられるようになりました。これにより、例えメッセージ、サービス、アクションに同じ名前を使ってしまったとしても、名前空間でそれぞれが隔離されるため、問題が起こることがなくなりました。

また、名前空間の分離以外に、ヘッダーファイルの拡張子には .h ではなく .hpp を使うようになったことや、ファイル名にキャメルケース²⁸ではなく、スネークケース²⁹を使うようになったこともわかります。

~/ros1/src/create_autonomy/ca_driver/include/create_driver/create_driver.h:

  
#include "ca_msgs/ChargingState.h"
#include "ca_msgs/Mode.h"
#include "ca_msgs/Bumper.h"
#include "ca_msgs/DefineSong.h"
#include "ca_msgs/PlaySong.h"

~/ros2/src/create_autonomy/ca_driver/include/create_driver/create_driver.hpp:

  
#include "ca_msgs/msg/charging_state.hpp"
#include "ca_msgs/msg/define_song.hpp"
#include "ca_msgs/msg/mode.hpp"
#include "ca_msgs/msg/play_song.hpp"
#include "create/create.h"

メンバー変数の更新

メンバー変数のうち、まずトピックのPublisher / Subcriber以外のものを更新します。前節の通り、 msg, srv, action という名前空間が追加されました。また、ROS2はC++14（に加えてC++17の一部）を使ってソースコードを書くことが推奨されているため、もはや生ポインターを使うことはやめて、特別な事情がない限りスマートポインター³⁰を使うように変更しましょう。

~/ros1/src/create_autonomy/ca_driver/include/create_driver/create_driver.h:

  
create::Create* robot_;
create::RobotModel model_;
tf::TransformBroadcaster tf_broadcaster_;
diagnostic_updater::Updater diagnostics_;
ca_msgs::Mode mode_msg_;
ca_msgs::ChargingState charging_state_msg_;
ca_msgs::Bumper bumper_msg_;
nav_msgs::Odometry odom_msg_;
geometry_msgs::TransformStamped tf_odom_;
ros::Time last_cmd_vel_time_;
std_msgs::Empty empty_msg_;
std_msgs::Float32 float32_msg_;
std_msgs::UInt16 uint16_msg_;
std_msgs::Int16 int16_msg_;
sensor_msgs::JointState joint_state_msg_;

~/ros2/src/create_autonomy/ca_driver/include/create_driver/create_driver.h:

ROS1の方にはないタイマー実行の管理のための timer_ 変数と現在時刻を取得するための ros_clock_ 変数が増えています。

  
ca_msgs::msg::Mode mode_msg_;
ca_msgs::msg::ChargingState charging_state_msg_;
ca_msgs::msg::Bumper bumper_msg_;
nav_msgs::msg::Odometry odom_msg_;
geometry_msgs::msg::TransformStamped tf_odom_;
rclcpp::Clock ros_clock_;  // タイムスタンプの管理
rclcpp::Time last_cmd_vel_time_;
std_msgs::msg::Empty empty_msg_;
std_msgs::msg::Float32 float32_msg_;
std_msgs::msg::UInt16 uint16_msg_;
std_msgs::msg::Int16 int16_msg_;
sensor_msgs::msg::JointState joint_state_msg_;

// ROS params
std::string dev_;
std::string base_frame_;

Publisher/Subscriber変数の更新

次にトピックのPublisher/Subscriberの変数を更新します。ROS2ではノードを実装する際、 rclcpp::Node もしくは rclcpp::LifecycleNode を継承することが推奨されています。後者はライフサイクルに対応したノードクラスです。このメンバーメソッドに create_publisher, create_subscription があるため、これらを呼ぶとPublisher/Subscriberを作成できます。また、テンプレート型になっているので、Publisher/Subscriberの実装がコンパイル時に型安全性が担保できるようになっています。

拙作ROS2版ではスマートポインターに XXX::SharedPtr （ std::shared_ptr<XXX> のエイリアス宣言）を用いましたが、他の変数と共有する予定がない場合、 ***::Ptr に変更して、より軽量な std::unique_ptr<XXX> を用いることもできます。

また、ROS2版はライフサイクルに対応したPublisherを用いています。使い方に関しては 1.8.5 項で説明します。

~/ros1/src/create_autonomy/ca_driver/include/create_driver/create_driver.h:

  
ros::Subscriber cmd_vel_sub_;
ros::Subscriber debris_led_sub_;

ros::Publisher odom_pub_;
ros::Publisher clean_btn_pub_;

~/ros1/src/create_autonomy/ca_driver/src/create_driver.cpp:

  
cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &CreateDriver::cmdVelCallback, this);
debris_led_sub_ = nh.subscribe("debris_led", 10, &CreateDriver::debrisLEDCallback, this);

odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
clean_btn_pub_ = nh.advertise<std_msgs::Empty>("clean_button", 30);

~/ros2/src/create_autonomy/ca_driver/include/create_driver/create_driver.hpp:

  
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr check_led_sub_;
rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr power_led_sub_;

rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Empty>::SharedPtr min_btn_pub_;
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Empty>::SharedPtr dock_btn_pub_;

~/ros2/src/create_autonomy/ca_driver/src/create_driver.cpp:

  
  "check_led", 10, std::bind(&CreateDriver::checkLEDCallback, this, std::placeholders::_1));
power_led_sub_ = create_subscription<std_msgs::msg::UInt8MultiArray>(
  "power_led", 10, std::bind(&CreateDriver::powerLEDCallback, this, std::placeholders::_1));
set_ascii_sub_ = create_subscription<std_msgs::msg::UInt8MultiArray>(

charge_pub_ = create_publisher<std_msgs::msg::Float32>("battery/charge", 10);
charge_ratio_pub_ = create_publisher<std_msgs::msg::Float32>("battery/charge_ratio", 10);

Subscriberのコールバックメソッドの更新

Subscriberの変数定義で記述したコールバックメソッドの定義も更新します。こちらについては、ROS1とROS2の間で大きな変更がほとんどありません。コールバックメソッドには直接関係ありませんが、ROS1では現在時刻を ros::Time::now() というクラスメソッドで呼んでいたのに対して、ROS2では rclcpp::Clock 型変数を用意してメンバーメソッドを呼ぶ必要があります。

~/ros1/src/create_autonomy/ca_driver/src/create_driver.cpp:

  
void CreateDriver::cmdVelCallback(const geometry_msgs::TwistConstPtr& msg)
{
  robot_->drive(msg->linear.x, msg->angular.z);
  last_cmd_vel_time_ = ros::Time::now();
}

~/ros2/src/create_autonomy/ca_driver/src/create_driver.cpp:

  
  robot_->enableDebrisLED(msg->data);
}

void CreateDriver::spotLEDCallback(const std_msgs::msg::Bool::UniquePtr msg)
{

Publisherのライフサイクル対応

4章で紹介したように、ROS2でライフサイクルという仕組みが導入されました。これにより、ROS2ノードは初期化前やスピン状態といった状態を持てるようになり、他のノードから現在状態を取得したり、制御したりできるようになりました。

rclcpp::LifecycleNode には、これらの状態遷移時に呼び出されるコールバックメソッドが空実装（常に成功が返る）されており、それらをオーバーライドすることで、実際の処理を実装できます。

~/ros2/src/create_autonomy/ca_driver/include/create_driver/create_driver.hpp:

  
  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
};

}  // namespace create_autonomy

#endif  // CREATE_DRIVER__CREATE_DRIVER_HPP_

実は 1.8.3項で説明したROS2版のPublisher変数の定義は on_configure に記述されていたのでした。

robot_: Roombaクライアント
odom_pub_: ライフサイクル対応版オドメトリPublisher
timer_: タイマー実行の管理

これらのメンバー変数の処理だけ抜き出して、内部実装の雰囲気を把握しましょう。

~/ros2/src/create_autonomy/ca_driver/src/create_driver.cpp:

on_configure はノードの構成時（実行直前時）に呼び出され、メンバー変数の定義（インスタンス作成）を行います。 timer_ 変数にはタイマー呼び出しされるメソッドとして、 CreateDriver::update メソッドを渡しています。その直後に cancel を呼び出していますが、これは create_wall_timer を呼び出すと、その直後からタイマー実行が開始されてしまうため、それを解除するために呼び出しています。あまり直感的ではないため、将来的に設計変更されるのではないでしょうか。

  
CallbackReturn CreateDriver::on_configure(const rclcpp_lifecycle::State &)
{
...
  robot_ = std::make_unique<create::Create>(model_);
...
  odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("odom", 30);
...
  timer_ = create_wall_timer(100ms, std::bind(&CreateDriver::update, this));
  timer_->cancel();
...
  return CallbackReturn::SUCCESS;
}

void CreateDriver::update()
{
  publishOdom();
  publishJointState();
  publishBatteryInfo();
  publishButtonPresses();
  publishOmniChar();
  publishMode();
  publishBumperInfo();
  publishWheeldrop();
...
}

その次に呼び出される on_activate はノードのスピン開始時に実行されます。インスタンス作成だけを行なった先ほどの変数に対して、 robot_ 変数はRoombaとの接続を開始するメソッドを呼び出し、もし接続に失敗すると状態遷移にエラーが発生したことを示す CallbackReturn::ERROR を返します。 odom_pub_ の方はライフサイクル対応したPublisherを実行開始します。実行をキャンセルしていた timer_ もここで再度実行させます。

  
CallbackReturn CreateDriver::on_activate(const rclcpp_lifecycle::State &)
{
  if (!robot_->connect(dev_, baud_)) {
    RCLCPP_FATAL(get_logger(),
      "[CREATE] Failed to establish serial connection with Create.");
    return CallbackReturn::ERROR;
  }
...
  odom_pub_->on_activate();
...
  timer_->reset();
  return CallbackReturn::SUCCESS;
}

ノードがスピン停止すると on_deactivate が呼び出されます。 timer_ をまず停止させてタイマー実行を止めます。そして、 odom_pub_ も実行停止して、Roombaとの接続も切断します。

  
CallbackReturn CreateDriver::on_deactivate(const rclcpp_lifecycle::State &)
{
  timer_->cancel();
  odom_pub_->on_deactivate();
...
  robot_->disconnect();
...
  return CallbackReturn::SUCCESS;
}

最後は、ノードが完全終了するときに呼び出される on_cleanup です。 reset() メソッドはスマートポインターの管理しているリソースを解放する処理を行います。前述の timer_->reset() と timer_.reset() は全く別物です。ご注意ください。

  
CallbackReturn CreateDriver::on_cleanup(const rclcpp_lifecycle::State &)
{
  timer_.reset();
...
  odom_pub_.reset();
...
  robot_.reset();
  return CallbackReturn::SUCCESS;
}

ノードにライフサイクルという仕組みが導入されたことにより、ノードの実行状態を制御することが容易になりました。エラーからの復帰も実現しやすいです。実装する上でもメソッドが別々に分かれており、ソースコードの見通しも良くなります。

スピン処理の更新

ROS1の場合には、スピン処理は開発者がコーディングする必要がありました。制御周期を決めてwhileループを回すというのが慣習ですが、その実装の仕方は開発者それぞれで異なり、品質にも差が出てしまいます。そこで、ROS2では create_wall_timer メソッドを使ったタイマー実行の記述方法を標準化し、誰でも同じように書くことができるようにしました。このおかげで main 関数の書き方は（ executor の種類以外は）ほぼどんなノードでも同じになり、とても単純になりました。ROS1の場合には ros::Rate の sleep メソッド呼び忘れがよくあるバグでしたが、そういったことを未然に防ぐことができます。

~/ros1/src/create_autonomy/ca_driver/src/create_driver.cpp:

  
void CreateDriver::spinOnce()
{
  update();
  diagnostics_.update();
  ros::spinOnce();
}

void CreateDriver::spin()
{
  ros::Rate rate(loop_hz_);
  while (ros::ok())
  {
    spinOnce();
...
    rate.sleep();
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ca_driver");
  ros::NodeHandle nh;
  CreateDriver create_driver(nh);

  try
  {
    create_driver.spin();
  }
  catch (std::runtime_error& ex)
  {
    ROS_FATAL_STREAM("[CREATE] Runtime error: " << ex.what());
    return 1;
  }
  return 0;
}

~/ros2/src/create_autonomy/ca_driver/src/create_driver.cpp:

  
  rclcpp::shutdown();

  return 0;
}

実は付録のコンポーネント指向ノードプログラミングを学ぶと、この main 関数さえ書かなくてよくなります。

まとめ

まだROS2ドライバが開発されていないRoombaを題材に、ROS1ソースコードのROS2移行をステップバイステップで説明しました。Python2のソースコードからPython3のソースコードにある程度自動変換を行なってくれる 2to3³¹のような公式ツールはROS2にはありません。ビルドツールやメッセージ通信、クライアントライブラリ構造など、ほとんど全ての部分に変更が加わったため、そうした移行サポートツールの開発は困難です。ですので、開発者は自力で移行作業を行なっていく必要があり、ソースコードの規模によっては多大な労力が必要です。本章がその労力をほんの少しでも削減するために役立てば幸いです。

著者がオリジナルの作者のソースコードレポジトリからフォークしたレポジトリでは、ジョイパッドを使った遠隔操作を行うためのパッケージもROS2移行を済ましてあります。お試しになりたい方は、下記レポジトリを参考にしてください。

脚注

¹ https://www.irobot-jp.com/press/pdf/20170914.pdf

² https://www.irobot-jp.com/press/pdf/20181010_2.pdf

³ https://www.irobot.lv/uploaded_files/File/iRobot_Roomba_500_Open_Interface_Spec.pdf

⁴ https://www.irobot.com/about-irobot/stem/create-2

⁵ https://www.turtlebot.com/turtlebot2/

⁶ http://kobuki.yujinrobot.com/about2/

⁷ https://store.irobot-jp.com/item/643.html

⁸ https://amzn.to/2Jweg3A

⁹ http://autonomylab.github.io

¹⁰ メタパッケージは package.xml しか持たず、それ自体は何の機能も提供しないROSパッケージです。 package.xml に書かれた依存パッケージをひとまとめに扱うことを目的としています。このおかげで、依存パッケージの解決を簡略化することができます。 http://wiki.ros.org/Metapackages

¹¹ 車輪を備えた移動型ロボットが、左右の車輪の回転数を積算することで自分の位置を推定する手法をオドメトリーと呼びます。

¹² レポジトリのフォーク元 https://github.com/AutonomyLab/create_autonomy にもプルリクエストを出して移行作業がマージされるように働きかけていますが、しばし時間がかかりそうなため、本書では著者レポジトリの方を参照してください。

¹³ https://github.com/youtalk/create_autonomy/tree/dashing-devel

¹⁴ https://github.com/youtalk/create_autonomy/tree/reformat-code

¹⁵ ソースコードに対し、コンパイラよりも詳細かつ厳密なチェックを行うプログラムです。

¹⁶ https://github.com/youtalk/create_autonomy/tree/lifecycle-node

¹⁷ https://github.com/youtalk/create_autonomy/tree/ros2-textbook

¹⁸ https://index.ros.org/doc/ros2/Contributing/Migration-Guide/

¹⁹ 「無保証」であることの明記と著作権およびライセンス条文自身の表示を再頒布の条件とするライセンス規定です。この条件さえ満たせば、BSDライセンスのソースコードを複製・改変して作成したオブジェクトコードをソースコードを公開せずに頒布できます。

²⁰ http://www.ros.org/reps/rep-0149.html#member-of-group-multiple

²¹ コンピュータのソフトウェアの解析手法の一種であり、実行ファイルを実行することなく解析を行います。静的解析はソースコードに対して行われる場合がほとんどです。

²² ROS1の CMakeLists.txt では catkin 独自のテスト用マクロ、変数が用いられていました。 http://docs.ros.org/melodic/api/catkin/html/howto/format1/rostest_configuration.html#cmakelists-txt