はじめに

ROS2は、マイコン等のリソースが少ない環境(eXtremely Resource Constrained Environments = XRCEs)をターゲットに含んでおり、それに向けた通信仕様であるDDS-XRCEがOMGにて標準化がすすめられています。

• DDS For Extremely Resource Constrained Environments

この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上で動作させます。

今回使用した機器のリストは以下になります。

また、システム構成としては、

といった感じになります。

Jetson Nanoの設定

ROS2のインストール

まず、Jetson NanoにROS2の環境を導入します。Jetson Nanoは標準で64bit ARM版のUbuntu 18.04を採用しているため、そのまま公式サイトの手順でパッケージからインストールできます。現在(2019年12月)で、GR-ROSEとROS2でやりとりする場合の、ROS2の対応バージョンはDashingです。

• Installing ROS 2 via Debian Packages

Micro-XRCE-DDS-Agentのインストール

DDS-XRCEはマイコン側のClientと他のROS2ノードを仲介するAgentが必要となります。次にこのAgentを導入します。GR-ROSEでは、eProsimaが公開してる、Micro XRCE-DDSのAgentを使用します。

• https://github.com/eProsima/Micro-XRCE-DDS-Agent

インストール方法は、公式に記載されています。

• Installation manual

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

key.axes

lauchファイルの作成

このままでも動きますが、操作はStickを使いたいのと、joyとteleopの起動でシェルを複数立ち上げるのは面倒なので、lauchファイルを使って起動します。

lauchファイルですが、パッケージでインストールしたところ、/opt/ros/dashing/share/teleop_twist_joy配下にはなかったので、githubにあるものを持ってきて修正しました。

• https://github.com/ros2/teleop_twist_joy

/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です。

• IDE for Gadget Renesas

バージョンは現状(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がそのまま利用可能です。（ピン番号だけ変更が必要）

• https://github.com/pololu/zumo-shield

受け取った、Twistメッセージの内容にしたがって、各モータを制御します。

GR-ROSEをZumoに搭載する。

最後に、GR-ROSEをZumoに搭載します。

電源は、ZumoのVinから取得することができるので、それをGR-ROSEの外部電源入力(VHコネクタ)に接続すればよいです。

これで、ROS2を使って、GR-ROSEでZumoを動かすことができます。

Zumoは加速度センサーを搭載しており、また、別売りですがモータのエンコーダも搭載可能ですので、その辺りの機能の追加も可能かと思います。

GR-ROSE側の詳しいプログラムについては、現在、技術書典８で配布予定の同人誌にて書く予定でおりますので、ご興味がある方は、技術書典８の１日目(2/29土曜)にお越しいただければと思います。
（余談ですが、ZumoにGR-ROSEのせる基板も製作予定です）

以上となります。