STMマイコンとROSでUDP通信

　今回はSTMマイコンとROS間のUDP通信を実装します。マイコンはSTM32F767ZI，ルーターはTL-WR902AC，ROSはNoeticを使用します。事前にUbuntu20.04にROSとCubeIDEをインストールしておいてください。

ROS側の実装

準備

まず初めにROSでプロジェクトを作りましょう。

$ cd ~/catkin_ws/src
$ catkin_create_pkg udp_sample roscpp rospy std_msgs

本筋からは逸れますが，今回のプログラムに用いるメッセージ型を実装しておきます。

$ cd udp_sample
$ mkdir msg
$ cd msg
$ nano SampleMsg.msg

以下の内容を書き込んでください。
(string型も試してみたかったのですが色々と面倒で断念しました……)

int32 int_sample
float32 float_sample

Ctrl+S，Ctrl+Xで保存して閉じます。
ROS講座10 カスタムROSメッセージに従って今作成したメッセージを使えるようにしてください。

最後にビルドしておきます。

$ cd ~/catkin_ws
$ catkin_make

データを送信するプログラムの実装

pythonで実装していきます。

$ roscd udp_sample
$ cd src
$ nano send_udp.py 

以下の内容を書き込みます。

#!/usr/bin/env python
import socket
import struct
import rospy
from udp_sample.msg import SampleMsg

def callback(data):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    server_address = ('192.168.0.20', 7777)
    message = struct.pack("<if",
                        data.int_sample,
                        data.float_sample)
    sock.sendto(message, server_address)
    sock.close()

if __name__ == "__main__":
    send_data = SampleMsg()
    rospy.init_node('send_udp')
    sub = rospy.Subscriber("ros_data", SampleMsg, callback, queue_size=10)
    rospy.spin()

プログラムの大まかな解説

プログラムが起動するとrospy.spin()で処理が停止し，Subscribeする状態になります。
このときにトピックを受信するとcallback()が呼び出されます。

callback関数内のこの部分ではソケットを作成しています。AF_INETで IPv4，SOCK_DGRAMでudpプロトコルを指定します。詳しくは公式ドキュメントを参照してください。

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

ここでデータを送る相手(マイコン)のIPアドレスとポートを指定します。今回は例としてこの値を使います。

server_address = ('192.168.0.20', 7777)

ここではデータをC言語で読み取り可能なバイナリデータに変換しています。
基本的にはstruct.pack(フォーマット, 値, 値……)という風に使います。
フォーマットの書き方などstructの詳しい使い方は公式ドキュメントを参考にしてください。

message = struct.pack("<if",
                    data.int_sample,
                    data.float_sample)

sock.sendto()で先ほど指定したIPとポートに対してデータを送信します。sock.close()でソケットを閉じます。

sock.sendto(message, server_address)
sock.close()

データを受信するプログラムの実装

$ nano receive_udp.py 

以下の内容を書き込みます。

#!/usr/bin/env python
import rospy
import socket
import struct
from udp_sample.msg import SampleMsg

class ReceiveUDP():
    def __init__(self, data_struct, server_address):
        self.data_struct = "<if"
        server_address = ('0.0.0.0', 8888)
        self.pub = rospy.Publisher("f7_data", SampleMsg, queue_size=10)
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.bind(server_address)
    def loop(self):
        self.sock.setblocking(0)
        try:
            data, address = self.sock.recvfrom(1024)
        except socket.error:
            return
        else:
            if struct.calcsize(self.data_struct) == len(data):
                f7_data = SampleMsg()
                [int_data, float_data] = struct.unpack(self.data_struct, data)
                f7_data.int_sample = int_data
                f7_data.float_sample = float_data
                self.pub.publish(f7_data)
            else:
                rospy.loginfo("received invalid data")
    def close(self):
        self.sock.close()

