ラズパイからM5Stackへメッセージを届けてBEEP音でお知らせ（ROS2/DDS）

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

M5Stackからラズパイへメッセージを届けてみる（ROS2/DDS）でやったことの逆バージョンです。

せっかくLCDとスピーカー付きのマイコン（M5Stack）なので両方使ってみます。

メッセージは “トピック” 上で送受信されます。

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

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

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

Micro-XRCE-DDS-Agent はJetsonにインストールしてみます。

段取りはこうなります。

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

２：Jetson Nanoにドッカーコンテナを導入してROS2とMicro-XRCE-DDS-Agent をインストール

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

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

５：M5StackでSubscriberを作成

６：通信実行して、M5Stackにメッセージが届いたらLCDに表示してBEEP音を鳴らす

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

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

とりあえずコンテナの作成までをやっておきます。

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

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

２：Jetson Nanoにドッカーコンテナを導入してROS2とMicro-XRCE-DDS-Agent をインストール

１で使ったドッカーイメージをまんまここでも使います。

ただし、コンテナの作成は以下のようなオプションでやります。

Jetsom Nano のIPアドレスが192.168.0.40 だとします。

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

コンテナ名はmy_ros2-A

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

sudo docker pull wisteriahill/ros:foxy-ros-base-l4t-r32.4.4

1	sudo docker pull wisteriahill/ros:foxy-ros-base-l4t-r32.4.4

コンテナ作成

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

起動

sudo docker start -i my_ros2-A

1	sudo docker start -i my_ros2-A

アップデート＆アップグレード

apt update
apy upgrade -y

apt install nano

apt update

apy upgrade -y

apt install nano

セットアップが終わったらCtrl + D で終了。

Micro-XRCE-DDS-Agent をインストールします。

コンテナ再起動

sudo docker start -i my_ros2-A

1	sudo docker start -i my_ros2-A

gitパッケージをインストール

apt install git

1	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/

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

コンテナを起動

sudo docker start -i my_ros2

1	sudo docker start -i my_ros2

以下を作成

【publisher.py】

topic名はchatterにしています。

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
 
rclpy.init()
 
node = Node("talker")
 
pub = node.create_publisher(String, "chatter",10)
 
msg = String()
 
msg.data = 'Hello M5Stack!'
 
pub.publish(msg)
 
node.destroy_node()
 
rclpy.shutdown()

import rclpy

from rclpy.node import Node

from std_msgs.msg import String

rclpy.init()

node = Node("talker")

pub = node.create_publisher(String, "chatter",10)

msg = String()

msg.data = 'Hello M5Stack!'

pub.publish(msg)

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でSubscriberを作成

Arduino IDE を起動

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

以下のコードで、XXXX、YYYYを書き換えてコピー・ペースト

上記に設定で例として、AgentのIPアドレスは192.168.0.40 だとしています。

topic名はchatterにしています。

#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.40"
#define AGENT_PORT 2018 //AGENT port number

void beep(){
  M5.Speaker.begin();  
  M5.Speaker.setVolume(4);  // 0は無音、1～8
  M5.Speaker.beep(); 
  delay(1000);               // 1秒間、鳴らす
  M5.Speaker.mute(); 
  
}

void subscribeString(std_msgs::String* msg, void* arg){
  (void)(arg);
  
  M5.Lcd.clear();
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setCursor(0, 110);
  M5.Lcd.println("Received: " + String(msg->data));
  beep();
}

class StringSub : public ros2::Node{
public:
  StringSub()
  : Node(){
    this->createSubscriber<std_msgs::String>("chatter", (ros2::CallbackFunc)subscribeString, nullptr);
  }
};

WiFiUDP udp;

void setup() {
  M5.begin();
  M5.Power.begin();
  WiFi.begin(SSID, SSID_PW);
　M5.Lcd.setTextSize(2);
  M5.Lcd.println("Wi-Fi initialized!");
  while(WiFi.status() != WL_CONNECTED);
  M5.Lcd.println("Wi-Fi connected!");
  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  M5.Lcd.print("START");
}

void loop() {
  static StringSub 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 "192.168.0.40"

#define AGENT_PORT 2018 //AGENT port number

void beep(){

M5.Speaker.begin();

M5.Speaker.setVolume(4); // 0は無音、1～8

M5.Speaker.beep();

delay(1000); // 1秒間、鳴らす

M5.Speaker.mute();

}

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

(void)(arg);

M5.Lcd.clear();

M5.Lcd.setTextColor(GREEN , BLACK);

M5.Lcd.setTextSize(2);

M5.Lcd.setCursor(0, 110);

M5.Lcd.println("Received: " + String(msg->data));

beep();

}

