こんにちはshotaです。このシリーズではRaspberry Pi Mouse（ラズパイマウス）でROS 2を動かしていきます。
今回は、アールティが開発・販売しているUSB出力9軸IMUセンサモジュール（以下、IMUモジュール）をROS 2で動かす方法を紹介し、その後ラズパイマウスにIMUモジュールを搭載してサンプルプログラムを実行します。
前回のおさらい
前回はマルチLiDARマウントオプションキットを取り付けて、ラズパイマウスROS 2サンプル集にあるSLAMサンプルを実行しました。
今回もROS 2サンプル集を使用するので、下記の記事を参考にサンプル集をインストールしてください。
また、ROS 2サンプル集ではブザーを鳴らしたりモータを回したりします。
デバイスドライバも忘れずにインストールしてください。
こちらの記事で、USB出力9軸IMUセンサモジュールのROSパッケージ更新内容について紹介しています。合わせてご覧頂けるとうれしいです。
作業に必要なもの
- Raspberry Pi Mouse V3
- 商品ページはこちら
- これまでの記事で、ROS 2 DashingをインストールしたUbuntu 18.04、をインストールしたRaspberry Pi Model 4 B、を搭載したラズパイマウスをセットアップしています
- USB出力9軸IMUセンサモジュール
- Raspberry Pi Mouse オプションキットNo.8 [マルチLiDARマウント]
- 商品ページはこちら
- 組み付けにはプラスドライバーが必要です
- 作業用PC
- Ubuntu 18.04のPCを用意しました。ROS 2 Dashingインストール済みです。
IMUモジュールをROS 2で動かす
まずはIMUモジュール単体をROS 2で動かします。
ROS 2パッケージのインストール
インストールするパッケージはrt_usb_9axisimu_driverです。
rt_usb_9axisimu_driver（以下IMU ROSパッケージ）は、ROSのKineticとMelodic、ROS 2のDashingに対応しています。
今回はROS 2 Dashingを使用するのでdashing-develブランチに切り替えて作業します。
ターミナルを開き、次のコマンドを実行してパッケージをビルド＆インストールします。
# パッケージのダウンロード(clone)と、依存関係のインストール $ cd ~/ros2_ws/src $ git clone -b dashing-devel https://github.com/rt-net/rt_usb_9axisimu_driver.git $ rosdep install -r -y -i --from-paths . # パッケージのビルド＆インストール $ cd ~/ros2_ws $ colcon build --symlink-install $ source ~/ros2_ws/install/setup.bash
センサの出力（トピック）をコマンドで確認する
パッケージがインストールされたので、パッケージを動かし（ノードを起動し）、センサ値を表示します。
つぎの動画のようにコマンドを入力していくので、作業前に参照してください。
まず、IMUモジュールをPCに接続します。
IMUモジュールをmicro-USBケーブでPCに接続する。
次のコマンドを実行してノードを起動します。
# Terminal 1 $ ros2 run rt_usb_9axisimu_driver rt_usb_9axisimu_driver
IMU ROSパッケージはROS 2のライフサイクル機能に対応しており、ノードの状態を変えることで、センサ値を出力します。
起動直後のノードの状態はUnconfiguredです。この状態ではUSBポートにアクセスしないので、USBケーブルを抜き差ししてもOKです。
それでは、ターミナルをもう一つ開き、ノードの状態を変更します。
次のコマンドを入力します。
# Terminal 2 $ source ~/ros2_ws/install/setup.bash $ ros2 lifecycle set rt_usb_9axisimu_driver configure
これにより、ノードはInactive状態に遷移します。
この状態では、ノードがUSBポートを開き、IMUモジュールと通信します。センサ値のトピックは出力されません。
続けて、次のコマンドを入力します。
# Terminal 2 $ ros2 lifecycle set rt_usb_9axisimu_driver activate
これにより、ノードはActive状態に遷移します。
この状態では、ノードがモジュールとの定期通信を開始し、センサ値をトピックとして出力します。
次のコマンドを実行することで、加速度・角速度（/imu/data_raw）、地磁気（/imu/mag）、モジュール温度（/imu/temperature）のトピックを確認できます。
$ ros2 topic echo /imu/data_raw $ ros2 topic echo /imu/mag $ ros2 topic echo /imu/temperature
終了する際はCtrl+Cを入力します。
ラズパイマウスでIMUモジュールを動かす
続いて、ラズパイマウスでIMUモジュールを動かす方法を紹介します。
オプションキットの組付け
ラズパイマウスにマルチLiDARマウントとIMUモジュールを組み付けましょう。
ちなみに、マルチLiDARマウントはUSB出力9軸IMUセンサモジュールだけでなく、
3種類のLiDAR（URG-04LX-UG01、RPLIDAR A1、LDS-01）にも対応しています。
オプションキットの組付けに必要なもの
IMUモジュールをマルチLiDARマウントに組み付けます。
モジュールの向きに注意してください。
IMUモジュールを組み付ける
マルチLiDARマウントをラズパイマウスに組み付けます。
マウントのツメをラズパイマウスに引っ掛けます。
マルチLiDARマウントの組付け
前方のツメをネジで固定します。
ツメをネジで固定する
micro-USBケーブルを接続して完成です。
micro-USBケーブルを接続する
ラズパイマウスROS 2サンプル集をインストールする
ラズパイマウスROS 2サンプル集のインストール方法は過去の記事で紹介しています。
新たにrt_usb_9axisimu_driverパッケージをインストールするため、実行コマンドを再掲します。
2020/10/13追記: パッケージのビルドに失敗するというご報告があったためpackage.xmlを修正しました。現在は下記コマンドでビルドできます。
$ cd ~/ros2_ws/src # Clone package $ git clone https://github.com/rt-net/raspimouse_ros2_examples $ git clone https://github.com/rt-net/raspimouse2 # For direction controller example $ git clone -b dashing-devel https://github.com/rt-net/rt_usb_9axisimu_driver # Install dependencies $ rosdep install -r -y --from-paths . --ignore-src # Build & Install $ cd ~/ros2_ws $ colcon build --symlink-install $ source ~/ros2_ws/install/setup.bash
direction_controllerサンプルを実行する
それでは、direction_controllerサンプルを実行します。
実行手順はパッケージのREADMEにも書かれています。
ラズパイマウス上で次のコマンドを実行します。
$ ros2 launch raspimouse_ros2_examples direction_controller.launch.py
このコマンドにより、次の動画のようにラズパイマウスの姿勢を制御するノードが起動します。
このサンプルでは、IMUモジュールのz軸周りの角速度を蓄積し、ラズパイマウスの姿勢（方向）を推定しています。
SW0を押すと現在の姿勢を0度として初期化します。
SW1を押すと0度を維持するようにPID制御が働きます。
SW2を押すと姿勢を±90度に変化させるようにPID制御が働きます。
このように、ラズパイマウスにIMUモジュールを組み合わせることで、
ラズパイマウス単体では出来なかった動作が実現します。
他にも、モジュールからは加速度や地磁気の値も取得できるので、ぜひ活用してみてください。
rqt_plotでトピックを描画する
動画の右下ではリアルタイムで更新されるグラフ（rqt_plot）が表示されています。
これを再現してみましょう。
先ほどと同じようにラズパイマウス上でノードを起動します。
$ ros2 launch raspimouse_ros2_examples direction_controller.launch.py
つづいて、作業用PCでターミナルを開きrqtコマンドを実行します。
$ rqt
rqtの実行①
Plugins→Visualization→Plotを選択します。
rqtの実行②
rqt_plotが起動します。
Topicの欄に/cmd_vel/angular/zと入力してプラスボタンを押します。
これにより、ラズパイマウスの走行速度司令トピック（cmd_vel）の回転速度（angular/z）を描画できます。
$ export ROS_DOMAIN_ID=22
と実行し、それぞれ同じ値の環境変数を設定してください。
rqtの実行③
つづいて、Topicに/heading_angle/dataと入力してプラスボタンを押します。
これにより、ラズパイマウスの姿勢（方向）を描画できます。
rqtの実行④
準備が整ったので、ラズパイマウスのスイッチを押し、姿勢制御を開始しましょう。
rqt_plotの十字の矢印を押し、マウスでドラッグするとグラフを移動できます。
右クリックでドラッグすると拡大縮小できます。
rqtの実行⑤
右上の停止ボタンを押すと、グラフの更新を止められます。
rqtの実行⑥
まとめ
今回は、
- USB出力9軸IMUセンサモジュールをROS 2で動かしました
- ラズパイマウスROS 2サンプル集のdirection_controllerサンプルを動かし、ラズパイマウスの姿勢を制御しました
- rqt_plotで走行速度司令（cmd_vel）と、ラズパイマウスの姿勢（方向）を描画しました
今回の記事でラズパイマウスをROS 2で動かすシリーズは一旦終了です。
またラズパイマウス & ROS 2の新しい情報が入った際に紹介します。