M5Stackを使ってDJI RoboMaster S1を半田付けなしでROS2対応ロボットにする

# まだ修正するかもです。

# はじめに

かなり期間があいてしまいましたが、コロナが始まる前にDJI RoboMaster S1を使って、企業対抗でロボット対戦の大会を行おうとしておりました。プラットフォームとしてDJI RoboMaster S1をハックして動かすというのをやってきたわけですが、はんだ付けなどを行う必要があり、ハードルが高いということもあるのか、あまり広まりませんでした。。。

そこで今回、一般の方でも気軽にはんだ付けをしなくても、RoboMaster S1をROS2対応して、ROS2の勉強とかロボットの勉強に活用できるようにしてみました。しかもみんな大好きM5Stackなので、きっと皆さんもやりやすいはず。。。


# 準備するもの

 - DJI RoboMaster S1
 - M5Stack Core2 （ほかのものでもESP32が入っているタイプであればOKなはず）
 - M5Stack用CAN-BUSユニット（CA-IS3050G）
 - 2x2ヘッダーコネクタ（なくても何とかなるかも）
 - 2.5mmピッチヘッダー用のピン2本
 - カシメ工具
 - ドライバー
 - PC（Arduino IDEをインストールしたもの）

ちょっとカシメ工具がいるのでまだハードルが高いと思われるかもですが、まあカシメくらいは何とかしてください。。。

# M5Stackの開発環境の準備

M5Stackの開発環境についてはこれまでいろいろな方が記事にされていると思いますので、それを参照していただければと思うのですが、今回micro-ROSを使用するため、記事執筆時点で最もCAN周り含めてライブラリが充実しているArduino IDEを用いた方法で行います。

## 1. Arduino IDEのダウンロードとインストール

Arduino IDEは最近最新の2.0系統がリリースされましたので、こちらをベースに説明してきます。

## 2. ボードマネージャでESP32/M5Stackを使えるようにする

ESP32/M5Stackに対応させるためには、外部サイトのURLをボードマネージャ追加する必要があります。PreferencesのAdditional boards manager URIsに以下のURLを追加します。

```
https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/arduino/package_m5stack_index.json
```

すると、ボードマネージャからM5Stackで検索をかけると、M5Stackシリーズのボードを追加することができます。


## 3. ESP32・CAN用のライブラリのセットアップ

ESP32やCANをArduino IDEで扱うためには、追加でライブラリを追加する必要があります。今回は、M5Stack avatarなども使っているため、以下のライブラリをインストールします。

- M5Stack by M5Stack
- M5Unified by M5Stack
- M5Atom by M5Stack　（持っているものに合わせてインストール）
- M5Core2 by M5Stack　（持っているものに合わせてインストール）
- M5Stack_Avatar by Shinya Ishikawa
- CAN by Sandeep Mistry

ここで、CANのライブラリについては、PINコンフィグを使用するボードに合わせて変更します。今回M5Stack Core2を使用するので、その場合は以下のようになります。

```
#define DEFAULT_CAN_TX_PIN GPIO_NUM_32
#define DEFAULT_CAN_RX_PIN GPIO_NUM_33
```

## 4. micro-ROSのライブラリのインストール

micro-ROSに関しては、ライブラリマネージャからインストールできないため、以下のGithubからZIPファイルでダウンロードして、手動でインストールします。

https://github.com/micro-ROS/micro_ros_arduino

現状はFoxyでしか動作確認をしていないので、FoxyブランチにしてZIPファイルをダウンロードしてください。

## 5. 必要なPythonのライブラリのインストール

```
sudo pip3 install pyserial
```

## 6. GithubからRoboStackS1（今回作成したファイル）をダウンロード

ここで、Githubから今回作成したRoboStackS1なるDJI RoboMaster S1をM5StackからCAN経由で制御するファイルをCloneもしくはZIPをダウンロードしてきます。

```
git clone https://github.com/RoboMasterS1Challenge/robostacks1.git
```


中には3種類のファイルが存在します。それぞれ以下のような機能を含んでいますが、ものによってはM5Atomなどでは動かないのでご注意ください。

 - RoboStackS1：PS4のコントローラーで動かせる（M5StackとPS4をBTで接続する必要あり）
 - RoboStackS1_avatar：M5Stackの液晶にAvatarが出せます
 - RoboStackS1_micro-ros：micro-ROS用のinoファイル。今回はこれを使います。

## 7. ビルドとM5Stackへの書き込み

RoboStackS1_micro-rosの一番上にあるM5Stackを接続するWiFi情報をそれぞれの環境に合わせて書き換えます。

```
#define MICRO_ROS_SSID "YOUR SSID"
#define MICRO_ROS_PASSWD "XXXXXXXX"
#define MICRO_ROS_SERVER_IP "YOUR IP"
#define MICRO_ROS_SERVER_PORT 8888
```

ライブラリ類をインストールできていると、RoboStackS1_micro-rosの中のinoファイルをコンパイルすることができると思います。書き込みについても問題なく行われると思います。

## 8. ROS2側のAgentの準備

ROS2のインストールなどは今回割愛しますが、micro-ROSのGitHubからFoxy用をCloneしてきてビルドしておきます。

https://github.com/micro-ROS/micro_ros_setup.git

```
sudo apt install python3-vcstool python3-rosdep
sudo rosdep init
rosdep update
```


あとはDocumentに従って以下のようにビルドすればOKです。

```
ros2 run micro_ros_setup create_agent_ws.sh
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash
ros2 run micro_ros_agent micro_ros_agent udp4 -p 8888
```



# RoboMaster S1と接続するケーブルの作成