class StringSub : public ros2::Node{

public:

StringSub()

: Node(){

this->createSubscriber<std_msgs::String>("chatter", (ros2::CallbackFunc)subscribeString, nullptr);

}

};

WiFiUDP udp;

void setup() {

M5.begin();

M5.Power.begin();

WiFi.begin(SSID, SSID_PW);

　M5.Lcd.setTextSize(2);

M5.Lcd.println("Wi-Fi initialized!");

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

M5.Lcd.println("Wi-Fi connected!");

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

M5.Lcd.print("START");

}

void loop() {

static StringSub StringNode;

ros2::spin(&StringNode);

}

６：通信実行して、M5Stackにメッセージが届いたらLCDに表示してBEEP音を鳴らす

●Jetson Nano でAgent を起動しておきます。

./MicroXRCEAgent udp4 -p 2018

1	./MicroXRCEAgent udp4 -p 2018

●Arduino IDE でSubscriber コードをM5Stack に書き込みます。

書き込み終了後、Agent を探して(discover)、接続を確立します。

●ラズパイでPublisher を起動してメッセージを送信。

python3 publisher.py

1	python3 publisher.py

M5Stack にメッセージが届いて、BEEP音が１秒鳴ります。

Appendix

M5Stack 側でPublisher とSubscriber を合体してみます。

publishとsubscribeでtopic名を変えておきます。一緒だと自分のpublishしたメッセージを自分でsubscribeするので、M5Stack画面がややこしくなります。

要変更：SSID、SSID_PW、AGENT_IP

Arduion IDE コード

publishのtopic：chatter

subscribeのtopic：listener

#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.40"
#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 2 //hz

String current_str = "Hello";
String previous_str = "";

void publishString(std_msgs::String* msg, void* arg){
  (void)(arg);
　static int cnt = 0;
　if(current_str ！= previous_str){
    cnt = 0;
    previous_str = current_str;
  }
　char str_buf[100];
  current_str.toCharArray(str_buf,100);
  sprintf(msg->data,"%s %d",str_buf, cnt++);
}

void subscribeString(std_msgs::String* msg, void* arg)
{
  (void)(arg);

  current_str = String(msg->data);
  M5.Lcd.println("Received: " + current_str );
}

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

WiFiUDP udp;

void setup() 
{
  M5.begin();
  WiFi.begin(SSID, SSID_PW);
  M5.Lcd.setTextSize(2);
  M5.Lcd.println("Wi-Fi initialized!");
  while(WiFi.status() != WL_CONNECTED);
  M5.Lcd.println("Wi-Fi connected!");
  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  M5.Lcd.print("START");
}

void loop() 
{
  static StringPubSub 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 "192.168.0.40"

#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 2 //hz

String current_str = "Hello";

String previous_str = "";

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

(void)(arg);

　static int cnt = 0;

　if(current_str ！= previous_str){

cnt = 0;

previous_str = current_str;

}

　char str_buf[100];

current_str.toCharArray(str_buf,100);

sprintf(msg->data,"%s %d",str_buf, cnt++);

}

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

{

(void)(arg);

current_str = String(msg->data);

M5.Lcd.println("Received: " + current_str );

}

class StringPubSub : public ros2::Node

{

public:

StringPubSub()

: Node()

{

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

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

this->createSubscriber<std_msgs::String>("listener", (ros2::CallbackFunc)subscribeString, nullptr);

}

};

WiFiUDP udp;

void setup()

{

M5.begin();

WiFi.begin(SSID, SSID_PW);

M5.Lcd.setTextSize(2);

M5.Lcd.println("Wi-Fi initialized!");

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

M5.Lcd.println("Wi-Fi connected!");

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

M5.Lcd.print("START");

}

void loop()

{

static StringPubSub StringNode;

ros2::spin(&StringNode);

}

【Subscriber.py】

topic：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()

【Publisher.py】

topic：listener

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

rclpy.init()

node = Node("talker")

pub = node.create_publisher(String, "listener",10)

msg = String()

msg.data = 'Hello!'

pub.publish(msg)

node.destroy_node()

rclpy.shutdown()

mport rclpy

from rclpy.node import Node

from std_msgs.msg import String

rclpy.init()

node = Node("talker")

pub = node.create_publisher(String, "listener",10)

msg = String()

msg.data = 'Hello!'

pub.publish(msg)

node.destroy_node()

rclpy.shutdown()

メッセージを変更すると、Subscriberに表示されるお尻の通し番号が更新されます。

topicを「chatter」にすれば、M5StackのSubscriberはこのpublishをスルーし、上記のSubscriberはこのpublishを１度だけ受信します。

FRONT

地図と画像のサイト

ラズパイからM5Stackへメッセージを届けてBEEP音でお知らせ（ROS2/DDS）

Be the first to comment

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