Zenn
⚡

ROS2(C++)とSTMのマイコンでUDP通信をする

2024/07/13に公開
C++
ROS2
stm32
cubeide
tech

ROSでやってる記事は見たけど、ROS2ではなかったので書くことにしました。(あと自分用備忘録です)
初心者なので、間違っている部分があれば遠慮なく指摘してください。

環境

CPU: Ryzen7 7700x
GPU: nVIDIA RTX4070Ti
OS: Ubuntu22.04
ROS 2: humble

マイコン: stm32 f767zi
マイコンはstm32 cubeIDEで開発しました。

なぜUDP通信をするのか

UDP通信をする以外の方法

ROS 2でマイコンと通信する方法はいくつかあります。
UDP通信をする以外で思いつくものでいえば、

  1. micro-ROS
  2. mros2
  3. USBでPCとマイコンを接続し、通信する
    あたりでしょうか。
    上の２つはマイコン上でROSを走らせることで、通信層を意識せずにマイコン側でROS2のトピックを受け取ることで動作できるため、非常に優秀です。しかしながら、
  • 複雑なRTOSをいじる必要が出てくる
  • 弊サークルでは低レイヤー(マイコン)と高レイヤー(ROS)で担当を分けているため、低レイヤー側にもROS2の知識が必要になる

という理由で、今回は使いませんでした。
また、USBによる通信は、コネクタがロボットに搭載するには抜けやすいこと、そして通信の安定性などから、選定から外れました。
以上の理由から、UDPの通信をするノードを作成します。

マイコン側(STM32 CubeIDE)

CubeIDE側も設定していきます。
まず、プロジェクトを作成します。今回はNUCLEO F767ZIを使うのでBoardの欄からF767ZIを探し、選択します。下記の様な画面になるまでは特に設定はいじらず進んでOKです。

ここまで来たら、おそらくMySTへのログインを求められるので、していない方は上部メニューのMySTからログインをしてください。
ここから、ボード側の設定をしていきます。
基本的には、ここを参考にさせていただきました。

Ethernetの設定

まずは、Connectivity>EthernetでModeをdisableから、RMIIにします。

RMIIとは

RMIIとは、Reduced Media-Independent Interfaceの略でPHYとMACを減らすために50MHzで2ビット幅のデータパスで動作することで、100Mbpsの通信を実現しています。
詳細はここで説明されています

また、同様に次の項目を設定していってください。
System Core>SYS debugをDisableからSerial Wireに
System Core>SYS Timebase Sourceを適当なタイマーに(今回はTIM14を使いました)

System Core>RCC High Speed ClockをBypass Clock Source
Low Speed ClockをCrystal/Ceramic Resonatorに

Connectivity>Ethernet>Parameter Settings(下部)>RX Mode　をInterruptに

FreeRTOSも設定します。
Middleware and Software packs>FreeRTOS　InterfaceをDisable→CMSIS V1
また、FreeRTOS設定下部のConfig Parametersから、Minimal Stack Sizeを256 Wordsに

IP周り(LWIP)の設定をします。
静的にIPを振りたかったので、DHCPを無効化して使いました。

LWIPとは

LWIPとは、Light weight IPの略で、幅広く使われてるオープンソースのTCP/IP実装
非常に軽量で、マイコンなどの組み込みシステムによく使われるらしいです。
ドキュメントはここがわかりやすかったです。
https://www.nongnu.org/lwip/2_1_x/index.html

Middleware and software packs>LWIP からEnabledのチェックを入れます
下部General Settingsから、LWIP_DHCPの項目をDisabledに
IP Address Settingsから、IPアドレスとネットマスク、ゲートウェイの設定をします。
今回は、IPアドレスは192.168.21.111に設定しました。

また、Configuration>Platform SettingsのDriver_PHYを両方LAN8742にしてください。

ピン設定

初期時点でEthernetに必要なピンは割り当てられているのですが、PB0だけGreen_LEDからETH_TXDに変更してください。

------------------ここまで設定が終わったら、Generate Codeしましょう---------------

プログラム

