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

はじめに

KickStarterでM5Stack製のスタックチャンを手に入れたので、 micro-ROSとROS 2、Arduino IDEを使ってスタックチャンのモータの制御と現在角度の取得をやってみようと思います。

KickStarterのM5Stack製スタックチャンの情報は下記リンクです。

KickStarter: M5Stack - スタックチャン

開発環境

下記の環境で開発しています。 ROS 2を使用するためUbuntu PCが必要になります。

PC
- Ubuntu 24.04 LTS
- ROS 2 Jazzy
- Arduino IDE 2.3.8
Arduino IDE - Boards
- esp32 3.3.8
- M5Stack 3.3.7
Arduino IDE - Library
- micro_ros_arduino 2.0.7-humble
- M5StackChan 1.0.1

M5Stack製スタックチャンのスペック

M5Stack製スタックチャンの詳細は下記リンクの公式ページで確認できます。

M5Stack製StackChan公式ページ

ケースも付属していて開封してみるとこんな感じでした↓

このスタックチャンはM5Stack CoreS3を搭載していてESP32-S3のマイコンが使用されています。通信はUSBケーブルで接続できる他にWi-FiとBluetoothなども利用することができます。インターフェースとしてタッチスクリーンが搭載されていて、画面上にログを出力もできるのでデバッグもしやすいです。

アクチュエータはサーボモータが2つあってYaw方向とPitch方向に首振りが可能、センサはカメラや9軸IMU、マイク、スピーカ、ボタン、近接センサなど様々なものが搭載されています。 microSDカードのスロットも付いています。また、バッテリは550mAhのものが搭載されています。電子工作でやりたいことは何でもできそうなくらい色々付いていて凄いですね。

Arduino IDEでプログラムを書き込む

それではArduinoの開発環境を構築してスタックチャンにプログラムを書き込んでいきます。

注意： 新しくプログラムを書き込むと出荷時に書き込まれていた標準のファームウェアは上書きされてしまうため注意してください

Arduino IDEを使ってプログラムをビルドしてスタックチャンに書き込む方法は公式でチュートリアルが用意されているため、こちらを参考にします。

M5Stack - StackChan Arduino サンプルプログラムのビルドと書き込み

Arduino IDEをインストールして起動する

まずはArduino公式サイトでArduino IDEをインストールします。今回はUbuntuを使用しているためLinux AppImageを選択してダウンロードしました。

Arduino公式サイト - Arduino IDEダウンロードページ

Ubuntu 24.04だとAppImageを開くために必要なパッケージが足りていないらしいのでインストールします。

sudo apt install libfuse2t64

Arduino IDEのAppImageに実行権限を付与してから起動します。

# 実行権限を付与
sudo chmod +x arduino-ide_2.3.8_Linux_64bit.AppImage
# --no-sandboxを付けないとエラーが出て起動できなかった
./arduino-ide_2.3.8_Linux_64bit.AppImage --no-sandbox

下図のように正常にArduino IDEが起動したら成功です。

必要なパッケージやライブラリをインストールする

Arduino IDEでスタックチャン開発に必要なパッケージやライブラリをインストールしていきます。

まずはM5Stackを開発するためBoards ManagerでM5Stackをインストールします。手順は公式チュートリアルそのままで実施します。

M5Stack - Arduino Boards Manager

次にM5Stack開発に必要なライブラリM5CoreS3等をインストールします。こちらも公式チュートリアルの手順をそのまま実施します。

M5Stack - Arduino Library

さらに追加でM5StackChanという名前のライブラリもインストールします。

最後にmicro-ROSのArduino用ライブラリmicro_ros_arduinoもインストールします。私は2.0.7-humbleバージョンをインストールしました。ちなみにmicro_ros_arduinoのGitHubリポジトリを見に行くと2.0.8-jazzyバージョンもリリースされていましたが、私の環境だと2.0.7-humbleバージョンではないとうまく動きませんでした。

micro_ros_arduinoの詳しい情報については下記のGitHubリポジトリを参照してください。

GitHub - micro_ros_arduino

