MATLAB/Simulink Homeの自動コード生成を使ってモーター制御ROSノードを作成したかった

前回からのあらすじ

前回は、MATLAB/Simulink Advent CalenderにMATLAB/Simulink Homeの自動コード生成（追加ツールボックス必要なし）を使ってCAN通信を実現するという内容で書きましたが、今回はROSを使って実際にモータを動かしてみたいとおもいます。

###MATLAB/SimulinkとROS
　MATLAB/Simulinkを開発しているMathworks社はROSへの積極的な対応を進めており、Robotics System Toolboxなどを"有償で"提供しています。MATLAB Homeを使えば、社会人でも比較的安価に利用できるので、制御系の勉強などに利用するのも良いかと思います。
　しかしながら、ROS2への対応については、MATLAB/SimulinkでROS2ノードを動かしてみるで書いたとおり非常に残念なことになっています。この原因は、ROS2からMATLAB本体の中にROSのライブラリ群を保持するようになってしまったため、サポートが非常に遅れてしまう原因となっています。ROS1に対しては、MATLAB/Simulinkのコード生成機能を使って、自前のROS環境でビルドできますので、非常に使いやすくなっています。
　前回書いたとおり、コード生成機能は非常に高価であるものの、MATLAB HomeでもRaspberry Piであれば無償でコード生成機能が使用でき、それもROSノードのコードもしっかりと作ってくれます。今回はこの機能を用いてモーターを制御してみたいと思います。

###準備
　今回は以下のような構成で進めていきます。ロボット開発している人なら一家に一台持っているであろう（うそ）、DJIのRoboMaster用のモーターであるGM6020を使用します。このモーターはダイレクトドライブで非常に静かな上、トルクも1Nm以上出力でき、価格も18,000円と比較的お手頃になっています。制御の信号にはCANを使用しており、モータードライバーも内蔵しているので、CANで複数台つなげるだけで簡単に台車の制御ができます。このモータを使えばつくばチャレンジなどのロボット台車の駆動系の制作も非常に楽になりそうです。（本来はRoboMasterのジンバル用ですが。。。）

• Ubuntu PC 16.04
• MATLAB R2019b→R2019a（理由は後述）
• Raspberry Pi 4 with CAN HAT
• DJI GM6020モーター

###それではやっていきましょう。
　まずはハードウェアの準備ですが、こちらは別記事に書いてあるので、追加でやる必要があるのはDJIのモーターの準備となります。と言っても、やることは24V電源の確保とIDの設定、CANの接続をするだけですので、はしょります。。。
　続いて、ROSに対応したSimulinkのモデルを作成していきます。
まずは、GM6020にVoltageコマンドを送信してモーターを回すブロックを作成しましょう。
　前回記事に書きましたが、CANの通信を実現するSimulinkのブロックがR2019bから用意されていますが、ビルド時に不具合がでる＆CANが2CH接続するのに対応していないということで、前回と同じくCAN device driver for mcp2515を使用していきます。

#####余談
ちなみに、前回リンクエラーとなったところはROSノードとしてコード生成すると、リンクエラーが発生しません。理由は、ROSノードのビルドにはCMakeが使用されているため、適切に依存関係が自動で解決されるので、リンクエラーが発生しなくなります。

#####余談２
CAN device driver for mcp2515は実はCAN 2CH対応していません。ただ、このライブラリ自身がSPIの通信を使って書かれているので、簡単に2CH対応ができます。ということで、2CH対応したものをgithubに上げました。→https://github.com/tatsuyai713/CAN-device-driver-mcp2515-for-Arduino-and-Raspberry-Pi-Dual-CAN-Support
今回は諸般の事情で、このライブラリを使ってCAN2にDJIのモータを接続して進めていきたいと思います。

　余談はこれくらいにして、以下のようなSimulinkを作成してみましょう。

こちらのブロックはROSのメッセージをサブスクライブして、そのデータをバイト列にしてCAN送信するという内容になっています。
注意点は以下のとおりです。

• ROSのSubscriberブロックは通常のC++のノードで書くようなCallbackには対応していないので、Pollingでしかデータを取得できない仕様になっている
• 取得したデータはたとえ要素数が１つでもBusとなっているためBusからデータを取り出す必要がある
• CANはByte列の送信であるため、Byte Packブロックでバイト列にしてVectorにほうり込んで送信しましょう
• CAN TransmitのブロックやCAN ReceiveのブロックはSPI通信を順次おこなうので、同時並行での実行はできないため、in端子とout端子を処理順に繋いでおきます（今回はまだ一個なので必要ないです）
• CANの設定は通信速度は1MHzで使っているHATモジュールに合わせてSPIクロックを設定しましょう

Simulinkのコンフィグは以下のような感じです。

ビルドするときには、Build and RunではなくBuild and Loadにしましょう。（勝手にビルド後に実行してしまうので。。。）
あとは、注意点としては、Localでroscoreを立ち上げておかないとコード生成すらできませんので、とりあえずroscoreを立ち上げておきましょう。

ビルドを実行するとコード生成が行われて、Raspberry Piにtgzがsshで送られ、展開してビルドまでを一括して実行してくれます。

が、が、がががががｇ

　そこは期待を裏切らないMathworksさんで、R2019bでRaspberry Piを使ってROSノードを作成しようとすると、なんとROS ToolboxがRaspberry Piのサポートをやめてしまったようで、コード生成ができなくなっています。（２０１９年１２月現在）