それではコードを書いていきます。今回はヘッダなどには分けずにmain.cにすべて直書きします。
実際に使うときは
lwipでudpを使う方法はいくつかありますが、(raw apiやsockets api等)
今回はsockets apiを使ってsocketを作成し、通信をしていきます。
lwipのドキュメントは(https://www.nongnu.org/lwip/2_1_x/index.html)を参照するといいと思います。

インクルードとマクロ定義

ここでは、sockets apiを使うためのlwipのヘッダファイルのインクルードとポートとアドレスの定義を行います。F7のアドレスは使いませんが、一応入れています。

main.c
/* USER CODE BEGIN Includes */
#include "sockets.h"
/* USER CODE END Includes */

~~~

/* USER CODE BEGIN 0 */
#define F7_ADDR "192.168.21.111"
#define PC_ADDR "192.168.21.100"
#define F7_PORT 4001
#define PC_PORT 4001
/* USER CODE END 0 */

StartDefaultTask内の処理

今回はmain関数内には一切処理は書かず、StartDefaultTask関数の中に処理を書いていきます。
基本やっていることはコメントに書いています。

main.c
void StartDefaultTask(void const * argument)
{
  /* init code for LWIP */
  MX_LWIP_Init();

  /* USER CODE BEGIN 5 */
  //データを格納する配列
  uint8_t rxbuf[16];
  uint8_t txbuf[20] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20};
  //アドレスを宣言
  struct sockaddr_in rxAddr,txAddr;
  //ソケットを作成
  int socket = lwip_socket(AF_INET, SOCK_DGRAM, 0);
  //アドレスのメモリを確保
  memset((char*) &txAddr, 0, sizeof(txAddr));
  memset((char*) &rxAddr, 0, sizeof(rxAddr));
  //アドレスの構造体のデータを定義
  rxAddr.sin_family = AF_INET; //プロトコルファミリの設定(IPv4に設定)
  rxAddr.sin_len = sizeof(rxAddr); //アドレスのデータサイズ
  rxAddr.sin_addr.s_addr = INADDR_ANY; //アドレスの設定(今回はすべてのアドレスを受け入れるためINADDR_ANY)
  rxAddr.sin_port = lwip_htons(PC_PORT); //ポートの指定
  txAddr.sin_family = AF_INET; //プロトコルファミリの指定(IPv4に設定)
  txAddr.sin_len = sizeof(txAddr); //アドレスのデータのサイズ
  txAddr.sin_addr.s_addr = inet_addr(PC_ADDR); //アドレスの設定
  txAddr.sin_port = lwip_htons(PC_PORT); //ポートの指定
  (void)lwip_bind(socket, (struct sockaddr*)&rxAddr, sizeof(rxAddr)); //IPアドレスとソケットを紐付けて受信をできる状態に
  socklen_t n; //受信したデータのサイズ
  socklen_t len = sizeof(rxAddr); //rxAddrのサイズ
  /* Infinite loop */
  for(;;)
  {
    n = lwip_recvfrom(socket, (uint8_t*) rxbuf, sizeof(rxbuf), (int) NULL, (struct sockaddr*) &rxAddr, &len); //受信処理(blocking)
    lwip_sendto(socket, (uint8_t*) txbuf, sizeof(txbuf), 0, (struct sockaddr*) &txAddr, sizeof(txAddr)); //受信したら送信する
  }
  /* USER CODE END 5 */
}

このような感じでソケットを用いて送受信の処理ができます。
本来は極力割り込み処理を使ってblockingな処理をなくしたかったのですが、やり方がよくわからなかったのでこのような処理になりました。(わかったらまた記事を作成するかも...?)

ROS 2側のプログラム

次に、PC(ROS 2)側でUDPの送受信をするプログラムを書いていきます。
今回は、送受信共通で使えるRos2UDPクラスを作成して、それをインクルードしてノードを作成していきます。
こちらもソケットを作成して通信を行うため、F7側のコードに似た感じになっています。

パッケージのディレクトリ

今回作成するudp_packageのディレクトリはこのようになっています。

.
├── CMakeLists.txt
├── include
│   └── robohan_udp.hpp
├── package.xml
└── src
    ├── receive_udp.cpp
    ├── robohan_udp.cpp
    └── send_udp.cpp
ros2_udp.hpp
#ifndef _ROS2_UDP_HPP_
#define _ROS2_UDP_HPP_

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string>
#include <cstring>

class Ros2UDP{
    public:
        //コンストラクタ
        Ros2UDP(const std::string& f7_address, int f7_port);

