はじめに
ROS 2でソフト開発するときにノードの単体テストを実施したいと思ったことはないでしょうか？ 単体テストをする場合，ノードのテスト入力を何らかの方法で生成しなければなりません。入力値計算用のコードをC++でコーディングするのは労力が必要です。もしテスト入力用のROSbagを簡単に準備できれば，ROS 2ソフトのテストがかなり楽になります。具体的な例として，ROS 2での制御系設計における単体テストが挙げられます。制御器単体の動作を確認する場合，制御系の目標値のテスト信号を準備する必要があります。目標値のテスト信号を内包したROSbagを楽に作れるのであれば，ROS 2での制御系設計のハードルが大幅に低減できるでしょう。
以上の背景のもと，今回はMATLABを活用してROSbagを作る方法をご紹介します。MATLABの機能を活用することで任意のテスト信号を内包したROSbagを生成できます。大規模なコーディングをせずにテスト用のROSbagを作り出せるため，手元にMATLABさえあればROS 2ソフトのテストを効率化できるでしょう。なお，本記事はMATLAB 2024a環境で実施しており，ROS 2 humbleを想定しております。MATLABのバージョンによってROS 2のサポート範囲が変わりますのでご注意ください。
手順
さっそく，MATLABでROSbagを作成する方法を解説していきます。まず，ROS ToolBoxの入ったMATLABを用意します。職場にMATLABが無い場合，早急にMATLABを来年度予算に計上する or MATLABを使える部署への異動をご検討ください。一家に一台MATLAB Home時代ですので，ご自宅のMATLAB Homeを利用いただいても問題ありません。もしROS ToolBoxがご自宅のMATLABに入っていない場合，お小遣いで購入しましょう。
ROS ToolBoxの入ったMATLABを起動できたら，ROSbagにしたい元データを準備します。正弦波のテスト信号をROSbagにしたいのであれば，MATLABのスクリプトで正弦波の時系列信号を生成しておきます。MATLABのsin関数を使えば数行程度で正弦波の時系列信号を生成できます。CSVファイルやmatファイル上で事前にテスト用信号を準備していれば，それをそのままMATLABにインポートして利用することも可能です。
元データが準備できたら，ROS ToolBoxを使って元データの時系列信号をROSbagファイルに変換します。ROSbagファイルの生成に使用する主なMATLAB関数は「ros2bagwriter」「ros2message」「ros2time」「write」「delete」の5つです。以下に時系列データをROSbagに変換するスクリプト例を記載します。
%% テスト用入力信号の生成
tt=0:0.01:20;
f=0.1;
y1=sin(tt*2*pi*f);
y2=cos(tt*2*pi*f);
%% ROSbagへの変換処理
bagWriter = ros2bagwriter("bag_files4/my_bag_file4");% ROSbagオブジェクト生成
for i=1:length(y1)
message2 = ros2message("geometry_msgs/TwistStamped");% メッセージ型定義
message2.header.stamp.sec=int32(tt(i));% タイムスタンプ打刻
message2.header.stamp.nanosec=uint32((tt(i)-fix(tt(i)))*10^9);% タイムスタンプ打刻
message2.twist.linear.x=y1(i);
message2.twist.linear.y=y2(i);
write(bagWriter, "/matlab/control", ros2time(tt(i)),message2);% データ書き込み
end
delete(bagWriter);% オブジェクト削除によるメタデータ生成
「ros2bagwriter」ではROSbagファイル生成のためのオブジェクトを定義します。本スクリプトでは引数を"bag_files4/my_bag_file4"と設定することにより，bag_files4/my_bag_file4という名前のROSbagファイルが生成します。「ros2message」ではROS 2のメッセージ情報を定義します。ここでは，geometry_msgs/TwistStamped型のメッセージのオブジェクトをmessages2という名前で定義しています。messages2の中にワークスペース上の変数(時間tt，テスト信号のy1とy2)の値を格納します。write関数を使うことでmessages2の情報をROSbagファイルのオブジェクトに書き込むことができます。本スクリプトでは，"/matlab/control"というTopic名のメッセージがROSbagファイルのオブジェクトに保存されます。ros2timeを使うことでROSの時刻情報も一緒に保存できます。
本スクリプトのようにデータ点数分forループを回してオブジェクトに情報を保存れば，テスト信号をROSbagに保存できます。最後に，delete関数を用いてオブジェクトを削除すればROSbagファイルが作業ディレクトリ上に生成されます。
ここまでの手順をまとめると以下のようになります。
-
ROS ToolBox入りMATLABの起動
自宅もしくは職場にあるMATLABを起動する。無ければ入手する。 -
ROSbagに変換したい元データの準備
元データをmatlabのワークスペースに保存しておく。 -
ROSbagオブジェクトへのデータ保存
ROS Toolboxの関数を用いてROSbagのオブジェクトを定義し，ワークスペースの時系列信号データをros2messageとwrite関数を用いて保存する。 -
オブジェクトの削除
ワークスペース上のオブジェクトをdelete関数で削除してメタデータを生成する。
以上の手順によりROSbagをMATLABで作り出すことができます。ワークススペースにデータさえ用意しておけば，たった十数行程度のスクリプトをMATLABで書くだけでROSbagファイルを生成できます。
注意点
ここではROS Toolboxを使う上でのいくつかの注意事項を述べておきます。
環境構築
MATLABとToolBoxをそのままインストールするだけではROS ToolBoxの機能を十分活用できません。3rdパーティツールを追加でインストールしなければなりません。環境構築に関してはMathWorks社のwebページの方で解説されています。
ユーザーが準備しなければならないツールとして，「Python」と「Visual Studio」の2つが挙げられます。Pythonに関してはMATLABのverによってPythonの対応バージョンも変わってくるので注意が必要です。2024aや2024bではPython3.9か3.10のインストールが必要です。Visual studioも2019か2022をインストールする必要があります。著者の環境でもPython3.9とVisual studio2022を使用すれば2024a・2024bのROS ToolBoxが動作することを確認しております。
独自メッセージ型への対応
ROS 2では独自メッセージ型を定義することで任意の型のメッセージを生成できます。ROS ToolBoxで独自メッセージを扱う場合，事前にmatlab上で独自メッセージファイルをビルド/読み込みしておく必要があります。具体的には，独自メッセージの定義ファイルをmatlabの作業ディレクトリに保存し，ros2genmsgというmatlab関数で独自メッセージのビルド/読み込みをしなければなりません。ros2genmsgでは独自メッセージの定義ファイルのパスを指定するだけでビルドと読みこみをしてくれます。ros2genmsgでビルドができれば，以後MATLAB上で独自メッセージを利用できるようになります。ただし，ファイルのパス指定が適切でないとビルドができないことがあります。
具体例として，ROS 2の独自メッセージ「ackermann_msgs」をros2genmsgでビルド/読み込みしてみましょう。下記の作業ディレクトリ上で独自メッセージのパスを指定してros2genmsgを実行するとエラーが発生してしまいます。
作業ディレクトリ
MATLA上でのros2genmsgのエラー
エラーの原因はパスの設定の仕方です。ros2genmsgでは独自メッセージのファイルよりも一つ上の階層のパスの指定をしないと，エラーが発生してしまいます。本ケースでは「C:\Users\trial\OneDrive\ackermann_msgs-ros2\」が指定されるようにパスを設定しなおせばエラーが解消されます。
※ちなみにこのパスのエラーに関してはMathWorksのエンジニアの方に対処方法を教えてもらいました。この場を借りてお礼申し上げます。
検証
実際に，MATLABからROSbagファイルを作り出せるかを検証してみましょう。Githubに先ほどのスクリプトを挙げてみたので，このサンプルを用いて検証していきます。
サンプルソフトではテスト用信号として正弦波の時系列信号をmatlabスクリプトで計算し，その時系列データをgeometry/twistedstamped型で出力します。そのままスクリプトを実行すると，ディレクトリに「bag_files4」という名前のROSbagファイルのフォルダが生成されると思います。
生成されたROSbagファイルをプロットして動作を確認してみましょう。まず，matlab上で生成したy1とy2の正弦波の波形が下記の図となります。このy1とy2をROSbagデータに変換することを試みます。
matlab上で計算したy1とy2の時系列プロット
サンプルソフトを実行すると，y1とy2の時系列データが/matlab/control/のtwiststamped型のx軸とy軸速度としてROSbagに保存されます。ROSbagをROS 2のplotjugglerでプロットした結果が下記の図となります。MATLAB上で計算したテスト信号と同じ波形の信号をROSbagデータとして生成できていることがわかります。
生成したROSbagの時系列プロット
以上の結果より，MATLABのROS ToolBoxを利用することで任意のテスト信号のROSbagを生成できることがわかりました。20行にも満たない小規模なMATLABスクリプトだけでテスト用のROSbagを準備できるため，ROS 2のソフト開発効率化に役立ちそうです。
おわりに
本記事ではMATLABにてROS 2のROSbagを生成する方法を解説しました。ROS ToolBoxを活用することで任意のテスト信号を内包したROSbagを生成でき，ROS 2ソフトのテストを効率化できるでしょう。ROS ToolBoxには様々な機能が実装されており，本記事の内容はあくまでToolBoxの機能の一側面にすぎません。今後ROS 2の開発がより広がるにつれて，MATLABでのROS ToolBoxの活用事例もより増えていくと思われます。本記事の内容がロボットや自動運転等の開発への助けとなれば幸いです。それではまたどこかでお会いしましょう。