ESP32でMPU9250のデータを送信するROS2ノードを作る

はじめに

7月に投稿したESP32をROS2ノードにする(ros2arduino)の続きです。

シミュレーションと実機でプログラムを共通化することを目的にROSの勉強をしています。

今回は、ESP32に9軸センサモジュールMPU9250を接続することで、
物体の姿勢や加速度の情報を送信するROS2ノードを作成しました。

送信頻度に関しては、現状のところ最大50Hz程度でセンサ情報の送信ができています。
一時的に頻度が落ちることがあるので、平均すると40数Hz程度でしょう。
より良い実装がありましたら、ぜひご教授いただけましたら幸いです。

環境

以下の環境で通信を行いました。
- ESP32-Devkit-C
- MPU9250
- Arduino IDE 1.8.9
- Ubuntu 18.04
- ROS2 Dashing

MPU9250は既に製造中止されていますが、
Amazonでは依然として中華モジュールが購入できる現状です。

趣味用途としては依然として多くの場面で使える性能を持ったセンサですし、
以前より価格が下がっていますから、
購入できる内に買ってしまう方が良いような気がします。

後継品も利用しやすい形で販売されればいいんですけどね。

MPU9250をESP32から読む

SparkFun MPU-9250 Digital Motion Processor (DMP) Arduino Libraryを利用しました。
このライブラリを利用する利点は、
MPU9250に搭載されているDigital Motion Processor(DMP)の情報取得に対応していることです。

これにより、加速度・角速度のデータから推定される姿勢の情報も取得することができます。
また、センサ値の校正やフィルタリング、歩数カウント等にも対応しているため、
利用シーンに合わせてMPU9250の性能を存分に活かすことができます。

基本的な使い方を紹介します。
まず、接続するピンですが、このライブラリの既定値は以下のとおりです。(I2C通信の場合)

• SCL->22番ピン
• SDA->21番ピン

VCCに加える電圧はモジュールによって異なりますが、私が購入したモジュールでは3.3Vを与えています。

基本的な関数を以下に紹介します。

#include <SparkFunMPU9250-DMP.h>
MPU9250_DMP imu;

// センサの起動と初期設定
// setup()関数等に記載します
imu.setSensors(INV_XYZ_ACCEL | INV_XYZ_GYRO); // 使用するセンサの指定(加速度 | ジャイロ)
imu.dmpBegin(DMP_FEATURE_SEND_RAW_ACCEL |     // 加速度センサの値を取得する機能の有効化
  DMP_FEATURE_GYRO_CAL |                      // 角速度センサの校生機能の有効化(8秒間の静止の観測後、補正値を更新)
  DMP_FEATURE_SEND_CAL_GYRO |                 // 角速度センサの校生値を取得する機能の有効化
  DMP_FEATURE_6X_LP_QUAT,                     // 姿勢を取得する機能の有効化(クオータニオンで取得)
  200);                                       // 更新周波数を200Hzに設定する

// センサ値の取得
// loop()関数等に記載します
if ( imu.fifoAvailable() )                        // 保存されているデータ量を取得して、1byte以上なら読み込む。 
{
  if ( imu.dmpUpdateFifo() == INV_SUCCESS)        // DMPの更新
  {
    data[0] = (double) imu.calcQuat(imu.qw);      // 姿勢の取得
    data[1] = (double) imu.calcQuat(imu.qx);
    data[2] = (double) imu.calcQuat(imu.qy);
    data[3] = (double) imu.calcQuat(imu.qz);
    data[4] = (double) imu.calcGyro(imu.gx);      // 角速度の取得
    data[5] = (double) imu.calcGyro(imu.gy);
    data[6] = (double) imu.calcGyro(imu.gz);
    data[7] = (double) imu.calcAccel(imu.ax);     // 加速度の取得
    data[8] = (double) imu.calcAccel(imu.ay);
    data[9] = (double) imu.calcAccel(imu.az);
  }
}

また、ESP32がマルチタスク対応なことを活かすために、シングルタスクでは用いられることのない1番コアにimuの更新を分担させました。
ちなみに0番コアはWiFiの送受信等に既定で用いられるために、
一定間隔で通信を行う場合には高頻度で実行されるタスクを0番コアに割り当てることは得策ではありません。

