プリメイドAIをROS対応させた話

# プリメイドAIをROS対応させた話

この記事は[ROSアドベントカレンダー](https://qiita.com/advent-calendar/2019/ros) 21日目の記事です。投稿が大幅に遅れてしまいました。

さて、プリメイドAIというロボットをご存知でしょうか？ 一部の界隈で流行った2万円で買えたホビーロボットです。基本的にサーボを取るために使われるものなのですが先駆者の方々がプロトコルを解析していたのでROS対応させてみました。

作業内容はざっとこんなものです。

* URDF 作成
* ros_control 対応
* gazebo plugin 作成
* moveit! 対応

だいたいのROS化フルセットですね。趣味でやる程度ならちょうどいい負荷量です。本当は歩行やファームウェアハックまで手を伸ばしたかったのですが、年末までの作業はここまでということで。

## 動作環境

今回の作業環境は以下を対象とします。

* Ubuntu 18.04
* ROS melodic
* gazebo 9.0.0

## 環境整備

とりあえず全てのレポジトリをcloneして環境を整備します。今回は簡単に動作確認ができるようにサンプルワークスペースを用意しました。

```bash
$ source /opt/ros/melodic/setup.bash
$ mkdir -p premaidai_ws/src
$ cd premaidai_ws
$ catkin init
$ wget https://raw.githubusercontent.com/chikuta/premaidai_ws/qiita-201912/premaidai_ws.rosinstall -O .rosinstall
$ rosinstall .
$ rosdep install --from-paths src --ignore-src -y
$ catkin build
```

## Bluetooth設定と接続方法

プリメイドAIの電源をつけたあとに以下のコマンドでBD Adressを取得します。プリメイドAIのBlueToothは `RNBT-XXXX` と認識されるので、このBD Addressをメモします。ここでは `00:06:66:84:XX:XX` として表記します。

```bash
  $ hcitool scan

  Scanning ...
    00:06:66:84:XX:XX	RNBT-XXXX
```

次に接続時に使用するチャンネルを取得します。出力される `RFCOMM` の `Channel` をメモします。環境によって違いが出ると思うので以後 `${CHANNEL}` と表記します。

```bash
  $ sdptool records 00:06:66:84:XX:XX

  Service Name: RNI-SPP
  Service RecHandle: 0x10000
  Service Class ID List:
    "Serial Port" (0x1101)
  Protocol Descriptor List:
    "L2CAP" (0x0100)
    "RFCOMM" (0x0003)
      Channel: 1
  Language Base Attr List:
    code_ISO639: 0x656e
    encoding:    0x6a
    base_offset: 0x100
```

`/etc/bluetooth/rfcomm.conf` に以下を記述して再起動します。

```text
rfcomm0 {
    bind yes;
    device 00:06:66:84:XX:XX;
    channel ${CHANNEL};
    comment "serial port";
}
```

以下のコマンドを入力してプリメイドAIを接続します。

```bash
  $ sudo rfcomm release ${CHANNEL}
  $ sudo rfcomm bind rfcomm0 00:06:66:84:XX:XX ${CHANNEL}
```

この場合、 `/dev/rfcomm0` がプリメイドAIと接続がされたシリアルポートになります。


## rviz上のプリメイドAIモデルを動かす

公開しているプリメイドAIのURDFモデルを使用して画面に表示されたものを動かしてみましょう。ここで使用したモデルは [kuroiwasiさんが公開したモデル](https://github.com/kuroiwasi/PremaidAI_Model)をROS対応させたものです。公開されているモデルを各パーツごとに分解し、パーツの重量を基にイナーシャを計算してURDFに記述しています。イナーシャの計算方法は [こちらのサイト](http://gazebosim.org/tutorials?tut=inertia)を参考にして計算しています。

```bash
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_description display.launch
```

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">とりあえずプリメイドAIのURDFを作ったのでおいておきますね。 関節角のリミット設定ができていないから週末までにやる予定。<a href="https://t.co/I4NecPFsG5">https://t.co/I4NecPFsG5</a> <a href="https://t.co/7QO3DMm24n">pic.twitter.com/7QO3DMm24n</a></p>&mdash; chikuta (@chikuta) <a href="https://twitter.com/chikuta/status/1153966405475135488?ref_src=twsrc%5Etfw">July 24, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## rviz経由でプリメイドAIを動かす

次にrvizで実機を動かしてみます。[通信プロトコル](https://docs.google.com/spreadsheets/d/1c6jqMwkBroCuF74viU_q7dgSQGzacbUW4mJg-957_Rs/edit#gid=2102495394)を基にプリメイドAI用の `ros_controller` を実装したので、[Bluetooth設定と接続方法](# Bluetooth設定と接続方法) に従って接続設定を行った後に以下のコマンドを実行してください。

```bash
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_controller rviz_control.launch serial_port:=/dev/frcomm0
```

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">プリメイドAIのC++コントローラーを追加。これぐらいが無改変ファームの性能限界かな。JointStateを取得しないなら20Hzでるぐらい。動画は10Hzで目標値設定。<a href="https://t.co/tgToOzPZ6y">https://t.co/tgToOzPZ6y</a> <a href="https://t.co/YGsUsrwXmo">pic.twitter.com/YGsUsrwXmo</a></p>&mdash; chikuta (@chikuta) <a href="https://twitter.com/chikuta/status/1196003376779034625?ref_src=twsrc%5Etfw">November 17, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## 物理シュミレータでプリメイドAIを動かす

ROSの物理シュミレータgazeboを用いてプリメイドAIを動かしてみましょう。gazeboを使って物理シミュレーションをするにあたり、gazeboプラグインを作成する必要があります。[drcsim_gazebo_plugin](https://bitbucket.org/osrf/drcsim/src/default/drcsim_gazebo_plugins/)あたりを参考にして実装しています。gazebo自体が後方互換性を考えずにAPIを更新してくるので、適宜ソースコードを追いながら実装することがおすすめです。

```bash
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_gazebo rviz_control.launch
```

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">とりあえず rviz と Gazebo を使ったサンプルを作成完了。適当に公開しておきますね。 <a href="https://twitter.com/hashtag/%E3%83%97%E3%83%AA%E3%83%A1%E3%82%A4%E3%83%89AI?src=hash&amp;ref_src=twsrc%5Etfw">#プリメイドAI</a> <a href="https://t.co/B6xAsFjDLL">https://t.co/B6xAsFjDLL</a> <a href="https://t.co/sLMaYqYI5Y">pic.twitter.com/sLMaYqYI5Y</a></p>&mdash; chikuta (@chikuta) <a href="https://twitter.com/chikuta/status/1166691147118305282?ref_src=twsrc%5Etfw">August 28, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## moveit経由でプリメイドAIを動かす

ROSのプランニングフレームワークである moveit を用いてプリメイドAIを動かしてみます。[MoveIt! Setup assistant](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html)を使って設定ファイルを作成していきます。

```bash
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_controller ros_control.launch serial_port:=/dev/frcomm0
```

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">プリメイドAIとmoveit! <a href="https://t.co/TsGMy2ziDj">pic.twitter.com/TsGMy2ziDj</a></p>&mdash; chikuta (@chikuta) <a href="https://twitter.com/chikuta/status/1188352772430655488?ref_src=twsrc%5Etfw">October 27, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">プリメイドAIとmoveit。レスポンス遅すぎて使い物にならんな。目標位置はランダムで生成。 <a href="https://t.co/TnUktbxGoj">pic.twitter.com/TnUktbxGoj</a></p>&mdash; chikuta (@chikuta) <a href="https://twitter.com/chikuta/status/1196393214908854272?ref_src=twsrc%5Etfw">November 18, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

思ったよりも上手く動いていませんね。

## 今後の課題

さて、ここまでやってみてわかった問題点ですが、プリメイドAIのデフォルトファームウェアではコマンドの送受信周期が 20Hz 程度しか保証できないことです。この通信遅延はmaster-slaveで動かす場合は致命的なのでカスタムファームウェアを作る必要性が発生します。幸い、[STM32-Arduino-ICSservo](https://github.com/devemin/STM32-Arduino-ICSservo/)に搭載されたマイコンのファームウェアを書き換える方法を公開している方がいらっしゃるのでこれを参考にして作業をするとよさそうです。自分が開発したカスタムファームウェアもそのうち公開できるかと。

## 参考情報

* [PremaindAI_TechVerification](https://github.com/neon-izm/PremaindAI_TechVerification)
* [PremaindAI_Model](https://github.com/kuroiwasi/PremaidAI_Model)
* [プリメイドAIパーツ軽量](https://drive.google.com/file/d/18qxGj2l_tFbyiChA9kbnJ-sgjDKE4dCX/view)
* [STM32-Arduino-ICSservo](https://github.com/devemin/STM32-Arduino-ICSservo/)