Serial通信したい．．．
ROS 2ノードとArduino Megaの間でシリアル通信を使ってデータを送受信するためのパッケージを作ったので公開します．

はじめに

ROS 2でロボットを作ろうとすると，マイコンにモータなどの処理をさせて，処理の分散をさせたい．それぞれの処理を作りこんで，あとはserial通信でつなげるだけ．
さて，そのSerial通信をどのようにするか．

ROS 2ではmicro-ros があり，トピック通信ができる！
が，Arduino UnoやArduino Megaは対応していないらしい．

でも，どうしてもArduino Megaでserial通信したいときがありますよね．

すると，このページを発見！

同じことを思っている方がいるものですね．

これをつかって，/cmd_velのトピックをSubscribeして，そのデータをSerialに送りたい．

が，なかなか文字化けやデータのバーストが発生しました．

これを解決するために，ROS 2とシリアル通信するために，C++でシリアルを送るコードをつくりました．

このパッケージでは，次のことができます．

Serialの送信
/cmd_velトピックを受け取り，serialでArduino Megaに送る．
Serialの受信
Arduino Mega から送られてきたserialデータをROS 2のトピックにpublishする．

環境

PC:
Ubuntu 22.04.3 LTS
ROS 2 Humble Hawksbill
マイコン:
Arduino Mega mega2560

インストール

gitリポジトリを任意のROS 2ワークスペースにクローン

git clone https://github.com/tomoswifty/ros2serial_arduino.git

ビルド

colcon build --packages-select ros2serial_arduino
source ~/.bashrc

Serialを送信するノード

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <string>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

using std::placeholders::_1;

int fd1;

class MySubscriber : public rclcpp::Node
{
public:
    MySubscriber()
        : Node("my_subscriber")
    {
        subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 10, std::bind(&MySubscriber::topic_callback, this, _1));
    }

private:
    void topic_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
    {
        char buf[32]; // メッセージのバッファーサイズを適切に制限
        int bytes_written;

        RCLCPP_INFO(this->get_logger(), "I heard: '%lf'", msg->linear.x);

        // メッセージをバッファに書き込む
        bytes_written = snprintf(buf, sizeof(buf), "%7.5f,%7.5f\n", msg->linear.x, msg->angular.z);

        if (bytes_written < 0 || bytes_written >= sizeof(buf)) {
            RCLCPP_ERROR(this->get_logger(), "Serial Failed: message formatting error");
            return;
        }

        printf("cmd_vel recv:%s\n", buf);

        int rec = write(fd1, buf, bytes_written);

        if (rec >= 0) {
            printf("Serial send:%s\n", buf);
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Serial Failed: could not write");
        }

    }

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
};