今回、M5Stackを使用するので、電源は基本的に内臓のバッテリーで動かすため、CANのケーブルのみをM5StackのCANアダプターに接続します。今回はRoboMaster S1上部に存在するインテリジェントコントローラーから４ピンのケーブルを外して、そのケーブルにピンを差し込んで使用します。RoboMaster S1のピンは、2.54mmピッチのヘッダと同じ間隔なので、一般的に販売しているヘッダーピンを指すこむだけでOKです。

![Slide1.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/179542/2a6c43b8-7a60-31f4-4292-3644661cea65.jpeg)

こんな感じになります。（水色がCAN H/白がCAN L）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/179542/e89fc406-e03a-1820-5a13-6c6859006a24.png)

# RoboMaster S1本体と接続して動作させる

あとは用意したものを接続するだけです。接続したあとに、RoboMaster S1を起動させてからM5Stackをリセットします。すると、しばらくすると、RoboMaster S1のモーションコントローラのLEDが青色の点滅となり、正常に通信できている状態となります。

ROS2のmicro-ROS Agentと通信できると、以下のトピックがM5Stackから送られてきていることがわかります。commandとなっているのはPC側からTopicをPublishすると、その支持に合わせて動作します。

```
ros2 topic list
/joint_states
/parameter_events
/robomasters1/command/blaster
/robomasters1/command/cmd_vel
/robomasters1/command/control_mode
/robomasters1/command/led
/robomasters1/status/chassis_info1
/robomasters1/status/chassis_info2
/robomasters1/status/imu
/robomasters1/status/wheel
/rosout
/tf
```

例えば以下のコマンドを送信するとLEDの色が変化します。

```
ros2 topic pub --rate 1 /robomasters1/command/led std_msgs/msg/ColorRGBA "{r: 0.0, g: 0.0, b: 255.0, a: 0.0}"
```




# 

# 技術的な話

## CANについて

RoboMaster S1はCANで各ユニット通しが通信していることは以前に記事にしました。その中で、通常のCANの使い方と異なりDJIはまるでUARTのようにCANのデータ長関係なく、バーストで転送するというヤバイ使い方をしていました。実はこれがこれまで、よくあるCANツールでRoboMaster S1をコントロールできなかった原因です。

一般的にArduinoとかで使われるCANのコントローラにMicroChip社製のSPI接続のものがあります。これらは非常に使いやすくCANの入門用にはよいのですが、SPI通信が原因なのかかなりの頻度でCANのメッセージがくる場合、取りこぼしが起きてしまいます。

少々の取りこぼしであればさほど問題にはなりませんが、先ほど書いたように、DJI RoboMaster S1は複数のCANのメッセージにまたがってUARTのようにデータを送信するため、途中の1個でも取りこぼしてしまうとデータを使うことができません。なので、かなり取りこぼしは少なくないといけないのですが、MicroChipのSPI接続のものはほぼほぼ取れない状況になってしまいます。

そこで、一般的にBufferが潤沢で安定して動作するといわれるNXP製のSJA1000シリーズか、マイコンに内蔵しているCANコントローラで割り込み処理で取りこぼさないようにプログラムを組む必要が出てきます。ただ、マイコンを用いたものは、CAN周りのマイコンのプログラミングはなかなか複雑でハードルが高いわけです。

今回ESP32を使っているM5Stackを使用したのですが、ESP32には内蔵のCANコントローラとしてSJA1000のクローンが入っているという情報を得たため、これならいけるのではと試したところ、安定して取得できることが確認できたため、M5Stackで簡単にRoboMaster S1とCANのデータをやり取りできるとなったわけです。

## マイコンのROS2への対応について

マイコン系のデバイスをROS2の環境でコントロールするためには、一般的に2つの方法があります。

- mROS2
- micro-ROS

mROS2は東京大学の高瀬先生が中心となって開発されている、ROS2の世界とマイコンをつなぐライブラリとなっています。ですので、こちらを使おうと考えていたのですが、ESP32には対応しておらず泣く泣く今回は使用しませんでした。

今回は、micro-ROSというPCにAgentを立ち上げてAgentとmicoro-ROS間をXRCE-DDSで通信させることで、マイコンとROS2の世界をつなげられるライブラリを使用しました。これはmROS2のように単体でROS2ノードとしてふるまうことができない点で使いにくさはあるものの、対応デバイスにESP32が含まれているなど、今回の目的には問題なく使用できるため、”今回は”micro-ROSを採用しました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/179542/70fdf7b4-e77f-5c89-9f65-545a3be00771.png)

とか書いているとnomumuさんが最新のmicro-ROSのembededRTPS対応について記載されていましたので、今回の私の記事のものをいろいろ改造すれば、AgentレスでのROS2ノード化が可能かもしれません。（今度試してみます。）

https://zenn.dev/nomumu/articles/2818649457c880


## DJI RoboMaster S1の3Dデータの取得について

著作権？意匠権？の関係もあり、今回私が公開したものの中に、DJI RoboMaster S1の3Dデータは付属していません。しかし、私が公開している動画にはなぜかRoboMaster S1の3Dデータが登場します。これは、私がDJIの社員だから手に入れられたとかではなく（むしろ競合という話も。。。）、DJIが公開しているWindows用のアプリがUnityで作成されており、個人の使用の範囲内でアプリに含まれるAssetデータから3Dデータを拝借して、いろいろと座標変換や部品に分解をするなどして使ったものです。詳しいやり方は以前私が記事にしたこちらを見ていただいて、頑張ってください。

https://qiita.com/tatsuyai713/items/92a5f2e7d3fb8d404b11