RoboMasterS1を解析してROSで動かす！（みんなで動かそう編）

ROSConJP 2019でLT発表しました

RoboMaster S1を改造したい皆さん、こんにちは。
だいぶ前から更新が滞っており申し訳ございません。
というのも、ROSConJPでLT発表してきたので、ネタバレ防止で更新していませんでした。
タイトルが変わったので気づかれたかもしれませんが、ROSからRoboMaster S1の操作に成功しました。

ということで発表資料はこちらにおいてあります。
https://www.slideshare.net/tatsuyaishikawa507/ros-con-jp-2019-lt-tatsuya-ishikawa
こんな感じでPS4のコントローラーからgeometry_msgs::Twistで操作できるようにしました。
ビデオは下をクリック↓

Githubで公開

とりあえずReadMeを英語で書くのがめんどくさいのでQiitaで日本語で書きます。

作ったもの

DJI RoboMaster S1 ROS Bridge Package
このパッケージにはROS-RoboMasterS1のブリッジノードとRoboMasterS1に搭載するNucleo-F767ZIのファームウェアのソースコードが含まれています。

なんでNucleo

京都大学のT先生やいろいろな方面からmROSなどのマイコン側でROSノードをたてることを検討しましたが、諸般の事情（D○IのCANがクソすぎる）で、RTOS上ではCAN取りこぼしが発生したため、泣く泣くRTOSなしになりました。
で、なんでNucleoを選んだかというと、TwitterのアンケートでSTM32が人気だったのと、入手性、性能、一番重要なCANが2ChあってかつEthernetがあるということで、Nucleo-F767ZIを選びました。ただ今後micro-ROS(Github）でROS2対応することも見越して、OLIMEX STM23-E407(Strawberry Linux)も手に入れています。

ただ、Nucleoのネジ穴間の距離がまさかのRoboMasterにはじめから開いている穴ピッチと同じという奇跡が起きて、簡単に機体に装着できます。（絶対DJI社内でもNucleo使っているでコレ

で、こんなんできました

PS4コントローラーのキーマップ

• 左ジョイスティック : Linear X と Linear Y
• 右ジョイスティック : ジンバルのPitchとYaw
• 右ジョイスティックのボタンのプッシュ : ブラスターを打つ
• L2 : 押してる間だけ台車部分とジンバルが動きます

まずは材料の準備

• DJI RoboMaster S1（高いけど頑張って買ってください
• UbuntuのPC（自分のは16.04LTS）STM32CubeIDEをインストール(これを参考にどうぞBooth)
• Nucleo-F767ZI（MouserなりDigikeyなりで手配）
• ユニバーサル基板 (http://akizukidenshi.com/catalog/g/gP-09972/)
• CANトランシーバーTJA1050の乗ったBreakout基板 (https://www.amazon.co.jp/dp/B076HHVZM1)
• ワイヤーハーネス
• 2.54mmピッチのコネクタとピン(https://www.amazon.co.jp/gp/product/B071RK5STD/)
• SONY PlayStation4(R) コントローラー

あと、無線で操作したい場合は以下のものも追加で買ってください。
- Ethernet-Wifiコンバーター (USBで電源供給できるので、WLI-UTX-AG300/Cが良い)
- DC-DCコンバータ (http://akizukidenshi.com/catalog/g/gK-09981/) （Nucleoからの5V電源では足りない）

とりあえずインストール

今回は動かそう編なので、技術的なところにはあまり触れずにとりあえずやってみましょう。
PS4コントローラー用のドライバーds4drvのインストール
$ sudo pip install ds4drv

UbuntuのPCをWiFiアクセスポイント化するためにhostapdをインストール
$sudo apt install hostapd

設定ファイルを書くよ（内容は適宜使ってるPCのやつにしてちょ）
$sudo vi /etc/hostapd/hostapd.conf
interface=wlp61s0
driver=nl80211
ssid=RoboMasterS1Host
hw_mode=g
channel=7
wpa=2
wpa_passphrase=robomasters1
wpa_key_mgmt=WPA-PSK
rsn_pairwise=CCMP

GithubからCloneしてくる
$ git clone --recursive https://github.com/tatsuyai713/robomaster_s1_can_hack.git
このままではもちろん何もできないので、rosフォルダの中身を自分のROSのworkspaceに移動するなり、シンボリックリンク貼るなりしてください。

乗っ取り基板を作る

今回の方法では、インテリジェントコントローラーはそのまま生かした状態で、インテリジェントコントローラーとCANのネットワークの間にNucleoを入れます。つまりインテリジェントコントローラーのCAN指示を書き換えてしまいます。（なのでCANが2ch必要なのです）
まあ、基板作りはそれほど説明しなくても良いかと思うのですが、以下の点に注意してハンダ付けしてください。

• RoboMaster S1のインテリジェントコントローラーのCANコネクタは上2ピンがCANのHighとLowで下がバッテリー電圧とGND（電源はちゃんとテスターで確かめてください・CANはどっちがHIGHか忘れました汗、後で書きます）
• Nucleoの電源はバッテリー電圧をそのままVINに入れてもOKですし、DC-DCを介して5Vを供給してもよし
• Wifiコンバーターの電源は500mAも食うので、DC-DCから供給すること
• 通常CANは終端抵抗を適切に入れなければいけないが、今回使うBreakoutボードは終端抵抗が入っているのでそのまま繋いでOK
• CANのBreakoutボードの電源は5V
• CANのRX/TXは1ch:PD0/PD1 2ch:PB12/PB6となっているので、Nucleoの裏の文字に従ってBreakoutボードと接続
• CANのH/Lの線はツイストペアにしましょう

STM32CubeIDEでFirmwareをビルドして書き込む

さすがにこれまでは説明していられないので、これを買って勉強してください→Booth
ゆーくりっどさんありがとうございます。

さて動かしてみましょう

ネットワーク設定

ネットワーク設定ですが、Githubにあるaccess_point.shのとおり
PC：192.168.0.1
RoboMasterS1：192.168.0.2
となります。なので、WLI-UTX-AG300/Cの設定はそれに合わせて行ってください。
このスクリプトは標準のNetworkManagerを止めてIPアドレスを設定し、hostapdを立ち上げるようになっています。

まずは、PCをアクセスポイント化するわけですが、
$ sudo access_point.sh start
で動くはずですが、たまに起動に失敗するので。
$ sudo service hostapd status
で確認してFailしている場合は
$ sudo service hostapd start
で自分で立ち上げてください。

アクセスポイントを止めるときは
$ sudo access_point.sh stop
でOKです。

PCをアクセスポイント化できていればスマホなどでWifiのアクセスポイントが現れているはずです。
そうなれば、あとは説明書に従ってWLI-UTX-AG300/Cの設定をして、設定が完了したらRoboMaster S1の基板に接続しましょう。

コントローラー設定

ここらへん読んで頑張ってください
http://cryborg.hatenablog.com/entry/2016/09/19/185501

PS4のコントローラーと接続して、コントローラーのLEDが青くなっていることを確認してください。

RoboMaster S1の電源を入れて動かす！

まずはちゃんと組み立てて、スマホで動作できるところまではやっておいてください。
その後乗っ取り基板が正常に接続ができていれば、電源をONにすると通常の接続の場合と同様にモーションコントローラーのLEDが青点滅するはずです。もしうまく行かない場合はモーションコントローラーのLEDは赤点滅になります。その場合は接続を確認してください。
よくある間違いはCANのH/Lが逆だったり、NucleoとCANのトランシーバーの接続のRX/TXが逆だったりします。

電源をONにしたら、しばらくおいておいて、Wifiの通信が確立するまで待ちましょう。
その後、ROSのブリッジノードを以下の通り立ち上げます。
（みんなROS詳しいから大丈夫ですよね）
$ export ROS_MASTER_URI=http://192.168.0.1:11311
$ export ROS_IP=192.168.0.1
$ roscore&
$ roslaunch ros_robomaster_s1_bridge robomaster_s1_bridge_joy.launch

これであとはPS4のコントローラーで操作できます。
通信がたまに途切れたりするので注意してください。
きちんと通信できていると、コントローラーで操作するとそれに合わせて、NucleoのLANのコネクタの右側のLEDがチカチカと点滅します。これで通信できているか確かめられます。
LEDが点滅してい動かない場合は、RoboMasterS1側の接続不良を疑いましょう。

次回

次回は詳細な技術的な内容で書きたいと思います。しばらくお待ちください。