int open_serial(const char *device_name)
{
    int fd = open(device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
    fcntl(fd, F_SETFL, 0);

    if (fd < 0) {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Serial Failed: could not open %s", device_name);
        return -1;
    }

    // 以下略...
    return fd;
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("Serialport");


    char device_name[] = "/dev/arduino-mega";
    fd1 = open_serial(device_name);

    if (fd1 < 0) {
        printf("Serial Failed \n");
        rclcpp::shutdown();
        return -1;
    }

    rclcpp::spin(std::make_shared<MySubscriber>());
    rclcpp::shutdown();
    return 0;
}

Serialを受信するノード

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

int openSerial(const char *device_name){
    int  fd1 = open(device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
    
    fcntl(fd1, F_SETFL, 0);
    struct termios conf_tio;
    tcgetattr(fd1, &conf_tio);
    
    speed_t BAUDRATE = B115200;
    cfsetispeed(&conf_tio, BAUDRATE);
    cfsetospeed(&conf_tio, BAUDRATE);

    conf_tio.c_lflag &= ~(ECHO | ICANON);

    conf_tio.c_cc[VMIN] = 0;
    conf_tio.c_cc[VTIME] = 0;

    tcsetattr(fd1, TCSANOW, &conf_tio);
    return fd1;
}

int main(int argc, char **argv){
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("serial_receive_node");
    auto serial_pub = node->create_publisher<std_msgs::msg::String>("serial_in", 1000);

    char device_name[] = "/dev/arduino-mega"; // /dev/ttyACM0
    char fd1 = openSerial(device_name);
    if(fd1 < 0){
        RCLCPP_ERROR(node->get_logger(), "Serial Failed: could not open %s", device_name);
        printf("Serial Failed \n");
        rclcpp::shutdown();
    }

    rclcpp::WallRate loop_rate(20);
    while(rclcpp::ok()){
        char buf[256] = {0};
        std::string data;
        int flag = 0;

        while(true){
            int recv_data = read(fd1, buf, sizeof(buf));
            if(recv_data > 0){
                flag = 1;
                data += std::string(buf, recv_data);

                auto serial_msg = std::make_unique<std_msgs::msg::String>();
                serial_msg->data = data;
                serial_pub->publish(std::move(serial_msg));
                
                std::cout << "recv: " << data << std::endl; 
                
                break;

            }else{
                if(flag == 0) break;
            }
        }
        loop_rate.sleep();
    }
    rclcpp::shutdown();
    return 0;
}

Arduinoのコード

#include <Arduino.h>


void send_serial(int left, int right){
  Serial.print(left);
  Serial.print(",");
  Serial.print(right);
  Serial.println();
  delay(200);
}


void receive_serial() {
    if (Serial.available() > 0) {
        String receivedData = Serial.readStringUntil('\n'); // 改行までのデータを読み込む
        // データのパース（カンマで分割された2つの値を取得）
        float linear_x;
        float angular_z;
        int commaIndex = receivedData.indexOf(',');

        if (commaIndex != -1) {
        String linearXStr = receivedData.substring(0, commaIndex);
        String angularZStr = receivedData.substring(commaIndex + 1);
        
        linear_x = linearXStr.toFloat();   // 文字列を浮動小数点数に変換
        angular_z = angularZStr.toFloat();
        
        // データをシリアルモニターに表示
        Serial.print("linear.x: ");
        Serial.println(linear_x);
        Serial.print("angular.z: ");
        Serial.println(angular_z);
        }
    }
}


void setup() {
    Serial.begin(115200);
}


void loop() {
    receive_serial(); // uncomment out to recieve serial data
    // send_serial(); // uncomment out to send serial data
    
}

使い方

送信

/cmd_vel をpublishする．joyまたは，keyboardを起動する．

# joy
ros2 launch teleop_twist_joy teleop-launch.py
# key
ros2 run teleop_twist_keyboard teleop_twist_keyboard

ROS 2ノードの実行

ros2 run ros2serial_arduino serial_send_node

これでArduinoのシリアルモニターでcmd_velの内容が確認できます．

受信

ros2 run ros2serial_arduino serial_receive_node

トピックを確認すると，ArduinoからのデータがPubされていることが確認できます．

ros2 topic echo /serial_in

おわりに

これで救われる方がいるのではないでしょうか．

今後は，

Jetson で動作確認
Arduino Uno での動作確認
Serialの送受信とTopicのPub,Sub
receiveしたデータをpub，subしたデータをsendするコードをまとめる

とかをやっていきます．

参考

micro-ros
https://github.com/micro-ROS/micro_ros_arduino

ros2 arduino
https://www.arduino.cc/reference/en/libraries/ros2arduino/

ROS 2ノードからシリアル通信するプログラム
https://sites.google.com/site/robotlabo/ros/ros-2%E3%83%8E%E3%83%BC%E3%83%89%E3%81%8B%E3%82%89%E3%82%B7%E3%83%AA%E3%82%A2%E3%83%AB%E9%80%9A%E4%BF%A1%E3%81%99%E3%82%8B%E3%83%97%E3%83%AD%E3%82%B0%E3%83%A9%E3%83%A0