はじめに
ROS2は、マイコン等のリソースが少ない環境(eXtremely Resource Constrained Environments = XRCEs)をターゲットに含んでおり、それに向けた通信仕様であるDDS-XRCEがOMGにて標準化がすすめられています。
このXRCEに対応したボードとしてGadget Renesas(がじぇるね)から、GR-ROSEが今年の５月に発売されています。
ROS2対応以外もGR-ROSEは、Serial1～4が一線で送受信を行えるようにセレクタが入っており、近藤化学のICSサーボをそのまま接続でき、ICSサーボのライブラリも標準で搭載されています。また、4.5V～18Vの外部電源入力を使用でき、モーターの電源に使用しながら、マイコンへ給電される等、サーボモータの利用を前提に作られたマイコンボードとなっています。
WiFiへの対応
GR-ROSEは、WiFi用にESP-WROOM-02Dが搭載されていますが、これまでROS2のライブラリからは有線のみ利用可能でした。
しかしながら、先日公開された、開発環境のIDE for GR 1.11から、WiFiに対応になりました！
そこで、今回はWiFi対応記念に、Pololu ZumoにGR-ROSEを載せ、Joyとteleop_toolsを使って、Logicool Wireless Gamepad F710から操作してみようと思います。
DDS-XRCEはマイコン側のClientから接続するAgentが必要になるため、そのAgentとJoy/teleop_toolsをJetson Nano上で動作させます。
今回使用した機器のリストは以下になります。
| 種別 | 名称 |
|---|---|
| マイコン | GR-ROSE |
| ローバー | Zumo for Arduino v1.2 |
| シングルボードコンピュータ | Jetson Nano |
| WiFiドングル | Buffalo WLI-UC-GNM2 |
| ジョイスティック | Logocool F710 |
| その他 | Arduinoシールド基板、ブラ板、2mmネジ、HV変換DCジャック（近藤科学）など |
また、システム構成としては、
といった感じになります。
Jetson Nanoの設定
ROS2のインストール
まず、Jetson NanoにROS2の環境を導入します。Jetson Nanoは標準で64bit ARM版のUbuntu 18.04を採用しているため、そのまま公式サイトの手順でパッケージからインストールできます。現在(2019年12月)で、GR-ROSEとROS2でやりとりする場合の、ROS2の対応バージョンはDashingです。
Micro-XRCE-DDS-Agentのインストール
DDS-XRCEはマイコン側のClientと他のROS2ノードを仲介するAgentが必要となります。次にこのAgentを導入します。GR-ROSEでは、eProsimaが公開してる、Micro XRCE-DDSのAgentを使用します。
インストール方法は、公式に記載されています。
Micro-XRCE-DDSは頻繁に手がいれられているため、v1.1.6を明示的に取得して今回確認しています。(これまで何回かくらってます）
~/Micro-XRCE-DDS-Agent$ git checkout v1.1.6
必要なROS2パッケージのインストール
次に、ROS2の必要なROS2パッケージを導入して動作を確認しておきます。
$ sudo apt install ros-dashing-teleop-tools ros-dashing-joy
JoyStickの動作確認
インストール完了後、F710のレシーバのUSBドングルをJetson Nanoにさして
$ lsusb
・・・
Bus 001 Device 006: ID 046d:c21f Logitech, Inc. F710 Wireless Gamepad [XInput Mode]
・・・
と表示され、
$ ls /dev/input/
by-id by-path event0 event1 event2 js0 mice
と、js0ができていればJoyStickは認識されています。
もし、
$ lsusb
・・・
Bus 001 Device 003: ID 046d:c22f Logitech, Inc.
・・・
と表示されている場合は、JoyStickの動作モードがDirectInputになっていますので、XInputに変更してください。
ROS1のJoyのROS Wikiの情報では、F710のキーマップはDirectInputとなっているのですが、ROS2のドライバではXInputモードでないとこのあとの動作確認で値が変化しませんでした。
それでは、実際にjoyが動作するかを確認します。
$ ros2 run joy joy_node
[INFO] [joy_node]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
ここでもう１つ別のコンソールを立ち上げ、
$ ros2 topic echo /joy
header:
stamp:
sec: 1577113622
nanosec: 38783526
frame_id: joy
axes:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
buttons:
- 0
・・・
として、JoyStickを操作すると、該当する値が変化することが確認できます。
続けて、joy_nodeを動かしたまま、別のシェルを立ち上げて
$ ros2 run teleop_twist_joy teleop_node
[INFO] [TeleopTwistJoy]: Teleop enable button 5.
[INFO] [TeleopTwistJoy]: Linear axis x on 5 at scale 0.500000.
[INFO] [TeleopTwistJoy]: Angular axis yaw on 2 at scale 0.500000.
としてteleop_nodeを動かします。コンソールに出力されるログによると、イネーブルボタンがbuttonの５、直進がaxisの5、回転がaxisの2となってるようです。後述しますが、F710の場合、イネーブルボタンはRB、直進と回転がそれぞれRTとLTになります。つまり、RBボタンを押しながらRTとLTを操作することで、/cmd_velのメッセージが出力されます。
ここで、/cmd_velのトピックをみると
$ ros2 topic echo /cmd_vel
linear:
x: 0.5
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.5
...
とトピックが送られてくることを確認できます。
teleop_twist_joyの設定
先ほどの/Joyのトピックの出力からF710のキーのマップを確認したところ、以下のようになっていました。MODEボタンはOFFです。
ROS1の時の、Microsoft Xbox 360 Wired Controller for Linuxとほぼ同じものでした。
key.button
| Index | Button name |
|---|---|
| 0 | A |
| 1 | B |
| 2 | X |
| 3 | Y |
| 4 | LB |
| 5 | RB |
| 6 | back |
| 7 | start |
| 8 | Logicool |
| 9 | Button stick left |
| 10 | Button stick right |
key.axes
| Index | Axis name |
|---|---|
| 0 | Left/Right Axis stick left |
| 1 | Up/Down Axis stick left |
| 2 | LT |
| 3 | Left/Right Axis stick right |
| 4 | Up/Down Axis stick right |
| 5 | RT |
| 6 | cross key left/right |
| 7 | cross key up/down |
lauchファイルの作成
このままでも動きますが、操作はStickを使いたいのと、joyとteleopの起動でシェルを複数立ち上げるのは面倒なので、lauchファイルを使って起動します。
lauchファイルですが、パッケージでインストールしたところ、/opt/ros/dashing/share/teleop_twist_joy配下にはなかったので、githubにあるものを持ってきて修正しました。
/opt/ros/dashing/share/teleop_twist_joy配下にlaunchとconfigディレクトリを作成します。
/opt/ros/dashing/share/teleop_twist_joy$ sudo mkdir launch
/opt/ros/dashing/share/teleop_twist_joy$ sudo mkdir config
launchディレクトリ内には、teleop-launch.pyを配置して、joy_configのデフォルト値をps3からf710に変更します。
・・・
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument('joy_config', default_value='f710'),
・・・
次に、configディレクトリ内に、xbox.config.yamlを配置して、ファイル名をf710.config.yamlに変更します。ファイルの中身は今回は変更せずそのまま使います。
その後、lauchファイルを実行します。
ros2 launch teleop_twist_joy teleop-launch.py
イネーブルボタンはXボタン、前進と回転は左のスティックとなります。/cmd_velにトピックが正しくでていれば完了です。
GR-ROSEでTwistメッセージを受け取る
GR-ROSEの開発環境設定
GR-ROSE側のソフトウェアの開発は、IDE for GRというArduino IDEライクな開発環境を使います。使い勝手はほぼArduino IDEです。
バージョンは現状(2019/12）で最新のV1.11を使用します。
※ IDE for GRはWindows版とMac版がありますが、以下の記述はMac版を想定しています。ただし、Windows版もほぼ同じと思います。
IDE for GRを起動し、メニューの「マイコンボード」でGR-ROSEを選択します。
その後、メニューの「スケッチの例」をみると、ROS2のサンプルコードが選べるようになります。
このサンプルコードをベースにして、必要な処理を追加していくことになります。
今回は、udp_listener_besteffortをベースにしました。
GR-ROSEのプログラム作成 - WiFi設定
まずは、WiFiの設定を記載します。ssid[]とpass[]を書き換えてください。IPはDHCPで取得するようになっています。ソース上にあるIPアドレスの設定は、有線を使用する際に使われますので、今回はなにも変更しなくてもよいです。
char ssid[] = "SSID"; // your network SSID (name)
char pass[] = "PASSWORD"; // your network password
int status = WL_IDLE_STATUS; // the Wifi radio's status
byte mac[] = { 0x74, 0x90, 0x50, 0x00, 0x79, 0x03 };
IPAddress ip(192, 168, 2, 52);
まずはこの状態でコンパイルしてGR-ROSEに書き込んで、Agentと繋がるかを確認してみます。
GR-ROSEへのプログラムの書き込みはmbedライクで、電源投入後にRSTボタンを押すとUSBドライブとして認識される（基板のLEDが光るので認識できます）ので、そこにプログラムを書き込む形になります。書き込みはIDE for GRがしてくれるので、"→"ボタンを押す前に、RSTボタンを押しておく必要があります。
プログラムの書き込み後に、シリアルコンソールを立ち上げておきます。（プログラム書き込み後に自動的にリブートしてUSBシリアルとして認識されます）
すると、
Attempting to connect to WPA SSID: <指定したSSID>
Discovery Agent...
とWiFiへの接続を開始して、WiFi接続が成功するとAgentを探しはじめます。Agentの検索はマルチキャストで行われますので、AgentのIPの指定は不要です。
ここで、Jetson Nanoのコンソールにログインして、Agentを起動します。
$ MicroXRCEAgent udp -p 2020 -d
パラメータですが、１つめはプロトコル(UDP/TCP)を指定し、-pでポート番号を指定します。最後に-dをつけてディスカバリ機能（マルチキャストでAgentを発見する機能）を有効にしておきます。
すると、GR-ROSEのシリアルコンソールに
Attempting to connect to WPA SSID: <指定したSSID>
Discovery Agent...
Found agent => ip: xxx.xx.xx.x, port: 2020
Chosen agent => ip: xxx.xx.xx.x, port: 2020
という感じで、Agentを発見して(xxx.xx.xx.xがAgentのIP)、選択されたことがわかります。
GR-ROSEのプログラム作成 - Twistメッセージの受信
サンプルは、String型のchatterトピックを受け付けるようになっていますので、今回はそこをTwist型のcmd_velトピックに変更します。
標準的なメッセージの型は用意されていますので、それを使用します。
ROS2用のメッセージヘッダ一式は以下にあります。（Appplications配下にIDE for GRをインストールした場合）
/Applications/IDE4GR.app/Contents/Java/hardware/arduino/rx65n/cores/ros2_msg
#include "ros2_msg/Ros2String.h"
を
#include "ros2_msg/Twist.h"
とします。
次に、トピックを受信した際のメッセージをデシリアイズする箇所を、Twist型で置き換えます。
具体的には、211行目のon_topic()の部分になります。
void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* mb, void* args) {
(void) session; (void) object_id; (void) request_id; (void) stream_id;
Twist topic;
Twist_deserialize_topic(mb, &topic);
Serial.println("Received topic: ");
Serial.println(topic.linear.x);
Serial.println(topic.linear.y);
Serial.println(topic.linear.z);
Serial.println(topic.angular.x);
Serial.println(topic.angular.y);
Serial.println(topic.angular.z);
とすると、受信したトピックの値がシリアルコンソールに出力されるかと思います。
あと少し、変更するところがあります。トピックをSubscribeする箇所です。全部で２箇所あり、１つめは159行目。
const char* topic_xml = "<dds>"
"<topic>"
"<name>rt/chatter</name>"
"<dataType>std_msgs::msg::dds_::String_</dataType>"
"</topic>"
"</dds>";
ここを
const char* topic_xml = "<dds>"
"<topic>"
"<name>rt/cmd_vel</name>"
"<dataType>geometry_msgs::msg::dds_::Twist_<</dataType>"
"</topic>"
"</dds>";
とします。
同様に、180行目からのdatareader_xmlの内容も書き換えます。
const char* datareader_xml = "<dds>"
...
以上で、ソースの修正は完了です。
GR-ROSEのプログラム作成 - Zumoの制御
Zumoのモーター制御は、PololuのZumo用Githubに公開のZumoMotorsがそのまま利用可能です。（ピン番号だけ変更が必要）
受け取った、Twistメッセージの内容にしたがって、各モータを制御します。
GR-ROSEをZumoに搭載する。
最後に、GR-ROSEをZumoに搭載します。
電源は、ZumoのVinから取得することができるので、それをGR-ROSEの外部電源入力(VHコネクタ)に接続すればよいです。
これで、ROS2を使って、GR-ROSEでZumoを動かすことができます。
Zumoは加速度センサーを搭載しており、また、別売りですがモータのエンコーダも搭載可能ですので、その辺りの機能の追加も可能かと思います。
GR-ROSE側の詳しいプログラムについては、現在、技術書典８で配布予定の同人誌にて書く予定でおりますので、ご興味がある方は、技術書典８の１日目(2/29土曜)にお越しいただければと思います。
（余談ですが、ZumoにGR-ROSEのせる基板も製作予定です）
以上となります。