Qiita Teams that are logged in
You are not logged in to any team

Log in to Qiita Team
Community
OrganizationEventAdvent CalendarQiitadon (β)
Service
Qiita JobsQiita ZineQiita Blog
19
Help us understand the problem. What are the problem?

More than 1 year has passed since last update.

posted at

updated at

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

執筆時点では、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_HISTORY1から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);
}
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
19
Help us understand the problem. What are the problem?