タスクの割当は以下で行うことができます。

// Start timer task
xTaskCreatePinnedToCore(
  update_imu,    // 関数名
  "update_imu",  // 自由に定義できる名前
  4096,          // 使用するメモリサイズ
  NULL,          // 関数に渡す引数
  1,             // 優先度
  &th[1],        // ハンドラ(複数のタスクを指定するときに用いる)
  1              // タスクを割り当てるコアの番号
);

センサ情報を送信するROS2ノード

次に、センサ情報の送信方法について紹介します。

ROS2もROSも情報の塊を一つのmessageとして送信します。
以下の例であれば、加速度という３次元(x,y,z)の情報を一つのベクトル(Vector3)と考えて送信させています。

messageには沢山の種類があり、センサ向けのものとしては、

• std_msgs (基本的な変数、配列)
• geometry_msgs (主に多次元の情報)
• sensor_msgs（IMUなどセンサに合わせた情報のまとまり）

が主に用いられます。私は、なるべく実装が小さいほうが良いと考えて、geometry_msgsを用いています。
ros2arduinoのソースをみたところ、
MultiArray(配列)系のmessageに未対応だと考えられます。
geometry_msgs::Vector3, geometry_msgs::Quaternionはどちらも単純なfloat64の集まりです。

最後に、基本的な操作を抜き出したものを記載します。

// Publisherと送信の定期実行を定義する
// ノードを示すクラス内に記載してください
ros2::Publisher<geometry_msgs::Vector3>* publish_linear_acceleration = this->createPublisher<geometry_msgs::Vector3>("MPU9250/linear_acceleration");
this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)pubAccl, nullptr, publish_linear_acceleration);

// 定期実行される関数の中身
void pubAccl(geometry_msgs::Vector3* msg, void* arg)
{
  (void)(arg);

  msg->x = data[7];
  msg->y = data[8];
  msg->z = data[9];

  if (DISP_SERIAL){
    Serial.println("publishLinearAcceleration");
  }
}

コード

最後に、コード全体を記載します。

// Debugging
#define DISP_SERIAL false
#include <stdio.h>

// ROS2 node
#include <ros2arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "ssid"
#define SSID_PW    "password"
#define AGENT_IP   "192.168.30.2"
#define AGENT_PORT 2018 //AGENT port number
#define PUBLISH_FREQUENCY 50 //hz

// MPU9250
#include <SparkFunMPU9250-DMP.h>
MPU9250_DMP imu;
double data[10];

// Multi task
TaskHandle_t th[2];

//ROS2 node
void pubQuat(geometry_msgs::Quaternion* msg, void* arg)
{
  (void)(arg);

  msg->w = data[0];
  msg->x = data[1];
  msg->y = data[2];
  msg->z = data[3];

  if (DISP_SERIAL){
    Serial.println("publishQuaternion");
  }
}

void pubGyro(geometry_msgs::Vector3* msg, void* arg)
{
  (void)(arg);

  msg->x = data[4];
  msg->y = data[5];
  msg->z = data[6];

  if (DISP_SERIAL){
    Serial.println("publishAngularVelocity");
  }
}

void pubAccl(geometry_msgs::Vector3* msg, void* arg)
{
  (void)(arg);

  msg->x = data[7];
  msg->y = data[8];
  msg->z = data[9];

  if (DISP_SERIAL){
    Serial.println("publishLinearAcceleration");
  }
}

//ROS2 node
class ESPQuadNode : public ros2::Node
{
public:
  ESPQuadNode()
  : Node()
  {
    ros2::Publisher<geometry_msgs::Quaternion>* publish_quaternion = this->createPublisher<geometry_msgs::Quaternion>("ESPQuad/sensor/quaternion");
    ros2::Publisher<geometry_msgs::Vector3>* publish_linear_acceleration = this->createPublisher<geometry_msgs::Vector3>("ESPQuad/sensor/linear_acceleration");
    ros2::Publisher<geometry_msgs::Vector3>* publish_angular_velocity = this->createPublisher<geometry_msgs::Vector3>("ESPQuad/sensor/angular_velocity");
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)pubQuat, nullptr, publish_quaternion);
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)pubAccl, nullptr, publish_linear_acceleration);
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)pubGyro, nullptr, publish_angular_velocity);
  }
};

