ROSでM5StackのIMUを使う

今回はM5StackをROSのIMUセンサモジュールとして使っていきます.
M5Stackにはいくつか種類がありますが, 今回はIMUを内蔵しているM5Stack Grayを使用します.

M5Stack Basicに、加速度、ジャイロ、磁気を計測可能な9軸センサ、MPU6886を搭載したモデルです。M5Stackは、320 x 240 TFTカラーディスプレイ、microSDカードスロット、スピーカーを備えたコンパクトで便利な開発モジュールです。ESP32を搭載しているため、Wi-FiおよびBlueto...

基本的には, M5StackのIMU用のサンプルプログラムであるM5StackFire_MPU9250.inoにrosserial_arduinoのPublisherを追加したコードになります. まずはArduinoのライブラリにrosserial_arduinoを追加してください.

ja/rosserial_arduino/jaTutorials/Arduino IDE Setup - ROS Wiki

また公式には推奨されていませんが, ArduinoIDEから[スケッチ->ライブラリをインクルード->ライブラリを管理]からインストールすることもできます.

今回開発したIMU用のコードが以下のM5StackFire_MPU9250_ROS.inoです.

M5Stackにこのコードを書き込んでもらいたいのですが, このままではコンパイルエラーが発生します.

原因は, IMU(MPU9250)用のファイルとrosserial用のファイルで, 同じ名前の変数(マクロ)が存在していることです. どちらのファイルにもINFOというマクロが存在しています. ファイルの中身を見てみたところ, MPU9250のINFOは宣言されてはいるものの使用されてはいないようなのでこちらの名前を変更します.
Arduino/libraries/M5Stack/src/utility/MPU9250.hの24行目辺りにある
#define INFO 0x01を#define AK8963_INFO 0x01と変更します. これでコンパイルが通るようになります.

M5Stackにプログラムが書き込めたら, M5Stackの準備は完了です.
次にUbuntu側にrosserialとrosserial-arduinoをapt-getで次のようにインストールします.

 sudo apt-get install ros-kinetic-rosserial-arduino
 sudo apt-get install ros-kinetic-rosserial

インストールできたら, M5StackとROSの入ったPCをUSBケーブルで接続します. 次のコマンドでUbuntuとM5Stackとの通信を行うノードを立ち上げます.

rosrun rosserial_python serial_node.py

通信がうまくいけば次のようにログが出力されimuというトピックが配信されています.

M5Stackは通信用のノードが立ち上がるまでは’Waiting ROS…’と画面に表示し, ノードが立ち上がると画面にIMUの値を表示します.