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

8236 ワード
Ubuntu Robot ROS Ubuntu テキストリンク

概要

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

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

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

環境

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

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

インストール

ROSのインストール

まずはROSをインストールしましょう。
こちら"ROS講座02 インストール"の記事がとても参考になります。

ros2vsrcパッケージインストール

つぎに、ROSからVS-RC003をシリアル経由で制御するROSパッケージros2vsrcをインストールします。
（頑張って作ってみました。)

$ 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を起動します。

$ roscore

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

$ rosrun ros2vsrc vsrc_connect _serialdev:=/dev/ttyUSB0

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

ros2vsrc/vsrc_connectの使い方

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

  • ros2vsrc/VsrcState (オリジナルメッセージ) (VS-RC003 -> ROS)
    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)から読み出したコントローラーの種類

  • ros2vsrc/VsrcControl (オリジナルメッセージ) (ROS -> VS-RC003)
    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) : 仮想コントローラー○ボタン

  • sensor_msgs/BatteryState
    VS-RC003から読み取ったバッテリーの電圧を配信するトピックです。

    • voltage (float32) : 電源電圧(V)

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

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

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

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

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

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

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

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

$ rostopic pub /vsrc_control ros2vsrc/VsrcControl "w_torque: 0"

また、battery_stateを購読すると現在の電圧が取得できます。

$ 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公式資料をご覧ください。
また、アドレス一覧はこちらです。

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

参考

一連の記事へのリンク

Conda紹介
first