if __name__ == "__main__":
    rospy.init_node('receive_udp')
    receive_udp = ReceiveUDP()
    try:
        while not rospy.is_shutdown():
            receive_udp.loop()
            rospy.Rate(50).sleep()
    except KeyboardInterrupt:
        pass
    finally:
        receive_udp.close()

プログラムの大まかな解説

この部分で受信データの形式と，受信側(つまりPC)のIPとポートを指定し，ReceiveUDPクラスのインスタンスを作成しています。

receive_udp = ReceiveUDP()

インスタンスを作成すると__init__()が呼ばれます。
__init__()内のこの部分で受信データの形式と，受信側(つまりPC)のIPとポートを指定します。
IPアドレスを0.0.0.0とすることで，PC上に存在するすべてのIPに対応することができるようです。

self.data_struct = "<if"
server_address = ('0.0.0.0', 8888)

この部分では先程と同様にIPv4，udpプロトコルのソケットを作成し，IPとポートを紐づけています。

self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(server_address)

whileループ内では大体50HzでReceiveUDPクラスのloop()関数が呼び出されるようになっています。

while not rospy.is_shutdown():
    receive_udp.loop()
    rospy.Rate(50).sleep()

loop()内のこの行ではソケットに受信可能なデータが来るまで処理を停止させています。

self.sock.setblocking(0)

データを受信した場合は以下の部分が実行され，受信データがdataに，送信元のIPがaddressに格納されます。
その際に何らかのエラーが発生した場合はreturnして再び待機状態になります。1024は受信バッファーサイズです。

try:
    data, address = self.sock.recvfrom(1024)
except socket.error:
    return

上手くいった場合は次の部分が実行されます。
受信データ形式と実際のデータのサイズが等しい場合は正常なデータを受信できていると判断しています。
struct.unpack()はstruct.pack()の逆で，与えられたフォーマットに従いバイナリデータを変換しています。
あとは変換されたデータをpublishしているだけです。

if struct.calcsize(self.data_struct) == len(data):
    f7_data = SampleMsg()
    [int_data, float_data] = struct.unpack(self.data_struct, data)
    f7_data.int_sample = int_data
    f7_data.float_sample = float_data
    self.pub.publish(f7_data)
else:
    rospy.loginfo("received invalid data")

プログラムが終了する際は以下の部分が実行されソケットが閉じられます。

finally:
    receive_udp.close()

↓

def close(self):
    self.sock.close()

最後にファイルを実行できるようにしておきましょう。

sudo chmod +x send_udp.py receive_udp.py

CubeIDE側の実装

新しくプロジェクトを作成します。前述の通り今回筆者が使うボードはSTM32F767ZIなのでそれ以外を使っている方はあくまでも参考程度にお願いします。

.iocファイルの設定

ピン設定

(126)PG11:ETH_TX_EN
(128)PG13:ETH_TXD0

RCC

HSE:Bypass Clock Source
LSE:Crystal/Ceramic Resonator

SYSの設定

Timebase Source:TIM14

ETHの設定

Mode:RMII

• Parameter Settings
  PHY Address:0
  Rx Mode:Interrupt Mode
  

FREERTOSの設定

Interface:CMSIS_V1
MINIMAL_STACK_SIZE:256

LWIPの設定

Enabledにチェックを入れてください。

• General Settings
  LWIP_DHCP:Disabled
  IP_ADDRESS:192.168.000.020（先ほどマイコンのIPとして指定した値）
  NETMASK_ADDRESS:255.255.255.000
  GATEWAY_ADDRESS:192.168.000.001
  

Clockの設定

HSEに対する入力を8MHzにしてください（一番左）。

以上の設定が終わったらGenerate Codeしましょう。

データを送受信するプログラムの実装

rtos_udp.h

ProjectExplorerのプロジェクト上で右クリックをしてNew>Header Fileを選択します。
Header file:rtos_udp.hとしてヘッダーファイルを作成してください。
そこに以下の内容を書き込みます。

