【micro-ROS】ROSのトピックでm5 stack core 2の表情を変える【m5stack-avatar】

お疲れ様です。秋並です。

今回は、micro-ROSを使い

• m5 stackでROSトピックからメッセージをsubscribeし、表情を変更する方法

を紹介します。

動作環境

• PC：UbuntuOSがインストール可能なPC（ラズパイなども可）
• OS： ubuntu 22.04
• ROS version： humble
• マイコン：m5 stack core 2
• ツール：VScode
  • 拡張機能に platformIO IDEを使用

前提条件

• PCにROS2がインストールされている
• ROS2の基本的な使用方法が分かっている
  • node, publisher, subscriberなどの基礎的な内容

micro-ROS agentのセットアップ

最初に、micro-ROS agentのセットアップが済んでいない方は、以下記事の「3, micro-ROS-Agebtを導入」の章を参考にセットアップを行ってください。

なお、今回はros humble環境で動作することとします。

コード実装/m5 stack core2への書き込み

次に、「メッセージをsubscribeした時に表情を変更する」コードを作成し、m5 stackに書き込みます。

最初に、vscodeを起動し、拡張機能のplatformIO IDEをインストールしてください。

次に、プロジェクトを作成します。

プロジェクトは以下のように設定してください。

• Name: 適当なプロジェクト名
• Board: 今回はM5stack core 2を使用しているので「M5Stack Core2」
• Framework: Arduino

プロジェクトの作成が完了すると、「platform.ini」というファイルが作成されるので、以下のように必要なライブラリを記載します。

[env:m5stack-core2]
platform = espressif32@6.3.2
board = m5stack-core2
framework = arduino
lib_deps = 
	m5stack/M5Unified@^0.1.16
	meganetaaan/M5Stack-Avatar@^0.8.2
	https://github.com/micro-ROS/micro_ros_arduino/archive/refs/heads/humble.zip
build_flags = 
	-L ./.pio/libdeps/micro_ros_arduino/src/esp32/
	-l microros

次に、src/main.cに以下コードを記載します。

// m5stack
#include <M5Unified.h>
#include <Avatar.h>

// micro-ROS
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int8.h>

// 定数
const int ROS_SERIAL_CONNECT_MODE = 0;
const int ROS_WIFI_CONNECT_MODE = 1;

//以下項目を、自分の環境に合わせて設定してください ###################
// WiFiのSSID（WiFi接続時のみ有効）
String WIFI_SSID = "*********";

// WiFiのパスワード（WiFi接続時のみ有効）
String WIFI_PASS = "**********";

// micro-ROS AGENTのIPアドレス（WiFi接続時のみ有効）
String ROS_AGENT_IP_ADDRESS  = "***********"; 

// micro-ROS AGENTのポート番号（WiFi接続時のみ有効）
uint ROS_AGENT_PORT = 8888; 

// ROS2の接続モード（ROS_SERIAL_CONNECT_MODE: シリアル通信、ROS_WIFI_CONNECT_MODE: WiFi通信）
int ros_connect_mode = ROS_SERIAL_CONNECT_MODE; 
//####################################################################

// ROS2の各変数を定義

// subscriber
rcl_subscription_t expression_subscriber;

// message
std_msgs__msg__Int8 expression_msg;

// executor
rclc_executor_t executor;

// support
rclc_support_t support;

// allocator
rcl_allocator_t allocator;

// node
rcl_node_t node;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

// avatarの初期化
using namespace m5avatar;
Avatar avatar;

/**
 * @brief  ROS2のチェックにエラーが出たときにエラーメッセージを表示する関数
 * @param  なし
 * @return なし
*/
void error_loop(){
  while(1){
    Serial.printf("ros2 error");
    delay(100);
  }
}