void update_imu(void *pvParameters)
{

  while(1)
  {
    if ( imu.fifoAvailable() )
    {
      // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
      if ( imu.dmpUpdateFifo() == INV_SUCCESS)
      {
        data[0] = (double) imu.calcQuat(imu.qw);
        data[1] = (double) imu.calcQuat(imu.qx);
        data[2] = (double) imu.calcQuat(imu.qy);
        data[3] = (double) imu.calcQuat(imu.qz);
        data[4] = (double) imu.calcGyro(imu.gx);
        data[5] = (double) imu.calcGyro(imu.gy);
        data[6] = (double) imu.calcGyro(imu.gz);
        data[7] = (double) imu.calcAccel(imu.ax);
        data[8] = (double) imu.calcAccel(imu.ay);
        data[9] = (double) imu.calcAccel(imu.az);

        if (DISP_SERIAL){
          Serial.println("------------------");
          Serial.print("IMU/Quat: [");
          for(size_t i=0; i<4; i++){          
            Serial.print(data[i]);
            Serial.print(", ");
          }
          Serial.println(" ]");

          Serial.print("IMU/Gyro: [");
          for(size_t i=0; i<3; i++){          
            Serial.print(data[i+4]);
            Serial.print(", ");
          }
          Serial.println(" ]");

          Serial.print("IMU/Accl: [");
          for(size_t i=0; i<3; i++){          
            Serial.print(data[i+7]);
            Serial.print(", ");
          }
          Serial.println(" ]");
        }
      }
    }
    delay(1);
  }
}

// ROS2 node
WiFiUDP udp;

void setup() 
{
  Serial.begin(115200);
  Serial.println("Setup started.");

  // MPU9250

  Serial.println("-- IMU.");
  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      Serial.println("Unable to communicate with MPU-9250");
      Serial.println("Check connections, and try again.");
      Serial.println();
      delay(1000);
    }
  }

  imu.setSensors(INV_XYZ_ACCEL | INV_XYZ_GYRO);
  imu.dmpBegin(DMP_FEATURE_SEND_RAW_ACCEL |
              DMP_FEATURE_GYRO_CAL |
              DMP_FEATURE_SEND_CAL_GYRO |
              DMP_FEATURE_6X_LP_QUAT,
              200);


//  // ROS2 node
  Serial.println("-- ROS.");
  WiFi.begin(SSID, SSID_PW);
  while(WiFi.status() != WL_CONNECTED){
    Serial.println("Unable to communicate with WiFi Access Point");
    Serial.println("Check connections, and try again.");
    Serial.println();
    delay(1000);
  }
  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  Serial.println("Setup completed!");

  // Start timer task
  xTaskCreatePinnedToCore(
      update_imu,
      "update_imu",
      4096,
      NULL,
      1,
      &th[1],
      1
  );

void loop() 
{
  static ESPQuadNode espNode;
  ros2::spin(&espNode);
}

実行結果

現状の実装ですと、約40~50Hz程度でセンサ情報の送信が可能でした。
この程度の頻度で取得ができるのであれば、ラジコン等への実装も可能だと考えます。
また、送信頻度PUBLISH_FREQUENCYを大きくしても、この最大周波数は変化しませんでした。

IMUの更新update_imuを0番コアに割り当てたとき、送信頻度が悪化しました(20Hz程度まで下落)。
マイコンが処理しきれないためだと考えられます。

まとめ

今回は、MPU9250のセンサ情報をROS2に送信するノードをESP32とros2arduinoを用いて作成しました。

WiFi内蔵マイコンESP32を用いることで、無線を介したROS2トピックの送受信が可能となりますから、
センサやアクチュエータの情報の送受信によって、ラジコン等をPCから簡単に制御することができると期待されます。

Gazebo等のシミュレーション環境と、実機環境でのプログラム共通化が可能となることから、
開発スピードの向上が期待できるのではないでしょうか。

みなさんも、ぜひ一度ROS(ROS2)を使ってみてくださいね。

※筆者も勉強中の身です。些細なことでもご意見・コメント等いただけますと幸いです！