VS-RC003を搭載した"KUMACO"をROSで動かす(中編)「VS-RC003を制御するROSノード」

#概要

VS-RC003というVstone社のロボット用制御ボードを搭載したロボットをROSと接続して利用するお話を投稿させていただきます。
課題としては、"KUMACO(クマコ)"という名前のぬいぐるみ型のホビーロボットを用いて、"いないいないバァ(顔が見えたら喜ぶ)"させたいと思います。

完成形はこんな感じです。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ぬいぐるみロボット、KUMACOに目が付いて、息子と”いないいないバァ”で遊べるように進化しました！<br><br>枯れたVS-RC003プラットフォームの二足ロボも、ROS化すれば命が宿ります(*⁰▿⁰*) <a href="https://t.co/EdMui2YzxN">pic.twitter.com/EdMui2YzxN</a></p>&mdash; みっちー (@Dream_Drive) <a href="https://twitter.com/Dream_Drive/status/1205626987265421312?ref_src=twsrc%5Etfw">December 13, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

前回は、ハードウェアとして、KUMACOがPCと繋がりました。
今回は、ROSノードとしてKUMACOをコントロールするお話です。

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i7-9750H |
| Ubuntu | 18.04 |
| ROS | Melodic |

# インストール
### ROSのインストール
まずはROSをインストールしましょう。
こちら"[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)"の記事がとても参考になります。

### ros2vsrcパッケージインストール
つぎに、ROSからVS-RC003をシリアル経由で制御するROSパッケージ<b>[ros2vsrc](https://github.com/dreamdrive/ros2vsrc)</b>をインストールします。
（頑張って作ってみました。)

```shell
$ cd ~/catkin_ws/src
$ git clone https://github.com/dreamdrive/ros2vsrc.git
$ cd ..
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
```
以上でインストール完了です。

# ros2vsrc/vsrc_connectの起動方法

使い方ですが、1つめのターミナルでROSMASTERを起動します。

```shell
$ roscore
```

2つめのターミナルで、vsrc_connectノードを起動します。
_serialdevに、デバイスファイルのパスを指定してください。

```shell
$ rosrun ros2vsrc vsrc_connect _serialdev:=/dev/ttyUSB0
```

これで、ROSとVS-RC003が繋がりました。

# ros2vsrc/vsrc_connectの使い方
ros2vsrc/vsrc_connectで使えるメッセージトピックは下記の通りです。

- <b>ros2vsrc/VsrcState (オリジナルメッセージ) (VS-RC003 -> ROS)</b> 
VS-RC003から読み取ったCPUの状態を配信するトピックです。
    - r_torque (bool) : 変数(251)から読み出した現在のサーボトルクの状態(ON/OFF)
    - r_mode (int32) : 変数(238)から読み出したCPUボードのモード
    - r_map (int32) : 変数(248)から読み出したマップ番号
    - r_idle (bool) : 変数(253)から読み出したアイドル中のフラグ(モーション再生中かどうか)
    - r_ctltype (int32) : 変数(240)から読み出したコントローラーの種類

- <b>ros2vsrc/VsrcControl (オリジナルメッセージ) (ROS -> VS-RC003)</b> 
VS-RC003に対して、仮想のVS-C1(コントローラー)の状態を配信するトピックです。
    - w_torque (bool) : トルクON/OFFの指示
    - s_ry (int32) : 仮想コントローラー右スティック上下
    - s_rx (int32) : 仮想コントローラー右スティック左右
    - s_ly (int32) : 仮想コントローラー左スティック上下
    - s_lx (int32) : 仮想コントローラー左スティック左右
    - b_up (bool) : 仮想コントローラー十字キー上
    - b_triangle (bool) : 仮想コントローラー△ボタン
    - b_start (bool) : 仮想コントローラーSTARTボタン
    - b_square (bool) : 仮想コントローラー□ボタン
    - b_select (bool) : 仮想コントローラーSELECTボタン
    - b_right (bool) : 仮想コントローラー十字キー
    - b_R3 (bool) : 仮想コントローラーR3ボタン
    - b_R2 (bool) : 仮想コントローラーR2ボタン
    - b_R1 (bool) : 仮想コントローラーR1ボタン
    - b_left (bool) : 仮想コントローラー十字キー左
    - b_L3 (bool) : 仮想コントローラーL3ボタン
    - b_L2 (bool) : 仮想コントローラーL2ボタン
    - b_L1 (bool) : 仮想コントローラーL1ボタン
    - b_down (bool) : 仮想コントローラー十字キー下
    - b_cross (bool) : 仮想コントローラー×ボタン
    - b_circle (bool) : 仮想コントローラー○ボタン

