ESP32をROS2ノードにする(ros2arduino)

# はじめに
ESP-WROOM-32をROS2に接続したときのメモです。
~~ros2arduinoライブラリのGitHubリポジトリに上がっていたissueを確認したところ、
うまくいきました。~~


通信は以下の環境で行いました。

+ ESP32 (devkit-C)
+ Arduino IDE 1.8.9
+ Ubuntu 18.04
+ ROS2 Dashing

ESP32とUbuntuはWiFiでudpで接続し、ESP32からUbuntuにPublishするサンプルを実行しました。

# 1. Ubuntu側の準備
セットアップは以下の投稿を参考に行いました。本稿では省略します。
[Micro XRCE-DDSのAgentをRaspberry Piで構築してTopicを送受信する](https://qiita.com/lutecia16v/items/5760551dd3a7a0d3e7d3)


執筆時点では、ESP32を用いたtcp通信には未対応なので、今回はudpでXRCE-DDS agentを起動します。

```console
MicroXRCEAgent udp -p 2018 -v 6
```

+ `-p 2018`を付けることで、は2018番ポートで通信できるようになります。
ポート番号はサンプルコードの書き込み時に使います。

+ `-v [0から6までの数字]`を付けることで、詳細なログを端末に表示することができます。
6が最も詳細になるので通信確認の際には`-v 6`がおすすめです。

# 2. ros2arduinoライブラリのインクルード
**追記(2020/01/17)：Arduino IDEのlibrary managerからもインストールできます。**

ROBOTS-GITのgithubリポジトリ([こちら](https://github.com/ROBOTIS-GIT/ros2arduino))からダウンロードします。


```console
$ git clone https://github.com/ROBOTIS-GIT/ros2arduino.git
```

~~[issue](https://github.com/ROBOTIS-GIT/ros2arduino/issues/24)によると執筆時点の最新版にはバグがあるそうで、コードの変更が必要でした。`STREAM_HISTORY`を`1`から`2`に変更してください。~~
追記(2020/01/17)： 現在の最新版(0.14)では、`STREAM_HISTORY 4`で定義されていますが、そのまま問題なく動きます。ですから、コードの変更は必要ありません。

```diff:src/ros2/xrcedds/xrcedds.cpp
+#define STREAM_HISTORY 2
-#define STREAM_HISTORY 1
```

変更したら、zipファイルに圧縮してArduino IDEにインクルードします。
`(Arduino IDE) Sketch -> Include Library -> Add .ZIP Library...`
追記(2020/01/17)： Arduino IDEのlibrary managerからもインストールできます。検索欄に`ros2arduino`と入力してみてください。

# 3. ESP32に書き込み (Publisher)
今回はwifiを用いたpublisherのサンプルを用います。
`(Arduino IDE) File -> Examples -> Examples from custom libraries -> ros2arduino -> publisher_wifi_udp`

ファイル先頭の以下の部分を、ご自身の環境に合わせて設定してください。

```cpp:publisher_wifi_udp
#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "接続するWiFiルータのSSID".
#define SSID_PW    "接続するWiFiルータのパスワード"
#define AGENT_IP   "ホストマシン(Ubuntu)のIPアドレス"
#define AGENT_PORT 2018 //AGENT port number <- 1項で指定したポート番号です

#define PUBLISH_FREQUENCY 2 //hz <- 送信頻度を変えられます。
```

ESP32に書き込んだら準備完了です。

# 4. 接続
サンプルコードでは500Hz程度まで遅延・欠落なく送信できました。

# その他
別の端末で`ros2 topic list`を実行して、`arduino_chatter`が出ていれば、ROS2にESP32が繋がっています。

```console
$ ros2 topic list
/arduino_chatter
/parameter_events
/rosout
```

内容の表示には、`ros2 topic echo /arduino_chatter`です。

```console
$ ros2 topic echo /arduino_chatter
data: Hello ros2arduino 67612
---
data: Hello ros2arduino 67613
---
data: Hello ros2arduino 67614
---
data: Hello ros2arduino 67615
---

...以下省略
```


MicroXRCEAgentのログを載せておきます。

```console
$ MicroXRCEAgent udp --port 2018 -v 6 -r ros2arduino.refs 
Enter 'q' for exit
[1561897862.161394] info     | UDPServerLinux.cpp | init                     | running...             | port: 2018
[1561897862.162033] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1561897870.055827] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F AA BB CC DD 81 00 FC 07
[1561897870.056256] info     | Root.cpp           | create_client            | create                 | client_key: 0xAABBCCDD, session_id: 0x81
[1561897870.056364] info     | UDPServerBase.cpp  | on_create_client         | session established    | client_key: 0xAABBCCDD, address: 192.168.30.223:47138
[1561897870.056555] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1561897870.060575] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 112, data: 
0000: 81 80 00 00 01 05 68 00 00 0A 00 11 01 02 00 00 59 00 00 00 3C 64 64 73 3E 3C 70 61 72 74 69 63
0020: 69 70 61 6E 74 3E 3C 72 74 70 73 3E 3C 6E 61 6D 65 3E 72 6F 73 32 5F 78 72 63 65 64 64 73 5F 70
0040: 61 72 74 69 63 69 70 61 6E 74 3C 2F 6E 61 6D 65 3E 3C 2F 72 74 70 73 3E 3C 2F 70 61 72 74 69 63
0060: 69 70 61 6E 74 3E 3C 2F 64 64 73 3E 00 00 00 00

...以下省略
```

# 5. Subscriber
Subscriberも追加したコードを載せておきます。
別の端末を開いて、以下のコマンドを開けばArduino IDEのシリアルモニタで受信結果が見られます。

`ros2 topic pub arduino_listener std_msgs/String "data: hello"`

```cpp:
#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "ssid"
#define SSID_PW    "password"
#define AGENT_IP   "192.168.30.2"
#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 500 //hz

void publishString(std_msgs::String* msg, void* arg)
{
//  Serial.println("publishString");
  (void)(arg);

  static int cnt = 0;
  sprintf(msg->data, "Hello ros2arduino %d", cnt++);
//  Serial.println("Sending: ");
}

void subscribeString(std_msgs::String* msg, void* arg)
{
  (void)(arg);

  Serial.println("Received: " + String(msg->data));
}

class StringPub : public ros2::Node
{
public:
  StringPub()
  : Node()
  {
    ros2::Publisher<std_msgs::String>* publisher_ = this->createPublisher<std_msgs::String>("arduino_chatter");
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishString, nullptr, publisher_);
    this->createSubscriber<std_msgs::String>("arduino_listener", (ros2::CallbackFunc)subscribeString, nullptr);
  }
};

WiFiUDP udp;

void setup() 
{
  Serial.begin(115200);
  Serial.println("Setup Started!");
  WiFi.begin(SSID, SSID_PW);
  Serial.println("Wifi init!");
  while(WiFi.status() != WL_CONNECTED);
  Serial.println("Wifi connected!");

  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  Serial.println("Setup completed!");
}

void loop() 
{
  static StringPub StringNode;

  ros2::spin(&StringNode);
}
```