#ifndef INC_RTOS_UDP_H_
#define INC_RTOS_UDP_H_

#include "lwip.h"
#include "lwip/sockets.h"

struct receive_data {
    int int_sample;
    float float_sample;
};

struct send_data {
    int int_sample;
    float float_sample;
};

struct receive_data GetROSData();
void SendF7Data(struct send_data*);
void UDPDefineTasks();

#endif /* INC_RTOS_UDP_H_ */

今回は受信したデータをそのままオウム返しするのでreceive_dataとsend_dataの中身は同じにします。

rtos_udp.c

Srcフォルダ内にrtos_udp.cというファイルを作りましょう。
そこに以下の内容を書き込みます。

------------（長いので畳んでいます）------------------

#include <rtos_udp.h>
static struct receive_data ros_data = { };
static struct send_data f7_data = { };
struct timeval tv;
//IPとポート
#define F7_ADDR "192.168.0.20"
#define PC_ADDR "192.168.0.100"
#define F7_PORT 7777
#define PC_PORT 8888

osThreadId udpTaskHandle;
uint32_t udpTaskBuffer[512];
osStaticThreadDef_t udpTaskControlBlock;
void UDPSendReceive(void const *argument);

void UDPDefineTasks() {
	osThreadStaticDef(udpTask, UDPSendReceive, osPriorityNormal, 0, 512, udpTaskBuffer, &udpTaskControlBlock);
	udpTaskHandle = osThreadCreate(osThread(udpTask), NULL);
}

void UDPSendReceive(void const *argument) {
	osDelay(1000);
	fd_set reading;
	int rxsock;
    int maxfd = 0;
	char rxbuf[256]; //最大受信データサイズ
	char txbuf[256]; //最大送信データサイズ
	struct sockaddr_in rxAddr, txAddr;
    //ソケットを作成(IPv4, UDPプロトコル)
	rxsock = lwip_socket(AF_INET, SOCK_DGRAM, 0);
    //構造体の初期化
	memset((char*) &rxAddr, 0, sizeof(rxAddr));
	memset((char*) &txAddr, 0, sizeof(txAddr));
    //受信アドレスの設定
	rxAddr.sin_family = AF_INET;
	rxAddr.sin_len = sizeof(rxAddr);
	rxAddr.sin_addr.s_addr = INADDR_ANY; //全てのIPから受信
	rxAddr.sin_port = htons(F7_PORT); //マイコンのポート
    //送信アドレスの設定
	txAddr.sin_family = AF_INET;
	txAddr.sin_len = sizeof(txAddr);
	txAddr.sin_addr.s_addr = inet_addr(PC_ADDR); //PCのIP
	txAddr.sin_port = htons(PC_PORT); //PCのポート
    //ソケットにマイコンのIPとポートを紐づける
	(void)lwip_bind(rxsock, (struct sockaddr*)&rxAddr, sizeof(rxAddr));
    //ディスクリプタ集合の初期化
	FD_ZERO(&reading);
	while (1) {
		FD_SET(rxsock, &reading);
		maxfd = rxsock + 1;
		memset(&tv, 0, sizeof(tv));
        tv.tv_usec = 20000;
		(void)select(maxfd, &reading, NULL, NULL, &tv);
        //readingにrxsockが登録されているか調べる
		if (FD_ISSET(rxsock, &reading)) {
			socklen_t n;
			socklen_t len = sizeof(rxAddr);
            //rxbufに受信データを格納
			n = lwip_recvfrom(rxsock, (char*) rxbuf, sizeof(rxbuf), (int) NULL, (struct sockaddr*) &rxAddr, &len);
			if (n == sizeof(struct receive_data)) {
				//rxbufの位置にreceive_data構造体を作る
				struct receive_data *d = (struct receive_data*) &rxbuf;
				//受信データをコピーする
				memcpy(&ros_data, d, sizeof(struct receive_data));
			}
		}
        //txbufの位置にsdを宣言
    	struct send_data* sd = (struct send_data*)&txbuf;
        //送信データをコピーする
		memcpy(sd, &f7_data, sizeof(struct send_data));
		(void)lwip_sendto(rxsock, (char*)txbuf, sizeof(struct send_data), 0, (struct sockaddr*)&txAddr, sizeof(txAddr));
		osDelay(10);
	}
}