        //ソケットにアドレスをバインド(関連付け)
        void udp_bind();

        //パケットを送信
        void send_packet(uint8_t *packet, uint8_t size);

        //受信する
        ssize_t udp_recv(uint8_t *buf, uint8_t size);
    private:
        int sock;
        struct sockaddr_in f7_addr;
};

#endif
ros2_udp.cpp
#include <cstdint>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string>
#include <cstring>
#include "ros2_udp.hpp"

//コンストラクタ
Ros2UDP::Ros2UDP(const std::string& f7_address, int f7_port){
    //ソケットを作成する。引数:(int domain, int type, int protocol)
    //domain: プロトコルファミリ(今回はAF_INET(IPv4)を選択)
    //type: ソケットのタイプ(今回はUDPを使うため、データグラムソケットを作成)
    //protocol: 使用プロトコルを設定。基本ソケットのタイプに従う0を入力。　
    sock = socket(AF_INET, SOCK_DGRAM, 0);
    f7_addr.sin_family = AF_INET;                               //f7のアドレスのプロトコルファミリを選択
    f7_addr.sin_addr.s_addr = inet_addr(f7_address.c_str());    //f7のアドレスを設定。
    f7_addr.sin_port = htons(f7_port);
}

//ソケットにアドレスをバインド
void Ros2UDP::udp_bind(){
    bind(sock, (const struct sockaddr *)&f7_addr, sizeof(f7_addr));
}

//パケットを送信する
void Ros2UDP::send_packet(uint8_t *packet, uint8_t size){
    sendto(sock, packet, size, 0, (struct sockaddr *)&f7_addr, sizeof(f7_addr));
}

//パケットを受信する
ssize_t Ros2UDP::udp_recv(uint8_t *buf, uint8_t size){
    memset(buf, 0, size);
    return recv(sock, buf, size, 0);
}

これで、ros2_udp.hppをインクルードすることで楽にUDP通信が扱えます。
それでは、ROS 2のノードを作成していきます。ROS 2のノードですが、pubsubはせず、100ms毎にUDPを送信するノードとUDPを受信して表示するだけのノードを作成します。
もちろん、subscribeしたデータを送信したり、受信したデータをpublishすれば他のノードの値を渡すこともできます。

送信ノード(送信パケットのサイズは16にしています)

send_udp.cpp
#include <cstdint>
#include <chrono>
#include <memory>
#include <string>
#include "ros2_udp.hpp"
#include "rclcpp/rclcpp.hpp"
#include "stdint.h"
#define PACKET_SIZE 16
#define F7_ADDR "192.168.21.111"
#define F7_PORT 4001

using namespace std::chrono;
class SendUDP : public rclcpp::Node
{
  public:
    SendUDP(const std::string& f7_address, int f7_port)
    : Node("send_udp"),f7_udp(f7_address,f7_port)    {
        //送信するデータの初期化(今回は全部１にしてみる)
        for(int i=0;i<PACKET_SIZE;i++){
            packet[i] = 1;
        }

        //タイマーで時間ごとにコールバックされる関数を設定
        timer_ = this->create_wall_timer(100ms, std::bind(&SendUDP::timer_callback, this));
    }

