こんにちは、山本です。
現在、新人研修の一環として、チーム開発研修を行っています。今日からはリレーブログの２回目ということで、メンバーの開発の進捗が伝わってくる頃かと思います。
今回は、私が開発を担当している、M5Stack と IMU を用いた「箱」の状態の監視システムについて紹介します。
M5Stack と IMU を使う
M5Stack について
いきなりですが、この研修では M5Stack を利用します。M5Stack は中国の M5Stack 社が開発・販売しているマイコン・IoTデバイスのブランドです。
M5Stack シリーズには様々な種類がありますが、基本的な「M5Stack Core Basic」には IMU は搭載されていません。そこで、Grove 端子で接続する、「6-Axis IMU Unit（日本語名：6軸IMUユニット）」を取り付けています。
M5Stack のライブラリ
M5Stack は機種ごとに用意されたライブラリと、M5Stack 製品用の統合ライブラリである M5Unified があります。（もちろん、ESP32 や各モジュールのライブラリをベタに触るという方法もありますが、ここでは触れません）
従来は、各機種ごとにライブラリが用意されていましたが、機種によってライブラリの使い方が違うなど、ちょっと難しいところもありました。
M5Unified は近年開発されている、M5Stack 製品用の開発ライブラリです。M5Unified を利用すれば、様々なボードを自動的に判定して、1 つのライブラリから扱うことができます。また、M5Stack のセンサーや拡張ボード類も扱うことができます。
今回は、勉強も兼ねて M5Unified を採用しています。
M5Stack で ROS 2 と通信する
マイコンで動く ROS
現在、ロボット開発ミドルウェアである ROS 2 はアールティの社内含め様々な場所で利用されています。そして、本研修で取り扱う CRANE-X7 も ROS 2 の環境が提供されており、研修中の開発も ROS 2 で行っています。
というわけで、取得した IMU の情報を ROS 2 に送ることができれば、物体への加速度を考慮した制御などが、将来的にはできるようになるかもしれません。
そこで、ROS 2 を M5Stack で動かしたいのですが、ここで問題があります。ROS 2 は PC + Ubuntu などの高性能な環境を前提としていて、マイコンでは手も足も出ないほど要求性能が高いです。
ですが、M5Stack のようなマイコン環境でも動作する ROS 2 の実装が存在しています。
micro-ROS と mros2
M5Stack 上で動作する ROS 2 実装として、「micro-ROS」と「mros2」について検討しました。
micro-ROS は、外部 PC 等に用意した Agent Node というアプリケーションを介することで、マイコンと PC の通信を可能にしています。社内では、ししかわさんの HAC-Chan や、ESP-32 版の Pi:Co Classic3 などで採用されています。また、Arduino に移植されており、手軽に利用できることから、インターネット上に情報も多いです。
mros2 は、組み込みデバイス用に開発された軽量な ROS 2 互換の実装で、Agent Node 不要で直接 ROS 2 Node と通信できることが特徴です。ですが、使える機能に制限があったり、情報が少なかったりします。
結論としまして、今回は mros2 を採用しました。理由は2点あり、「Agent なしで動作したほうが、セットアップ手順を簡略化できる」、「送るデータが 3 軸＋ 3 軸の 6 つだけで、機能的に十分」だからです。
※最近は micro-ROS も 組み込み用の embeddedRTSP を利用して、Agent Node 不要で PC 上の ROS 2 と直接通信する仕組みが整いつつあるようです。
実装＆動作
ESP-IDF の環境構築
M5Stack の環境構築には、ESP-IDF を利用しました。これにより、M5Stack に搭載されている ESP32 マイコンの性能を引き出せます。
ESP-IDF の依存関係に、M5Unified と mros2 を追加します。また、M5Unified と依存関係にある、M5GFX も追加しておきます。
# main/CMakeLists.txt
idf_component_register(
SRCS "main.cpp"
INCLUDE_DIRS "."
PRIV_REQUIRES "M5Unified" "M5GFX" "mros2-esp32"
)
また、プロジェクト設定ファイルである、sdkconfig の初期値を予め設定しておきます。手動での設定項目をなくすことで、ミスを予防します。
ここでは、ライブラリの規模が大きいため、ESP32 のパーティションを拡大し、mros2 の動作のために IPv6 を無効化しています。
# sdkconfig.defaults
CONFIG_PARTITION_TABLE_SINGLE_APP_LARGE=y
CONFIG_PARTITION_TABLE_FILENAME="partitions_singleapp_large.csv"
CONFIG_LWIP_IP6_FRAG=n
設定の初期値を指定する方法は、shota さんの記事を参考にしました。
M5Unified を利用した、IMU の取得
M5Unified には、IMU を利用したサンプルプログラムが存在します。Arduino 用に記述されていますが、ESP-IDF でも同じように利用できます。
IMU の情報を取得するための処理の一部を示します。
auto imu_update = M5.Imu.update(); // IMU の情報を更新し、取得します
if (imu_update) {
auto data = M5.Imu.getImuData();
// 加速度
float ax = data.accel.x;
float ay = data.accel.y;
float az = data.accel.z;
// 角速度
float gx = data.gyro.x;
float gy = data.gyro.y;
float gz = data.gyro.z;
}
上記のようにして、各値を取得できます。
また、このままでは重力加速度が常にかかっており、見栄えがよくありません。そこで、ハイパスフィルターを実装し、重力加速度を取り除きました。実装は Google の Android 用のサンプルプログラムを参考にしました。
mros2 を利用した、IMU 情報の送信
mros2 の msg 型に sensor_msgs/msg/Imu Message は実装されていません。そこで、Vector3 型の linear と angular を内包する、geometry_msgs/msg/Twist.msg で代用します。
ここでは、IMU 情報の送信コードの一部を抜粋します。
// 変数を定義
geometry_msgs::msg::Twist imu_data;
geometry_msgs::msg::Vector3 accel;
geometry_msgs::msg::Vector3 gyro;
// 加速度を代入
accel.x = data.accel.x;
accel.y = data.accel.y;
accel.z = data.accel.z;
// 角速度を代入
gyro.x = data.gyro.x;
gyro.y = data.gyro.y;
gyro.z = data.gyro.z;
// Topic として送信
imu_data.linear = accel;
imu_data.angular = gyro;
pub.publish(imu_data);
実行
ここまで実装したものをテストします。
まず、単体での動作です。画面上に加速度と角速度の値を表示できていることが、確認できました。追加で、バッテリー残量とネットワークステータスの表示機能、キャリブレーション用ボタンも実装しています。
続いて、PC と接続した様子です。ROS 2 の RQt を利用して、M5Stack から出力されている値を受信し、グラフで表示しました。左側が加速度、右側が角速度です。それぞれ動かしたタイミングで値が変化していることを確認できました。
まとめ
今回は、M5Stack と mros2 を用いて、6軸 IMU の情報を Topic として送信、PC から ROS 2 上で取得しました。これを利用することで、「箱」の状態を PC 上で可視化できるようになりました。
