LoginSignup
5
5

More than 1 year has passed since last update.

@tomoswifty

ROS 2とArduino MegaでSerial通信をしたい

Posted at

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

5
5
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
5