ESP32CAMをMicro XRCE-DDSでROS2ノード化する
https://micro-xrce-dds.docs.eprosima.com/en/latest/
Micro XRCE-DDSはマイコンでも動作する軽量なクライアントライブラリで利用できるDDSとのインターフェースです。マイコンはMicro XRCE-DDS Agentと通信し，Micro XRCE-DDS AgentはマイコンとのやりとりをROS2が利用するDDSへ転送します。
この記事ではDocker for WindowsでMicro XRCE-DDS Agentを実行し，ESP32側ではros2arduinoをMicro XRCE-DDS Client として利用します。
環境
ホスト
マイコン
- ESP32-CAM
- ros2arduino 0.2.1
ファイアウォールの設定
以下の説明ではMicro XRCE-DDS AgentはUDPの2018ポートを使用します。Windows ファイアウォールの設定を変更し，docker backendへのUDPでのアクセスを許可しておきます。
Micro XRCE-DDS Agentの準備
Micro XRCE-DDS Agentはdockerで動作させます。以下のDockerfileを作成します。
# docker build -t ros2:dashing . FROM ubuntu:18.04 ENV DEBIAN_FRONTEND noninteractive ENV ROS_DISTRO dashing # ロケールのセットアップ RUN apt-get update && apt-get install -y locales && \ dpkg-reconfigure locales && \ locale-gen ja_JP ja_JP.UTF-8 && \ update-locale LC_ALL=ja_JP.UTF-8 LANG=ja_JP.UTF-8 ENV LC_ALL ja_JP.UTF-8 ENV LANG ja_JP.UTF-8 ENV LANGUAGE ja_JP.UTF-8 # APTソースリストの設定 RUN apt-get update && \ apt-get install -y curl gnupg2 lsb-release && \ curl http://repo.ros2.org/repos.key | apt-key add - && \ sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu \ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' && \ apt-get update # ROS2パッケージのインストール RUN export ROS_DISTRO=dashing && \ apt-get install -y ros-$ROS_DISTRO-desktop \ python3-colcon-common-extensions python3-rosdep python3-argcomplete && \ rosdep init && \ rosdep update ## 環境設定 RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc ## Micro XRCE-DDS Agentのビルド ## https://github.com/ROBOTIS-GIT/ros2arduino RUN apt-get install -y git WORKDIR work SHELL ["/bin/bash", "-c"] RUN git clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.git && \ cd Micro-XRCE-DDS-Agent && \ git checkout -b v1.3.0 refs/tags/v1.3.0 && \ mkdir build && cd build && \ source /opt/ros/dashing/setup.bash && \ cmake .. && \ make && \ make install && \ ldconfig /usr/local/lib/
Dockerfileを作成したら以下のコマンドでビルドします。
> docker build -t micro-xrce-dds-agent:v1.3.0 .
ビルドしたら以下コマンドで起動します。
> docker run --rm -it -p 192.168.0.10:2018:2018 -p 192.168.0.10:2018:2018/udp micro-xrce-dds-agent:v1.3.0 # cd MicroXRCEAgent/build # ./MicroXRCEAgent udp4 -p 2018
起動すると以下のようなメッセージが表示されます。
root@docker-desktop:/work/Micro-XRCE-DDS-Agent/build# ./MicroXRCEAgent udp4 -p 2018 Press CTRL+C to exit [1612241367.034260] info | UDPv4AgentLinux.cpp | init | running... | port: 2018
ESP32CAM側プログラム
あらかじめArduino IDEでros2arduinoライブラリをインストールしておきます。
Arduino IDEで以下のプログラムを作成し，ESP32へ転送します。#define AGENT_IPにはMicro XRCE-DDS Agentを動作させているホストのIPアドレスを指定します。
#include <ros2arduino.h> #include <WiFi.h> #include <WiFiUdp.h> #define SSID "ssid" #define SSID_PW "password" #define AGENT_IP "ipaddress ex.192.168.0.10" #define AGENT_PORT 2018 //AGENT port number #define PUBLISH_FREQUENCY 1 //hz #define LED_BUILTIN 4 void publishString(std_msgs::String* msg, void* arg) { (void)(arg); static int cnt = 0; sprintf(msg->data, "Hello ros2arduino %d", cnt++); } void subscribeLed(std_msgs::Bool* msg, void* arg) { (void)(arg); digitalWrite(LED_BUILTIN, msg->data); } class StringPub : public ros2::Node { public: StringPub() : Node("ros2arduino_pub_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::Bool>("arduino_led", (ros2::CallbackFunc)subscribeLed, nullptr); } }; WiFiUDP udp; void setup() { Serial.begin(115200); Serial.setDebugOutput(true); Serial.println(); pinMode(LED_BUILTIN, OUTPUT); WiFi.begin(SSID, SSID_PW); while(WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); } Serial.println(""); Serial.print("WiFi Connected. IP address : "); Serial.println(WiFi.localIP()); ros2::init(&udp, AGENT_IP, AGENT_PORT); Serial.println("Setup completed."); } void loop() { static StringPub StringNode; ros2::spin(&StringNode); }
ESP32CAMとMicro XRCE-DDS Agentの接続
上記のプログラムを書き込んだESP32CAMを起動すると，Micro XRCE-DDS AgentにESP32CAMが接続され以下のようなメッセージが表示されます。
root@83535a5f72b3:/work/Micro-XRCE-DDS-Agent/build# ./MicroXRCEAgent udp4 -p 2018 Press CTRL+C to exit [1612242146.336194] info | UDPv4AgentLinux.cpp | init | running... | port: 2018 [1612242160.276226] info | Root.cpp | create_client | create | client_key: 0xAABBCCDD, session_id: 0x81 [1612242160.279092] info | SessionManager.hpp | establish_session | session established | client_key: 0x2864434397, address: 172.17.0.1:8331 [1612242160.281801] info | SessionManager.hpp | establish_session | session re-established | client_key: 0x2864434397, address: 172.17.0.1:8331 [1612242160.281854] info | SessionManager.hpp | establish_session | session re-established | client_key: 0x2864434397, address: 172.17.0.1:8331 [1612242160.281866] info | SessionManager.hpp | establish_session | session re-established | client_key: 0x2864434397, address: 172.17.0.1:8331 [1612242160.281873] info | SessionManager.hpp | establish_session | session re-established | client_key: 0x2864434397, address: 172.17.0.1:8331 [1612242160.281880] info | SessionManager.hpp | establish_session | session re-established | client_key: 0x2864434397, address: 172.17.0.1:8331 [1612242160.281887] info | SessionManager.hpp | establish_session | session re-established | client_key: 0x2864434397, address: 172.17.0.1:8331
ROS2との接続
Micro XRCE-DDS Agentを起動するために作成したDockerイメージにはROS2 Dashingの環境も含まれています。以下のようにしてros2 topic listを実行し，arduino_chatterとarduino_ledが表示されることを確認します。
> docker run --rm -it --net host micro-xrce-dds-agent:v1.3.0 # ros2 topic list /arduino_chatter /arduino_led /parameter_events /rosout
ros2 topic echo arduino_chatterを実行すると以下のようにESP32CAMからPublishされたメッセージが表示されます。
root@docker-desktop:/work# ros2 topic echo arduino_chatter data: Hello ros2arduino 257 --- data: Hello ros2arduino 258 --- data: Hello ros2arduino 259 --- data: Hello ros2arduino 260 --- data: Hello ros2arduino 261 --- data: Hello ros2arduino 262 --- data: Hello ros2arduino 263 ---
また，ros2 topic pub arduino_led std_msgs/msg/Bool '{data: true}'を実行するとESP32CAM上に実装されているLEDが点灯します。ros2 topic pub arduino_led std_msgs/msg/Bool '{data: false}'を実行すると消灯します。
root@docker-desktop:/work# ros2 topic pub arduino_led std_msgs/msg/Bool '{data: true}'
publisher: beginning loop
publishing #1: std_msgs.msg.Bool(data=True)
publishing #2: std_msgs.msg.Bool(data=True)