  private:
    rclcpp::TimerBase::SharedPtr timer_;
    Ros2UDP f7_udp;
    uint8_t packet[PACKET_SIZE];

    
    void timer_callback(){
        //送信処理(ros2_udp.cppで定義)
        f7_udp.send_packet(packet, sizeof(uint8_t)*PACKET_SIZE);
        //送信したデータを表示
        RCLCPP_INFO(this->get_logger(),"send: %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d",packet[0],packet[1],packet[2],packet[3],packet[4],packet[5],packet[6],packet[7],packet[8],packet[9],packet[10],packet[11],packet[12],packet[13],packet[14],packet[15]);
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SendUDP>(F7_ADDR,F7_PORT);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

受信ノード

receive_udp.cpp
#include "ros2_udp.hpp"
#include "rclcpp/rclcpp.hpp"
#include <cstdint>
#include <sys/socket.h>
#include <string>
#include <cstring>
#include <stdio.h>
#define BUFFER_SIZE 20
#define ANY_ADDR "0.0.0.0"
#define F7_PORT 4001

class ReceiveUDP : public rclcpp::Node{
	public:
		ReceiveUDP(const std::string& f7_address, int f7_port)
			:Node("receive_udp"),f7_udp(f7_address,f7_port){
				f7_udp.udp_bind();
			}
        void receive_data(){
            uint8_t buf[BUFFER_SIZE] = {0};
            ssize_t received_length = f7_udp.udp_recv(buf, sizeof(buf));

            if(received_length > 0){
                RCLCPP_INFO(this->get_logger(), "Received Data = %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",buf[0],buf[1],buf[2],buf[3],buf[4],buf[5],buf[6],buf[7],buf[8],buf[9],buf[10],buf[11],buf[12],buf[13],buf[14],buf[15],buf[16],buf[17],buf[18],buf[19]);
            }else{
                RCLCPP_ERROR(this->get_logger(),"Failed to receive udp data.");
            }
        }
	private:
        Ros2UDP f7_udp;
};

int main(int argc, char * argv[]){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<ReceiveUDP>(ANY_ADDR, F7_PORT);
    while(1){node->receive_data();}//無限ループして受信処理
    rclcpp::shutdown();
    return 0;
}

これで一応は送信、受信のノードができました。
受信に関してはブロッキングな処理になるので、非同期的な処理などを入れたほうがいいかもしれないです。
一応ですが、CMakeLists.txtとpackage.xmlもおいておきます。

CMakeLists.txt
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(udp_package)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(rclcpp REQUIRED)
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

set(send_sources
src/send_udp.cpp
src/ros2_udp.cpp
include/ros2_udp.hpp)

set(receive_sources
src/receive_udp.cpp
src/ros2_udp.cpp
include/ros2_udp.hpp)

add_executable(udp_send ${send_sources})
add_executable(udp_receive ${receive_sources})
target_include_directories(udp_send PRIVATE ./include)
target_include_directories(udp_receive PRIVATE ./include)

install(TARGETS
	udp_send
    udp_receive
	DESTINATION lib/${PROJECT_NAME}
	)

ament_package()
package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>udp_package</name>
  <version>0.0.0</version>
  <description>Package for sending and receiving UDP in ROS2.</description>
  <maintainer email="legitlegit0108@gmail.com">legit0108</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

これで、UDPのプログラムがすべてかけました。
ワークスペースのルートディレクトリに移動してから、colcon build --symlink-installでビルドしておきましょう。

動作テスト

それでは、動作するか確認してみます。
動作テストした環境は画像のような感じです。

F7はPCにst-linkでつなぎ、main.cを開いた状態でRunのボタンを押しましょう。（⇓このボタン）

PC側は、ビルドしたワークスペースのディレクトリで

source ./install/setup.bash
ros2 run udp_package udp_receive

もう一つのコマンドプロンプトを開いて

source ./install/setup.bash
ros2 run udp_package udp_send

と打って実行しましょう。

実行結果

ROS2 送信プログラムはこのような出力が得られます。

ちゃんと1が16個送信されていることがわかります。

次に、受信プログラムは、F7側から返ってきた値を読み取っていて、このような出力が得られます。

F7側から、1~20の値が送信されていることが確認できました！

一応F7側で値が受け取れているのかの確認もしておきましょう。
値の確認にはDebug modeを使います。
まず、受信の際に処理を止めるため、下の画像のように、送信する部分でBreakPointを設定します。行数が書いてある部分を右クリックするとメニューが出るので、Toggle breakpointを押すと青い丸がつきます。

BreakPointがあると、デバッグモードの際にそこで処理が一時停止します。
それでは、
Runボタンの左側にある、Debug(虫のボタン)を押しましょう。
Resumeボタンを押すことで、次の処理へ進みます。
そして、受信する部分まで行き、PC側でUDP送信プログラムを動かすと受信処理がされ、
右のVariablesタブを見るとちゃんとrxbufの配列が1で埋められているのがわかると思います。

終わり

今回は、CubeIDEとPCの通信方法の一つとしてUDP通信で実装する方法を紹介しました。
今回書いたプログラムは他の処理を止めてしまう可能性があるため、もう少し非同期やマルチスレッドなどの処理を入れないと行けないかもしれません。また知見を得たら記事を書きます。
ここまで長い記事を書いたのは初めてなので結構新鮮でした。間違っているところ等あればコメントをください。

参考にさせていただいた記事

Discussion