ROS2を使って、GR-ROSEでZumoを動かす

# はじめに

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

- [DDS For Extremely Resource Constrained Environments](https://www.omg.org/spec/DDS-XRCE/)

このXRCEに対応したボードとして[Gadget Renesas(がじぇるね)](https://www.renesas.com/jp/ja/products/gadget-renesas.html)から、[GR-ROSE](https://www.renesas.com/jp/ja/products/gadget-renesas/boards/gr-rose.html)が今年の５月に発売されています。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/51f6eacf-1260-b920-44d6-4e8034c6af89.png)

ROS2対応以外もGR-ROSEは、Serial1～4が一線で送受信を行えるようにセレクタが入っており、近藤化学のICSサーボをそのまま接続でき、ICSサーボのライブラリも標準で搭載されています。また、4.5V～18Vの外部電源入力を使用でき、モーターの電源に使用しながら、マイコンへ給電される等、サーボモータの利用を前提に作られたマイコンボードとなっています。

# WiFiへの対応

GR-ROSEは、WiFi用にESP-WROOM-02Dが搭載されていますが、これまでROS2のライブラリからは有線のみ利用可能でした。

しかしながら、先日公開された、開発環境の[IDE for GR](https://www.renesas.com/jp/ja/products/gadget-renesas/ide-gr.html) 1.11から、WiFiに対応になりました！

そこで、今回はWiFi対応記念に、[Pololu Zumo](https://www.pololu.com/category/169/zumo-robot-for-arduino)にGR-ROSEを載せ、[Joy](https://index.ros.org/p/joy/)と[teleop_tools](https://index.ros.org/p/teleop_tools/github-ros-teleop-teleop_tools/)を使って、[Logicool Wireless Gamepad F710](https://gaming.logicool.co.jp/ja-jp/products/gamepads/f710-wireless-gamepad.html)から操作してみようと思います。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/492500e8-901c-8dad-9354-d20381956d4d.png)

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ジャック（近藤科学）など|

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/96d47cff-aad8-29f4-3f45-9a1f82c36298.png)

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/7111e127-df50-1183-d011-babe9b5a3539.png)

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

# 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](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/)

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

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

- [https://github.com/eProsima/Micro-XRCE-DDS-Agent](https://github.com/eProsima/Micro-XRCE-DDS-Agent)

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

- [Installation manual](https://micro-xrce-dds.readthedocs.io/en/latest/installation.html)

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](http://wiki.ros.org/joy)の情報では、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にあるものを持ってきて修正しました。

- 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`に変更します。

```teleop-launch.py
・・・
    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](https://www.renesas.com/jp/ja/products/gadget-renesas/ide-gr.html)

バージョンは現状(2019/12）で最新のV1.11を使用します。

※ IDE for GRはWindows版とMac版がありますが、以下の記述はMac版を想定しています。ただし、Windows版もほぼ同じと思います。

IDE for GRを起動し、メニューの「マイコンボード」で`GR-ROSE`を選択します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/5585468c-1c68-7a13-7b6c-a5900a22abeb.png)

その後、メニューの「スケッチの例」をみると、ROS2のサンプルコードが選べるようになります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/60cde8aa-daf1-1bb4-6f15-404302652cc5.png)

このサンプルコードをベースにして、必要な処理を追加していくことになります。

今回は、udp_listener_besteffortをベースにしました。

## GR-ROSEのプログラム作成 - WiFi設定

まずは、WiFiの設定を記載します。`ssid[]`と`pass[]`を書き換えてください。IPはDHCPで取得するようになっています。ソース上にあるIPアドレスの設定は、有線を使用する際に使われますので、今回はなにも変更しなくてもよいです。

```udp_listener_besteffort.ino
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に搭載します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51272/748f04cb-80d4-0777-6251-0f008ee238c9.png)

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

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

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

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

以上となります。