　これは、ダメです、絶対ダメです、MathworksさんはROSのコミュニティにケンカでもふっかけているのか、世の中の流れに逆行して、サポートを切ってしまいました。。。なんとかしてください。まじで。。。
　ということで、R2019bはROS/ROS2を使っている人たちにには使い物にならないので、さっさと消去してR2019aに乗り換え（戻り）ましょう。

　R2019aで仕切り直しということで、R2019aでビルドを行うとスムーズにビルドまで行われま。。。せん。

　
あーlibxmlrpcが無いようです。。。

$ sudo apt install libxmlrpcpp-dev

これでうまく行くようになりま、、、、せんっ！

libconsole関連がないとのことですね。。。

$ sudo apt install librosconsole-dev

これでやっとビルドできるようになります。

ビルドできたわけですが、このSimulinkのビルドのスクリプトでは、catkin_ws下のdevel/lib/に実行ファイルを作成するだけで、installへのコピーもlaunchもありません。なので、launchコマンドでlaunchする場合は、別途launchのみを書いたパッケージを作っておきましょう。

　あと、もうひとつ重大な問題があり、このRaspberry Pi用のコード生成機能では、最後にRaspberry Pi上からソースコードを削除されてしまいます。つまり、コード生成の実行後にsshでRaspberry Piに入って、catkin_makeを実行すると、ビルドエラーになります。。。なので、コード生成実行後は、src下のコード生成したフォルダを手動で削除しないといけません。（めんどくさい。。。）

とりあえず今回はLaunch書くのもめんどくさいので、実行だけしてみます。

$ roscore&
$ cd ~/catkin_ws
$ cd ./devel/lib/qiita1/qiita1_node &

これであとはrostopic pubで指令値を与えていきましょう。

$ rostopic pub -r 10 /gm6020_1/cmd_voltage std_msgs/Int16 "data: 10000"

これでモーターが回ればOKです。

で、今度は以下のようにちゃんとしたモデルを作成してみます。

　このモデルは、ROSの世界からVoltageの指示を受けるとそれをCANで送信し、モーターから出力されている、各種の角度や速度などのデータをCANから取得してROSトピックとして出力しています。先程も述べたように、CallbackでROSトピックを取得できないので、ROSトピックの取得のみ、周期0.001secにして、他の処理は0.01sec周期で動作するようにしています。

#####余談３
　ここでまたまた余談のお時間です。このモデルを見ていただくとわかるのですが、ROSパラメータは使用していません。ROSパラメータの通信用のBlockは用意されているものの使っていません。それは、ROSパラメータの場合、例えば周期が0.01だとするとパラメータを得るために0.01周期でrosmasterに問い合わせが発生してしまいます。それによってrosmasterの負荷が高くなってしまいます。ROSパラメータを使用する場合は、起動時に一回だけ取得するようにSimulinkモデルを組みましょう。

　さてこのしっかり作ったモデルからコード生成を実行してみましょう。

・・・
・・・
・・・
・・・

えー、予想通りダメです。
これは何かというと、32bitの信号に何か掛け算などで32bitを超えると判断されると、Simulinkのコード生成は型のチェックを行うようです。このときlong型は、32bitのKernelだと32bit長で64bitのカーネルだと64bitなので、残念ながら今回Raspberry Pi4を使用していることもあり、64bitカーネルのため、longの型のサイズを64bitと設定する必要があります。
　ということで、long型を64bitに設定しましょう。

(　ﾟдﾟ) ・・・
　
(つд⊂)ｺﾞｼｺﾞｼ
　
(；ﾟдﾟ) ・・・
　
(つд⊂)ｺﾞｼｺﾞｼｺﾞｼ
　　, .
（；ﾟ Дﾟ） …！？

設定ができない。。。。。
はあああああああああああ。

もう怒りました。制限はかければ良いというわけじゃないと思うのですが、、、、
ということで、無理やり変えます。
どうやるか。
それは、Simulinkのモデルのファイルは最近slxファイルになっています。こちらのファイルはMicrosoftのOfficeシリーズと同じで、各種のXMLファイルなどをZIP圧縮したものになっています。
ということは・・・

はいそうです。解凍しますｗ

解凍すろと以下のファイル構成になっています。

で、

$ vi simulink/configSet0.xml

で、

<P Name="ProdBitPerLong">32</P>
を
<P Name="ProdBitPerLong">64</P>

に書き換えます。そしてZIPで圧縮しなおして拡張子をslxに戻せばOKです。
認識しない場合は、ファイル構造をもう一度確認してみてください。直下にファイルがないといけないので注意が必要です。

はい、できました。
これでビルドも完璧です。
これで、GM6020モーターのCAN通信を完全にブリッジできるROSノードが作成できました。

###PID制御によるモーターの位置制御
　さてここで、やっとモーターの制御に入れるわけですが、もう記事も長くなったので、次回にしたいと思います。

こんな感じのSimulinkモデルを作成しました。いろいろノウハウが詰まったものとなっていますので、参考にしてください。ただまだ動かしていないので、動くかは知りません。。。

なお、これらのSimulinkモデルは以下のGithubで公開しています。
https://github.com/tatsuyai713/DJI-GM6020-Motor-Controller-for-Simulink

では、また次回。