/**
 * @brief  M5Avatarの表情を変更するコールバック関数
 * @param  msgin: 受信したメッセージ（表情の種類）
 * @return なし
*/
void subscription_callback(const void * msgin)
{  
  const std_msgs__msg__Int8 * msg = (const std_msgs__msg__Int8 *)msgin;
  if (msg->data == (int8_t)Expression::Neutral)
  {
    avatar.setExpression(Expression::Neutral);
  }else if(msg->data == (int8_t)Expression::Happy)
  {
    avatar.setExpression(Expression::Happy);
  }else if(msg->data == (int8_t)Expression::Sleepy)
  {
    avatar.setExpression(Expression::Sleepy);
  }else if(msg->data == (int8_t)Expression::Doubt)
  {
    avatar.setExpression(Expression::Doubt);
  }else if(msg->data == (int8_t)Expression::Sad)
  {
    avatar.setExpression(Expression::Sad);
  }else if(msg->data == (int8_t)Expression::Angry)
  {
    avatar.setExpression(Expression::Angry);
  }
  M5.update(); 
}

void setup()
{
  // M5Stackの初期化
  M5.begin();

  // シリアル通信の初期化
  Serial.begin(115200);

  // micro-ROS agent に接続
  if(ros_connect_mode == ROS_SERIAL_CONNECT_MODE) // シリアル通信で接続
  {
    set_microros_transports();
  }
  else if(ros_connect_mode == ROS_WIFI_CONNECT_MODE) // WiFi通信で接続
  {
    set_microros_wifi_transports((char*)WIFI_SSID.c_str(), (char*)WIFI_PASS.c_str(), (char*)ROS_AGENT_IP_ADDRESS.c_str(), ROS_AGENT_PORT);
  }

  // ROS2の初期化
  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // ノードを作成
  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));

  // Subscriberを作成
  // M5Avatarの表情を取得するsubscriber
  RCCHECK(rclc_subscription_init_default(
    &expression_subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int8),
    "/expression"));

  // executorの作成
  // 今回は、1つのsubscriberを登録
  RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator)); // 第3引数は登録する（timer, subscriber, service, client, guard condition）の総数を設定
  
  // subscriberの登録：rclc_executor_add_subscription（executor, subscriber, subscriberが取得するmsg, subscribeした時に実行されるcallback, コールバックの呼び出しタイプ（ALYAYS or ON_NEW_DATA)）
  RCCHECK(rclc_executor_add_subscription(&executor, &expression_subscriber, &expression_msg, &subscription_callback, ON_NEW_DATA));

  // M5Avatarの初期化
  avatar.init();
}

void loop()
{
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
  delay(100);
}
 

上記コードを簡単に説明すると

rosの/expressionトピックからint8型のメッセージを受信すると、受信した数値に対応した表情に変化する。

という処理が実行されます。

最後に、m5stack core2にコードを書き込みます。VScodeの場合、右上の矢印ボタンをクリックすると書き込みが開始します。

上記コードをm5 stack core 2に書き込んだ時点では、顔は表示されません。
後述するmicro-ROS agentに接続された時点で表示されます。

m5 stackにメッセージをpublishし、表情を変更する

ここまでで、準備が完了したので実際にROSのトピックにメッセージを配信し、表情を変化させてみましょう。

最初に、agent側のPCとm5stackをusbで接続します。

次に、PCで以下コマンドを実行し、micro-ROS agentを起動します。

ros2 run micro_ros_agent micro_ros_agent serial --dev [ポートの名称（例：/dev/ttyACM0）] -v6

agentを起動しても、m5 stack core2に顔が表示されない場合は、m5 stack core2下部にあるボタンをクリックし、再起動してください。

m5 stack core2に顔が表示されたら、PCで別ターミナルを起動し、以下コマンドでtopicを配信します。

ros2 topic pub /expression std_msgs/Int8 "data: 1"

m5 stack core2に表示されている顔が「怒っている顔」に変化したら成功です。

data: 1 の、 1 の部分 を変更すると、対応する表情に変化します。

なお、各表情に対応する数値は以下の通りになります。
0: Happy
1: Angry
2: Sad
3: Doubt
4: Sleepy
5: Neutral

さいごに

今回は、micro-ROSを使い、m5 stack側でメッセージをsubscribeして、表情を変更する方法を紹介しました。

この記事が少しでも皆さんのお役に立てれば幸いです。