ROS2でUDPnodeを作ろう！

# はじめに
皆さんはros2を動かしているときに、どうやって低レイヤーと通信しようか悩むことがあると思います。今はmrosやmicro-rosなどがありますが、私は単純udp通信を使ってros側のpcとマイコンを通信してみたいと思います。
# 使用環境
* pc側

ubuntu20.04
ROS2 foxy

* マイコン側
NUCLEO-F767ZI
mbed OS 6
GNU Arm Embedded Toolchain 10.3-2021.07
Mbed CLI 2

# 準備
* pcとマイコンをLANケーブルでつなぎます。
* ubuntu側のnetworkを以下のコマンドで写真のように変更する。
```
nm-connection-editor
```
![Screenshot from 2024-05-05 00-12-47.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3531353/9b86f358-d4e9-8094-11df-ec6fa061829b.png)
* ubuntu側のファイアウォールを無効化する。
```
sudo ufw disable
```

# マイコン側のサンプルコード
このサンプルコードは数値を渡すだけです。
ロボコンなどでは、この数値にモータの速度司令を送ったりしてrosとマイコン側が連携します。
```
#include "mbed.h"
#include "EthernetInterface.h"
#include "rtos.h"
#include <cstring>

void receive(UDPSocket *receiver);

int main()
{
    const char *destinationIP = "10.42.0.1";
    const uint16_t destinationPort = 4000;
    const char *myIP = "10.42.0.111";
    const char *myNetMask = "255.255.255.0";
    const uint16_t receivePort = 5000;

    EthernetInterface net;
    SocketAddress destination, source;
    UDPSocket udp;
    Thread receiveThread;

    net.set_dhcp(false);
    net.set_network(myIP, myNetMask, "");
    printf("start\n");

    if (net.connect() != 0)
    {
        printf("network connection Error\n");
        return -1;
    }
    else
    {
        printf("network connection success\n");
    }

    udp.open(&net);
    udp.bind(receivePort);

    destination.set_ip_address(destinationIP);
    destination.set_port(destinationPort);
    receiveThread.start(callback(receive, &udp));

    // 送信データの設定
    float float_data[4] = {1.23, 4.56, 7.89, 10.11};

    // 送信
    while (1)
    {
        // 送信データのパック
        char send_data[16]; // 大きさ４のfloat型（4 * 4 = 16バイト） 
        memcpy(send_data, float_data, 4 * sizeof(float));


        // データをUDPで送信
        if (const int result = udp.sendto(destination, send_data, sizeof(send_data)) < 0)
        {
            printf("send Error: %d\n", result);
        }

        ThisThread::sleep_for(1s);
    }

    // 受信スレッドの終了を待つ（実行されない場合はコメントアウトしても良い）
    // receiveThread.join();

    udp.close();
    net.disconnect();
    return 0;
}

void receive(UDPSocket *receiver)
{
    SocketAddress source;
    char buffer[16]; // 送信データサイズに合わせる
    while (1)
    {
        memset(buffer, 0, sizeof(buffer));
        if (const int result = receiver->recvfrom(&source, buffer, sizeof(buffer)) < 0)
        {
            printf("receive Error: %d\n", result);
        }
        else
        {
            printf("from IP: %s port: %d data received\n", source.get_ip_address(), source.get_port());

            // 受信データをfloat型の配列とint型の配列にアンパック
            float float_array[4];

            memcpy(float_array, buffer, 4 * sizeof(float));

            printf("Received Float Array: %.2f, %.2f, %.2f, %.2f\n", float_array[0], float_array[1], float_array[2], float_array[3]);

        }
    }
}
```
# ROS2のnodeでUDP通信をする。
送信するudp_send_nodeと受け取るudp_receive_nodeを今回実装した。
## 今回使う独自msg(ToF7.msg)
UDP通信でpcからマイコンにx,y各方向の速度司令と、目標回転角度、今の機体の角度を送るmsgを定義している。
```
float64 v_x
float64 v_y
float64 aim_arg
float64 yaw
```
## udp_send_node
実際のコード
```
import rclpy
from rclpy.node import Node
from gakurobo_msgs.msg import ToF7
import socket
import struct

class UdpSend(Node):
    def __init__(self):
        super().__init__('udp_send_node')
        self.sub_tof7 =self.create_subscription(
            ToF7,'twist_aim',self.tof7_cb,10
        )
        timer_period = 0.1
        self.timer = self.create_timer(timer_period,self.timer_callback)
        self.send_info=ToF7()
        self.destination_ip = "10.42.0.111"
        self.destination_port = 5000
        self.flaot_data=[0.0,0.0,0.0,0.0]
        self.int_data=[100,200,300,400]
    
    def send_data(self,destination_ip, destination_port, float_data, int_data):
        # 送信データのパック
        send_data = struct.pack('<{}f{}i'.format(len(float_data), len(int_data)), *float_data, *int_data)
        # UDPソケットの作成
        with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as udp_socket:
            destination_address = (destination_ip, destination_port)
            # データをUDPで送信
            udp_socket.sendto(send_data, destination_address)
    
    def tof7_cb(self,msg):
        self.send_info=msg
        self.flaot_data[0]=self.send_info.v_x
        self.flaot_data[1]=self.send_info.v_y
        self.flaot_data[2]=self.send_info.aim_arg
        self.flaot_data[3]=self.send_info.yaw
    
    def timer_callback(self):
        self.send_data(self.destination_ip,self.destination_port,self.flaot_data,self.int_data)
        self.get_logger().info('udp_send data\n')
        
def main():
    rclpy.init()
    node = UdpSend()
    print('udp_send_node is started...')
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()
        
if __name__ == "__main__":
    main()
```
### udp_send_nodeの解説
* tof7_cb関数でsubした速度司令たちをfloat配列に格納しています。
* send_data関数で指定されたIPアドレスとポートにUDPパケットを送信しています。struct.pack関数を使用して、浮動小数点数と整数のデータをバイナリ形式にパックしています。
* timercallbackでudpを送信しています。

# udp_recive_node
この受け取るnodeはsampleですので受け取ることしかしていません。用途によってパブリッシャーを追加して実際のロボコンで使ってみてください。
実際のコード
```
import rclpy
from rclpy.node import Node
import socket
import struct

# 受信用のIPアドレスとポート番号を設定
UDP_IP = "0.0.0.0"  # すべてのネットワークインターフェースを受け付ける
UDP_PORT = 4000  # 送信元のポート番号と合わせる

# UDPソケットを作成
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((UDP_IP, UDP_PORT))


class UdpReceive(Node):
    def __init__(self):
        super().__init__('udp_receive_node')
        timer_period = 0.01
        self.timer = self.create_timer(timer_period,self.timer_callback)
        
    def timer_callback(self):
         # データを受信（バッファサイズを指定）
        data, addr = sock.recvfrom(12)  # float型の配列のバッファサイズを指定

        # 受信したデータを解析
        self.float_array = struct.unpack('ffff', data)  # float型の配列としてデータを解釈
        self.get_logger().info(f'Received Float Array:{self.float_array}')

def main():
    rclpy.init()
    node = UdpReceive()
    print('udp_receive_node is started...')
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()
        
if __name__ == "__main__":
    main()
```
# 終わりに
今回は私がなれているため、マイコン側をmbedで書きましたが、mbedはcan通信周りが非常に使いにくいのでcubeideで書いたほうが使いやすいと思います。今回のudpを使うとラズパイが簡単にロボコン用のコントローラーになるので新入生教育などにも使いやしのかなと感じました。

# 参考文献
* https://qiita.com/hbvcg00/items/552fb765530ebea3e4d8