はじめに

概要

M5Stack製のStackChan（スタックチャン）をアールティの小型移動ロボットRaspberry Pi Mouse（ラズパイマウス）に搭載して、 スタックチャンのカメラを使った顔の検出と追従を行いました。

スタックチャンのカメラ画像をmicro-ROSでトピックとして配信、 ラズパイマウス側のROS 2で画像を受け取ってOpenCVで顔を検出、 顔の位置が画像の中心かつ顔の大きさが範囲内に収まるようにスタックチャンとラズパイマウスを動かします。

注意： ちなみにラズパイマウスは自社製品ですが今回は個人開発です。宣伝とかではないです。

これまでにやったこと、今回やること

まずラズパイマウスをROS 2で動かせるようにセットアップしました。

Raspberry Pi Mouseをセットアップ

次にmicro-ROSとArduinoを使ってROS 2のトピック経由でスタックチャンのモータを動かせるようにしました。

スタックチャンをmicro-ROSで動かす - Arduino編

そして今回はスタックチャンのカメラ画像をトピックで配信してOpenCVによる顔の検出を行い、 スタックチャンとラズパイマウスのモータを動かして顔を追従させます。

開発環境

開発環境は前回と同様なので前回の記事を参照ください。

スタックチャンをmicro-ROSで動かす - Arduino編（再掲）

スタックチャンのカメラ画像をトピックで配信

まずはスタックチャンのカメラから画像を取得してmicro-ROSで配信します。

カメラ画像の取得方法は下記の公式のサンプルプログラムを参考にしました。

M5Stack - StackChan Camera カメラ

実際に作成したプログラムは下記です（だいたいはGeminiに書いてもらいました）。

GitHub - stack-chan_micro-ros_arduino/camera_node.ino

注意点としては画像は比較的にデータサイズが大きいため、マイコンで処理する場合は扱う画像を小さくするなどの対策が必要になることがあります。 また、micro-ROSで画像をトピックとして配信するわけですが、ここでもデータサイズに制限があってあまり大きなデータは扱えないようです。

そのため今回は.frame_size = FRAMESIZE_QQVGAのように解像度は160x120に設定して、 fmt2jpg()でjpg形式に圧縮してからCompressedImageとして配信しています。 画像がグレースケールで問題なければPIXFORMAT_GRAYSCALEを設定したり、 解像度がもっと小さくてもよければFRAMESIZE_96X96を設定したりもできます。 また、配信周期は一応5Hzに設定していますが、実際は1～2Hz程度になっていました。

以上のプログラムでスタックチャンのカメラ画像を/stackchan/camera/image/compressedというトピック名で配信できました。

スタックチャンにプログラムを書き込んでmicro-ROS agentを起動した後、rqtを使って配信されているカメラ画像を確認できました。 実行方法は下記のREADMEを参照ください。

GitHub - stack-chan_micro-ROS_arduino/README#camera

顔を検出してスタックチャンとラズパイマウスで追従する

カメラ画像が取得できたので本題の顔の検出と追従を行います。 ROS 2のトピックのpublish/subscribeを行うので専用のROS 2パッケージを作成しました。 下記のGitHubリポジトリとして公開しています。

GitHub - YusukeKato/stackchan_on_raspimouse

処理の流れとしては下記の通りです。

  1. スタックチャン側でカメラ画像を取得
  2. micro-ROSを使ってトピックとして配信
  3. PC側のROS 2でカメラ画像を受け取る
  4. OpenCVで顔を検出
  5. 画像内の顔の位置と大きさに応じてスタックチャンのモータの目標位置とラズパイマウスの速度の指令を配信
  6. スタックチャン側でモータの目標位置を受け取ってモータを制御
  7. ラズパイマウス側で速度指令を受け取ってモータを制御

これまでの作業で1,2,6,7はすでにできるようになっているため、 ここでは3,4,5の手順を行うROS 2のノードの作成が必要です。 そして実際に作成したノードは下記となります（だいたいはGeminiに書いてもらいました）。

GitHub - stackchan_on_raspimouse/face_tracker.py

OpenCVのPythonによる顔検出サンプルは下記にあります。 OpenCVを使うと顔検出のような画像認識系の機能が簡単に実装できてとても便利です。

GitHub - opencv/facedetect.py

画像内の顔の位置が検出できたら画像中心の位置との差分を取ってスタックチャンのモータの目標位置を計算しています（P制御）。 ラズパイマウスの速度指令も同様に計算しています。 今回はラズパイマウスを前後方向にだけ動くようにします（あまり広い範囲を動かせる環境がなかったため）。

下記のようにPゲインや顔の大きさの範囲をパラメータとして設定していて実際の動作に合わせて調整できます。

# stackchan param
self.kp_yaw = 2.0
self.kp_pitch = 2.0
# raspimouse param
self.target_face_width = 90.0
self.kp_linear = 0.003
self.deadband = 5.0

あとは実際にロボットを動かすので安全のため指令値に制限をかけています。

# 例：スタックチャン
target_yaw = max(-1280, min(1280, target_yaw))
target_pitch = max(0, min(900, target_pitch))
# 例：ラズパイマウス
cmd_vel_msg.twist.linear.x = max(-0.15, min(0.15, cmd_vel_msg.twist.linear.x))

以上の流れで顔を検出してモータへ指令値を出すROS 2のノードが作成できました。

それでは実際に動かしてみます。 動作方法はそれぞれのGitHubリポジトリを参照ください。

注意： 実際にロボットを動かす際はロボットが衝突したり落下したり何かを巻き込んだりなど危険な場合があるため注意してください

実際に動かしてみた様子が下記のGIF画像になります。 スタックチャンのカメラの画角内に顔が入るとスタックチャンとラズパイマウスが動くことが確認できました。

ちなみに、このGIF画像は比較的うまく動いた時の様子です。 やはり画像トピックの配信周期が1～2Hz程度のため動きはかなりカクカクしています。 あとはカメラの画角の範囲が狭いので基本的には真正面付近ではないと顔が検出できません。 とりあえず動いたのは良かったのですが改善の余地は大いにあると思います。

以上です。

おわりに

スタックチャンを購入した時に目標としていたラズパイマウスとの連携がようやく実現できました。 ひとまずは思っていたことができて良かったです。 ただ動きとしてはまだまだ改善できると思うので余裕があればやりたいと思います。 また、スタックチャンには他にもいろいろセンサとかスピーカーとか付いているので、 他の遊び方もできたらと思います。 それでは、また。