M5Stackからラズパイへメッセージを届けてみる（ROS2/DDS）

こういうWi-Fiプライベート・ネットワークで、M5Stackからラズパイにメッセージを届けてみます。

前回のJetson Nano を使った場合はROS2のDDSでシンプルにPub/Sub通信ができていました。

こんな感じで、メッセージは “トピック” 上で送受信されます。

が、M5Stack のようなリソースの小さい環境の場合はMicro-XRCE-DDS というテクノロジーを挟んで使います。

こういうイメージだそうです。Low Resources Device というのがM5Stack (マイコン)になります。

M5stackのMicro-XRCE-DDS-Client としては、ros2arduinoを使ってみます。

Micro-XRCE-DDS-Agent はラズパイにインストールします。

段取りはこうなります。

１：ラズパイ４にドッカーコンテナを導入してROS2をインストール

２：ラズパイ４のコンテナにMicro-XRCE-DDS-Agent をインストール

３：ラズパイ４でSuscriber を作成

４：M5Stackにros2arduinoをインストール

５：M5StackでPublisherを作成

６：通信実行

１：ラズパイ４にドッカーコンテナを導入してROS2をインストール

以下のページの前半をご参照ください。

まだコンテナは作成しません。

OSはUbuntu Server 20.04 LTS を使うので、Windows などからSSH接続します。

ラズパイ４でROS2を動かしてみる（DDS）

２：ラズパイ４のコンテナにMicro-XRCE-DDS-Agent をインストール

１のROS2ドッカーイメージを使ってコンテナを作成し、これにMicro-XRCE-DDS-Agent インストールします。

ラズパイのIPアドレスを調べておきます（例：192.168.0.34だとします）。

通信は２０１８番ポートを使ってUDPで行います。

コンテナ名はmy_ros2-A

イメージはwisteriahill/ros:foxy-ros-base-l4t-r32.4.4

sudo docker create -it --name my_ros2-A --network host -p 192.168.0.34:2018:2018 -p 192.168.0.34:2018:2018/udp wisteriahill/ros:foxy-ros-base-l4t-r32.4.4

WARNINGが出ますが、気にしない

コンテナを起動

sudo docker start -i my_ros2-A

1	sudo docker start -i my_ros2-A

アップデート＆アップグレード、パッケージのインストール

apt update
apt upgrade -y
apt install nano
apt install git

apt update

apt upgrade -y

apt install nano

apt install git

Micro-XRCE-DDS-Agentをインストール

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
cmake ..
make
make install
ldconfig /usr/local/lib/

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

cmake ..

make

make install

ldconfig /usr/local/lib/

３：ラズパイ４でSuscriber を作成

【subscriber.py】
ノード名：listener
トピック名：chatter

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

rclpy.init()

node = Node("listener")

def chatter_callback(msg):
    print("{0}".format(msg))

node.create_subscription(String, "chatter", chatter_callback,10)

try:
    rclpy.spin(node)
except KeyboardInterrupt:
    pass

node.destroy_node()
rclpy.shutdown()

import rclpy

from rclpy.node import Node

from std_msgs.msg import String

rclpy.init()

node = Node("listener")

def chatter_callback(msg):

print("{0}".format(msg))

node.create_subscription(String, "chatter", chatter_callback,10)

try:

rclpy.spin(node)

except KeyboardInterrupt:

pass

node.destroy_node()

rclpy.shutdown()

４：M5Stackにros2arduinoをインストール

まず、開発環境としてArduino IDE を使います。例えば以下のページの後半をご参照ください。

Jetson Nano にM5Stack 開発環境をセットアップ（メモ）

Arduino IDEで、ros2arduinoをインストールします。

スケッチー＞ライブラリをインクルードー＞ライブラリを管理を選択

ros2arduinoを検索してインストールします。

このバージョンのメジャー.マイナー番号は２でインストールしたMicro-XRCE-DDS-Agentのバージョン（1.3.0）と関連しています。このページ参照

バージョンは最新（0.2.1）をインストールします。

５：M5StackでPublisherを作成

スケッチ例のros2arduinoのPublisher_wifi_udpというコードを参考にします。