以上で準備は完了です。

スタックチャンにプログラムを書き込む

それではArduino IDEを使ってスタックチャンにプログラムを書き込んでみます。

PCとスタックチャンをUSBのtype-Cケーブルで接続し、スタックチャンの電源を入れてから画面下にあるボタンをLEDが緑色に点灯するまで長押しします。

次に、Arduino IDEで公式チュートリアルに従ってボードはM5CoreS3を選択し、ポートもスタックチャンのものを選択します。私の環境では/dev/ttyACM0でした。

その後、上部タブからFile > Examplesの中にあるサンプルプログラムを選びます。公式チュートリアルではM5StackChan > RGB_LEDが紹介されています。このサンプルプログラムを書き込むとスタックチャン上部のLEDが点灯します。私はM5GFX > Basic > BarGraphを選択しました。

最後にArduino IDEの左上にある矢印ボタンを押して、プログラムのビルドとスタックチャンへの書き込みを実行します。

注意： プログラム書き込み時、ポートへの書き込み権限がなくエラーが出たため、次のコマンドを実行しました。 sudo chmod 777 /dev/ttyACM0

プログラムの書き込みに成功すると下図のように画面上にカラフルなバーが表示されました。

Arduino IDEでのプログラムの書き込み方法は以上です。

micro-ROSとROS 2でトピック通信する

それでは本題のmicro-ROSを使ってスタックチャンを動かすところをやっていきます。具体的にはPC側のROS 2とスタックチャン側のmicro-ROSでトピック通信を行います。今回はサーボモータ2つの角度をトピックで送受信（pub/sub）します。

参考情報

micro-ROSの使い方は下記のmicro_ros_arduinoのGitHubリポジトリを参考にしました。

GitHub - micro_ros_arduino

micro_ros_arduinoのサンプルプログラムもリポジトリ内に用意されています（下記リンクはkiltedブランチです）。

micro_ros_arduino/tree/kilted/examples

M5Stack製スタックチャンのArduinoライブラリのサンプルプログラムは下記GitHubリポジトリ内にあります。

GitHub - m5stack/StackChan-BSP

作成したプログラム

上記の情報を参考にしつつ、Geminiにプログラムの大枠を作成してもらい、いい感じに整えたのが下記プログラムです。

このプログラムでは、2つのサーボモータの現在角度をstackchan/joint_statesトピックでpublish、目標角度をstackchan/joint_commandsトピックでsubscribeします。角度の範囲はPitchは0～900、Yawは-1280～1280となっています。

このプログラムを使用する時はWi-FiとIPアドレスの設定を書き換えてからスタックチャンに書き込んでください。

#include <Arduino.h>
#include <M5StackChan.h>
#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32_multi_array.h>

// Wi-Fi
char ssid[] = "SSID";  // 自分のWi-FiのSSID
char password[]= "PASSWORD";  // 自分のWi-Fiのパスワード
// micro-ROS agent
char agent_ip_address[] = "IP_ADDRESS";  // PCのIPアドレス
size_t agent_port = 8888;
// motor angle pub/sub
rcl_publisher_t angle_pub;
rcl_subscription_t angle_sub;
std_msgs__msg__Int32MultiArray pub_msg;
std_msgs__msg__Int32MultiArray sub_msg;
// {Yaw angle, Pitch angle}
int32_t pub_data[2] = {0, 0};
int32_t sub_data[2] = {0, 0};
// micro-ROS
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

// target angle subscriber
void angle_subscription_callback(const void * msgin) {
  const std_msgs__msg__Int32MultiArray * msg = (const std_msgs__msg__Int32MultiArray *)msgin;
  if (msg->data.size >= 2) {
    // get target angle
    int32_t target_yaw = msg->data.data[0];
    int32_t target_pitch = msg->data.data[1];
    // limit angle
    target_yaw = max(-1280, min(1280, (int)target_yaw));
    target_pitch = max(0, min(900, (int)target_pitch));
    // move motor
    M5StackChan.Motion.move(target_yaw, target_pitch);
  }
}

