【micro-ROS】取得したImuデータをパブリッシュする【M5 stack core 2】

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

今回は、micro-ROSを使い、m5 stack側のImuデータをROS2トピックとしてsubscribeする方法を紹介します。

前提条件

• ROS2の基本的な使用方法が分かっている（node, publisher, subscriber）

動作環境

• PC:ノートpcやラズパイなどの制御ボード(micro-ROS agentを起動するために使用)
• OS : ubuntu 22.04
• ROS version : humble
• m5 stack core 2
• VScode
  • 拡張機能に plantformioを使用

micro-ROS agentのセットアップ

最初に、micro-ROS agentのセットアップが済んでいない方は、以下記事の「3, micro-ROS-Agebtを導入」の章を参考にセットアップを行ってください。なお、今回はros humble環境で動作することにします。

m5 stackのコード実装

次に、m5stackに「メッセージをsubscribeした時に表情を変更する」コードを作成します。

今回は、vscode ＋platformIOを使用します。

最初に、拡張機能のplantformioをインストールしてください。

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

プロジェクトは以下のように設定してください。
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
	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>

// micro-ROS
#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/imu.h>

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

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

// 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の各変数を定義
// publisher
rcl_publisher_t publisher_imu;

// message
sensor_msgs__msg__Imu imu_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)){}}

// Imuデータを格納する変数
float accX = 0.0F; 
float accY = 0.0F; 
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;    

// 時刻取得
extern "C" int clock_gettime(clockid_t unused, struct timespec *tp);

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

/**
 * @brief  指定したSSIDとパスワードでWiFiに接続する
 * @param  ssid: 接続するWiFiのSSID
 * @param  password: 接続するWiFiのパスワード
 * @return なし
*/
void wifi_connect(const char *ssid, const char *password)
{
  unsigned long connect_start_time = millis();
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    M5.Lcd.printf("WiFi connecting...\n");
    delay(500);
    // 一定時間経過してもWiFiに接続できない場合、エラーを表示
    if (millis() - connect_start_time > 10000)
    {
      M5.Lcd.clear();
      M5.Lcd.printf("WiFi connection failed\n");
      delay(1000);
      return;
    }
  }
  M5.Lcd.clear();
}

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

  // LCDに表示する文字サイズを設定
  M5.Lcd.setTextSize(2);

  // imuの初期化
  M5.Imu.init();

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

  // WiFiに接続
  if(ros_connect_mode == ROS_WIFI_CONNECT_MODE)
  {
    wifi_connect((char*)WIFI_SSID.c_str(), (char*)WIFI_PASS.c_str());
  }

  // 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));

  // Publisherを作成
  // imuデータを送信するためのpublisher
  RCCHECK(rclc_publisher_init_default(
    &publisher_imu,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
    "/imu"));

  // 姿勢は取得しないので、共分散行列の0番目に-1をセット（ROS2のImuメッセージのドキュメントを参照）
  imu_msg.orientation_covariance[0] = -1;
}

void loop()
{
  M5.Lcd.setCursor(0, 0);

  // Imuデータを取得
  M5.Imu.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.Imu.getAccelData(&accX,&accY,&accZ); 

  // degree/sec^2 → radian/sec^2に変換（ROS2のImuメッセージのドキュメントを参照）
  gyroX = gyroX * 0.0174533F;
  gyroY = gyroY * 0.0174533F;
  gyroZ = gyroZ * 0.0174533F;

  // G → m/sec^2に変換（ROS2のImuメッセージのドキュメントを参照）
  accX = accX * 9.8F;
  accY = accY * 9.8F;
  accZ = accZ * 9.8F;

  // 時刻を取得 
  timespec ts = {0};
  clock_gettime(CLOCK_REALTIME, &ts);

  // メッセージにIMUデータをセット
  imu_msg.angular_velocity.x = (double)gyroX;
  imu_msg.angular_velocity.y = (double)gyroY;
  imu_msg.angular_velocity.z = (double)gyroZ;
  imu_msg.linear_acceleration.x = (double)accX;
  imu_msg.linear_acceleration.y = (double)accY;
  imu_msg.linear_acceleration.z = (double)accZ;
  imu_msg.header.stamp.nanosec = ts.tv_nsec;
  imu_msg.header.stamp.sec = ts.tv_sec;

  // メッセージを送信
  RCSOFTCHECK(rcl_publish(&publisher_imu, &imu_msg, NULL));

  // 送信したデータをモニタ上に表示
  M5.Lcd.printf("gyroX: %f\n", gyroX);
  M5.Lcd.printf("gyroY: %f\n", gyroY);
  M5.Lcd.printf("gyroZ: %f\n", gyroZ);
  M5.Lcd.printf("accX: %f\n", accX);
  M5.Lcd.printf("accY: %f\n", accY);
  M5.Lcd.printf("accZ: %f\n", accZ);

  delay(10);
}
 

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

一定周期で、rosの/imuトピックに対してsensor_msgs/msg/Imu型のメッセージを配信します。

取得されるImuデータの単位などをROS2用に合わせるために単位変換などの処理をおこなっています。

詳しくは下記事をご確認ください
【ROS】sensor_msgs/Imuメッセージを扱うときに注意すべきポイント

最後に、m5stack core2にコードを書き込みます。

imuデータをsubescribeする

ここまでで、準備が完了したので実際にImuデータをsubscribeしてみましょう。

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

次に、agent側のPCで以下コマンドを実行します。

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

接続が完了すると、M5 stack core 2のディスプレイにImuのセンサ値が表示されます。

agentを起動しても、Imuのセンサ値が表示されない場合は、m5 stack core2の下部にある再起動ボタンを押してください。

最後に、以下コマンドで/imuトピックからデータがsubscribeされていることを確認してみてください。

ros2 topic echo /imu

さいごに

今回は、micro-ROSを使い、m5 stackのImuデータをsubscribeする方法を紹介しました。

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