struct receive_data GetROSData(){
	//データを取得する関数
	return ros_data;
}

void SendF7Data(struct send_data *data){
	//データを送信する関数
	memcpy(&f7_data, data, sizeof(struct send_data));
}

main.c

先程作成したヘッダーファイルをインクルードします

/* USER CODE BEGIN Includes */
#include "rtos_udp.h"
/* USER CODE END Includes */

構造体を宣言しておきます。

/* USER CODE BEGIN PFP */
static struct receive_data ros_data = {};
static struct send_data f7_data = {};
/* USER CODE END PFP */

RTOSのスレッドを宣言します。

/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
UDPDefineTasks();
/* USER CODE END RTOS_THREADS */

今回は以下のように変更し受信したデータをそのまま返すようにします。

/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
  /* init code for LWIP */
  MX_LWIP_Init();
  /* USER CODE BEGIN 5 */
  /* Infinite loop */
  for(;;)
  {
    ros_data = GetROSData();
    memcpy(&f7_data, &ros_data, sizeof(struct send_data);
    SendF7Data(&f7_data);
    osDelay(10);
  }
  /* USER CODE END 5 */
}

/* USER CODE BEGIN Header_StartSensorTask */

以上でCubeIDE上での作業は終わりです。コンパイルしてマイコンに書き込みましょう。

ネットワークの設定

ルーターの側面にあるMode SwitchをAP/Rng Ext/Clientにしてください。

ルーターを電源に接続してからPCでWiFiに接続します。
ブラウザから192.168.0.1にアクセスすると，ユーザー名とパスワードを聞かれるので両者ともにadminと入力します。

こんな感じの画面が表示されたら、まずワイヤレス2.4GHzを無効化します。
次にお好みでワイヤレス5GHzのワイヤレスネットワーク名を変更してください。
変更するといったんWiFiの接続が切れるので，通常のWiFiに接続しなおしてください。
ターミナルを開いて以下のコマンドを入力します。

$ sudo apt install network-manager
$ nmtui

　紫色の画面が表示されたら
Edit a connection→<Add>→Wi-Fiと順に選択し，Edit Connection画面を開きます。

プロファイル名は適当，
デバイスにwlp2s0，
SSIDに先程設定したネットワーク名，
セキュリティをWPA&WPA2パーソナル，
パスワードにルーターのパスワードを入力します。
IPv4設定をマニュアルにして，アドレスに192.168.0.100/24と入力します（PCのIPとして設定した値/24）。
IPv6設定を無効化します。
以上を設定出来たら<OK>から画面を閉じて再起動します。

$ nmtcli

ルーターのWiFiに接続できていることを確認してください。

テスト

以上ですべての作業が終わりましたので，早速テストしてみましょう。
F7とルーターに電源を供給し，互いをコネクターでつなぎます。PCをルーターのWiFiに接続させたらターミナルを開いてください。
以下のコマンドをそれぞれ別のターミナルに打ち込みます。

$ roscore
$ rosrun udp_sample receive_udp.py 
$ rosrun udp_sample send_udp.py 
$ rostopic echo /f7_data
$ rostopic pub -1 /ros_data udp_sample/SampleMsg 114 5.14

上手くいっていればこんな感じになります。

pubする値を変更するとechoで受け取る値も変わるはずです。

終わり

今回はROSserialなどに頼らず、UDPでマイコンとROSをつなぐ方法について紹介しました。やり取りするデータを変更するなどして、是非活用してみてください。