はじめに
皆さんは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
- 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を使うとラズパイが簡単にロボコン用のコントローラーになるので新入生教育などにも使いやしのかなと感じました。