はじめに

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を送受信する

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

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リポジトリ(こちら)からダウンロードします。

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

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

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

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

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が繋がっています。

$ ros2 topic list
/arduino_chatter
/parameter_events
/rosout

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

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

...以下省略

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

$ 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"

#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);
}

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