- <b>sensor_msgs/BatteryState</b> 
VS-RC003から読み取ったバッテリーの電圧を配信するトピックです。
 - voltage (float32) : 電源電圧(V)

以上、３つのトピックを使って、VS-RC003を制御します。

試しに、トピックを送ってKUMACOを動かしてみます。
まず、w_torqueに1を書き込むとロボットの全身のサーボがトルクオンになります。

```shell
$ rostopic pub /vsrc_control ros2vsrc/VsrcControl "w_torque: 1"
```

b_R3に1を書き込むと、R3が押された状態になり、前編で作成した「やったぁ.txt」モーションが実行されます。

```shell
$ rostopic pub /vsrc_control ros2vsrc/VsrcControl "b_R3: 1"
```

b_R3に0を書き込むと、ボタンを離した状態になり、アイドルモーションに戻ります。

```shell
$ rostopic pub /vsrc_control ros2vsrc/VsrcControl "b_R3: 0"
```

w_torqueに0を書き込むと脱力します。

```shell
$ rostopic pub /vsrc_control ros2vsrc/VsrcControl "w_torque: 0"
```
また、battery_stateを購読すると現在の電圧が取得できます。

```shell
$ rostopic echo /battery_state/voltage
7.39499998093
---
7.39499998093
---
7.39499998093
---
...
```

# ros2vsrc/vsrc_connectで何をやってるの？

VS-RC003は、内部はメモリマップで変数が管理されており、もともと外部のシリアルポートを用いて、メモリを書き換える機能が備わっています。
データのやりとりはアスキーコードで行われるため、そのまま人間が見てわかるようなプロトコルになっています。

たとえば、VS-RC003のメモリ情報をPCに読み込む場合はこんな感じです。

```:メモリマップ2009eaのデータを5バイトを読み出す
Send   ： r 2009ea 05[¥r][¥n]
Return ： r 2009ea 05[¥r][¥n]#2009ea 00 00 00 00 00
```

メモリマップでは1アドレスが2byteで、一度に16byte読み込めるため、最大8アドレス分の読み出しが可能です。
また、メモリに書き込む場合は、こんな感じです。

```:メモリマップ2009f6に1を書き込む(サーボモータON)
Send   ： w 2009f6 01 00[¥r][¥n]
Return ： w 2009f6 01 00[¥r][¥n]
```

これらを利用して、ros2vsrc/vsrc_connectでは必要なメモリマップだけを抽出し、ROSメッセージとメモリマップをゲートウェイする役割を担っています。

VS-RC003のシリアル通信については、こちらの[vstone公式資料](https://www.vstone.co.jp/products/vs_rc003hv/download/VSRC003_serial_manual.pdf)をご覧ください。
また、[アドレス一覧はこちら](https://www.vstone.co.jp/products/vs_rc003hv/download/VS-RC003_valuelist.pdf)です。

---

これで、仮想コントローラーから、KUMACOのモーションを再生するところまで、完成しました。
次はいよいよROSのリソースを用いて、KUMACOをROSの世界で制御したいと思います。

# 参考
- [ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)


# 一連の記事へのリンク

- [VS-RC003を搭載した"KUMACO"をROSで動かす(前編)「PCとKUMACOを繋ぐ話」](https://qiita.com/Dream_Drive/items/15b09fc0d6581f9f4192)
- [VS-RC003を搭載した"KUMACO"をROSで動かす(中編)「VS-RC003を制御するROSノード」](https://qiita.com/Dream_Drive/items/4a28119d1275f6e3dcad)
- [VS-RC003を搭載した"KUMACO"をROSで動かす(後編)「KUMACOを画像処理から制御する」](https://qiita.com/Dream_Drive/items/0216af27f0c9394f5513)