このコードをコピーしておきます。

SSID、SSID_PW、AGENT_IPはご自分の環境に合わせて書き換えます。

今回の例ではAGENT_IPは192.168.0.34

ノード名はros2arduino_pub_node

ファイルー＞新規ファイル

XXXX、YYYY、ZZZZを書き換えてコピー・ペースト

M5***で記述されている所が追加箇所

下記のコードではトピック名は「arduino_chatter」から「chatter」に変更しておきました。

#include <M5Stack.h>
#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "XXXX"
#define SSID_PW    "YYYY"
#define AGENT_IP   "ZZZZ"
#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 2 //hz

void publishString(std_msgs::String* msg, void* arg)
{
  (void)(arg);
  static int cnt = 0;
  sprintf(msg->data, "Hello ros2arduino %d", cnt++);
  String strBuf = msg->data;
  strBuf.concat("\n");
  M5.Lcd.println(strBuf);
}

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

WiFiUDP udp;

void setup() {
  M5.begin();
  M5.Lcd.print("START\n");
  WiFi.begin(SSID, SSID_PW);
  while(WiFi.status() != WL_CONNECTED);

  ros2::init(&udp, AGENT_IP, AGENT_PORT);
}

void loop() 
{
  static StringPub StringNode;
  ros2::spin(&StringNode);
}

#include <M5Stack.h>

#include <ros2arduino.h>

#include <WiFi.h>

#include <WiFiUdp.h>

#define SSID "XXXX"

#define SSID_PW "YYYY"

#define AGENT_IP "ZZZZ"

#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 2 //hz

void publishString(std_msgs::String* msg, void* arg)

{

(void)(arg);

static int cnt = 0;

sprintf(msg->data, "Hello ros2arduino %d", cnt++);

String strBuf = msg->data;

strBuf.concat("\n");

M5.Lcd.println(strBuf);

}

class StringPub : public ros2::Node

{

public:

StringPub()

: Node("ros2arduino_pub_node")

{

ros2::Publisher<std_msgs::String>* publisher_ = this->createPublisher<std_msgs::String>("chatter");

this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishString, nullptr, publisher_);

}

};

WiFiUDP udp;

void setup() {

M5.begin();

M5.Lcd.print("START\n");

WiFi.begin(SSID, SSID_PW);

while(WiFi.status() != WL_CONNECTED);

ros2::init(&udp, AGENT_IP, AGENT_PORT);

}

void loop()

{

static StringPub StringNode;

ros2::spin(&StringNode);

}

６：通信実行

ラズパイ

SSHを閉じていた場合は再度接続してコンテナーを起動します。

sudo docker start -i my_ros2-A

1	sudo docker start -i my_ros2-A

まず、ラズパイ側でAgentを起動しておきます。M5StackのPublisherはこのAgentを探して（discover）接続を確立します。

cd /Micro-XRCE-DDS-Agent/build

Agentを起動（1.3.0では以下のようなオプションになります）

./MicroXRCEAgent udp4 -p 2018

cd /Micro-XRCE-DDS-Agent/build

Agentを起動（1.3.0では以下のようなオプションになります）

./MicroXRCEAgent udp4 -p 2018

SSHをもう一つ開いて、コンテナーに入ります。

sudo docker exec -it my_ros2-A bash

1	sudo docker exec -it my_ros2-A bash

Subscriberを起動

python3 subscriber.py

1	python3 subscriber.py

Arduino IDE でM5StackにPublisherコードをコンパイルして書き込みます。

書き込み終了後にAgentを見つけて接続。

M5Stackからメッセージが送信されます。

ラズパイのSubscriberにメッセージが届いています。

Micro-XRCE-DDSを使ってpublishされたメッセージは他のノードでもsubscribeできます。

例えばネットワーク内のJetsonにこのページで作成したROS2のコンテナを作って、３のsubscriberを起動しておきます。

sudo docker start -i my_ros2

python3 subscriber.py

sudo docker start -i my_ros2

python3 subscriber.py

メッセージが届きます。

DOMAIN_IDが０の場合、受信できます。

メッセージをスルーしたい場合はIDを変えておきます。

