Go to Qiita Advent Calendar 2024 Top
LoginSignup
32
25

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@isseikz

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

Last updated at Posted at 2019-06-30

はじめに

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);
}
32
25
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
32
25

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?