// current angle publisher
void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  if (timer != NULL) {
    // get current angle
    int32_t current_yaw = M5StackChan.Motion.getCurrentXAngle();
    int32_t current_pitch = M5StackChan.Motion.getCurrentYAngle();
    // publish current angle
    pub_msg.data.data[0] = current_yaw;
    pub_msg.data.data[1] = current_pitch;
    rcl_publish(&angle_pub, &pub_msg, NULL);
    // print log
    M5StackChan.Display().setCursor(0, 80);
    M5StackChan.Display().setTextColor(TFT_WHITE, TFT_BLACK); 
    M5StackChan.Display().printf("  Yaw   : %4d    \n", current_yaw);
    M5StackChan.Display().printf("  Pitch : %4d    \n", current_pitch);
  }
}

void setup() {
  // init StackChan
  M5StackChan.begin();
  M5StackChan.Display().setTextSize(3);
  M5StackChan.Motion.goHome();
  delay(1000);
  // connect Wi-Fi
  M5StackChan.Display().printf("> Connecting Wi-Fi...\n");
  set_microros_wifi_transports(ssid, password, agent_ip_address, agent_port);
  delay(2000);
  // connect ROS 2
  M5StackChan.Display().clear();
  M5StackChan.Display().setCursor(0, 0);
  M5StackChan.Display().setTextColor(TFT_GREENYELLOW);
  M5StackChan.Display().printf("> ROS 2 Ready!\n");
  M5StackChan.Display().setTextColor(TFT_WHITE);
  // init micro-ROS
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "stackchan_node", "", &support);
  // current angle publisher
  rclc_publisher_init_default(
    &angle_pub, &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
    "stackchan/joint_states");
  // target angle subscriber
  rclc_subscription_init_default(
    &angle_sub, &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
    "stackchan/joint_commands");
  // set publisher
  pub_msg.data.capacity = 2;
  pub_msg.data.size = 2;
  pub_msg.data.data = pub_data;
  // set subscriber
  sub_msg.data.capacity = 2;
  sub_msg.data.size = 0;
  sub_msg.data.data = sub_data;
  // set publish timer
  rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(100), timer_callback);
  // set micro-ROS executor
  rclc_executor_init(&executor, &support.context, 2, &allocator);
  rclc_executor_add_subscription(&executor, &angle_sub, &sub_msg, &angle_subscription_callback, ON_NEW_DATA);
  rclc_executor_add_timer(&executor, &timer);
}

void loop() {
  M5StackChan.update();
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
}

PC側でmicro-ROSと通信するために下記コマンドでAgentを起動してください。 Dockerのインストールが必要です。

sudo docker run -it --rm --net=host microros/micro-ros-agent:jazzy udp4 --port 8888 -v6

PC側で下記コマンドを実行してスタックチャンのモータの現在角度を確認できます。手動でモータを動かすと分かりやすいです。

ros2 topic echo /stackchan/joint_states

次にPC側で下記コマンドを実行するとスタックチャンのモータを動かせます。 {data: [200, 500]}の部分で角度を指定できます。

ros2 topic pub --once /stackchan/joint_commands std_msgs/msg/Int32MultiArray "{data: [200, 500]}"

以上でmicro-ROSとROS 2でのトピック通信が確認できました。

おわりに

今回はスタックチャンをArduinoとmicro-ROSで動かしてみました。 Geminiのおかげもあってそこまで詰まることもなくmicro-ROSを導入できました。スタックチャンにはたくさんセンサが載っているのでモータ以外もトピックでpub/subしたいと思っています。あとせっかくのスタックチャンなので顔も表示してあげたいとも思っています。引き続きスタックチャン開発を楽しんでいきます。それでは、また。

はじめに#

開発環境#

M5Stack製スタックチャンのスペック#

Arduino IDEでプログラムを書き込む#

Arduino IDEをインストールして起動する#

必要なパッケージやライブラリをインストールする#

スタックチャンにプログラムを書き込む#

micro-ROSとROS 2でトピック通信する#

参考情報#

作成したプログラム#

おわりに#