export ROS_DOMAIN_ID=2

1	export ROS_DOMAIN_ID=2

ラズパイのみにメッセージを届けたい場合、以下のようにコンテナを作っても可です。

sudo docker create -it --name my_ros2-A -p 192.168.0.34:2018:2018/udp wisteriahill/ros:foxy-ros-base-l4t-r32.4.4

Appendix

ROS2をWebで使う場合、こういうのもあるようです。

ros2-web-bridge

Appendix2

せっかくのM5Stack Gray なのでIMU（慣性計測ユニット）の中のPitch、Roll、Yawの値を取得して、メッセージで送信してみます（要書き換え：SSID、SSID_PW、AGENT_IP）。

#define M5STACK_MPU6886 

#include <M5Stack.h>
#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "XXXX"
#define SSID_PW    "YYYY"
#define AGENT_IP   "192.168.0.34"
#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 2 //hz


//IMU-----------------------
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;

float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;

float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

float temp = 0.0F;
//-------------------------

void publishString(std_msgs::String* msg, void* arg){
  (void)(arg);
  //M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  //M5.IMU.getAccelData(&accX,&accY,&accZ);
  //M5.IMU.getTempData(&temp);
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  M5.Lcd.setCursor(0, 110);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
  sprintf(msg->data, "Pitch: %5.2f /Roll: %5.2f /Yaw: %5.2f",pitch, roll, yaw);
  
}

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

WiFiUDP udp;


void setup() {
  M5.begin();

  M5.Power.begin();
    
  M5.IMU.Init();

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);

  M5.Lcd.print("START\n");
  WiFi.begin(SSID, SSID_PW);
  while(WiFi.status() != WL_CONNECTED);

  ros2::init(&udp, AGENT_IP, AGENT_PORT);
}



void loop() 
{
  static StringPub StringNode;
  ros2::spin(&StringNode);
}

#define M5STACK_MPU6886

#include <M5Stack.h>

#include <ros2arduino.h>

#include <WiFi.h>

#include <WiFiUdp.h>

#define SSID "XXXX"

#define SSID_PW "YYYY"

#define AGENT_IP "192.168.0.34"

#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 2 //hz

//IMU-----------------------

float accX = 0.0F;

float accY = 0.0F;

float accZ = 0.0F;

float gyroX = 0.0F;

float gyroY = 0.0F;

float gyroZ = 0.0F;

float pitch = 0.0F;

float roll = 0.0F;

float yaw = 0.0F;

float temp = 0.0F;

//-------------------------

void publishString(std_msgs::String* msg, void* arg){

(void)(arg);

//M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);

//M5.IMU.getAccelData(&accX,&accY,&accZ);

//M5.IMU.getTempData(&temp);

M5.IMU.getAhrsData(&pitch,&roll,&yaw);

M5.Lcd.setCursor(0, 110);

M5.Lcd.printf(" %5.2f %5.2f %5.2f ", pitch, roll, yaw);

sprintf(msg->data, "Pitch: %5.2f /Roll: %5.2f /Yaw: %5.2f",pitch, roll, yaw);

}

class StringPub : public ros2::Node{

public:

StringPub()

: Node("ros2arduino_pub_node")

{

ros2::Publisher<std_msgs::String>* publisher_ = this->createPublisher<std_msgs::String>("chatter");

this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishString, nullptr, publisher_);

}

};

WiFiUDP udp;

void setup() {

M5.begin();

M5.Power.begin();

M5.IMU.Init();

M5.Lcd.fillScreen(BLACK);

M5.Lcd.setTextColor(GREEN , BLACK);

M5.Lcd.setTextSize(2);

M5.Lcd.print("START\n");

WiFi.begin(SSID, SSID_PW);

while(WiFi.status() != WL_CONNECTED);

ros2::init(&udp, AGENT_IP, AGENT_PORT);

}

void loop()

{

static StringPub StringNode;

ros2::spin(&StringNode);

}

こんな感じ。

FRONT

地図と画像のサイト

M5Stackからラズパイへメッセージを届けてみる（ROS2/DDS）

Be the first to comment

Leave a Reply コメントをキャンセル