ROS 2ノードからシリアル通信するプログラム

　Micro-ros for Arduinoを使ってシリアル通信するプログラムを考えていたのですが、ArduinoUNOとかArduinoMegaなどの古いボードは対応していなかったので、普通にROS 2ノードから通信するサンプルプログラムを作りました。プログラム名に「for Arduino」というのをつけないでほしいぐらいですね。。。

ArduinoUNOの「スケッチ例(Example)」->「01.Basics」->「AnalogReadSerial」を書き込んだときにPCで受信するプログラムの作成例になります。

ROS 2がインストールされていることを前提とします。

マイコンからシリアル通信でデータ受信

C++での受信プログラムになります。

連続して送られてくるアナログ電圧の値を受信しているときに改行コードをチェックして数字がおかしくならないようにしています。

もっと良いプログラムがあったらぜひ教えてください。。。

下記の要領でパッケージを作成します。

cd ~/ros2_ws/src

ros2 pkg create --build-type ament_cmake --node-name serial_node serialsample

serialsampleという名前のパッケージにserial_nodeを作成しました。適当に名前を変えてください。

次にCMakeLists.txtを編集。

cd serialsample

gedit CMakeLists.txt

編集した内容は下記。３行追加。

cmake_minimum_required(VERSION 3.8)

project(serialsample)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

  add_compile_options(-Wall -Wextra -Wpedantic)

endif()

# find dependencies

find_package(ament_cmake REQUIRED)

find_package(rclcpp REQUIRED) # 追加

find_package(std_msgs REQUIRED) # 追加

include_directories(${serial_INCLUDE_DIRS})

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package(<dependency> REQUIRED)

add_executable(serial_node src/serial_node.cpp)

target_include_directories(serial_node PUBLIC

  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

  $<INSTALL_INTERFACE:include>)

target_compile_features(serial_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

ament_target_dependencies(serial_node rclcpp std_msgs) # 追加

install(TARGETS serial_node

  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)

  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)

  ament_lint_auto_find_test_dependencies()

endif()

ament_package()

次にプログラムの本体を書き換え。

cd ~/ros2_ws/src/serialsample/src

gedit serial_node.cpp

書き換えた内容は下記の通り。

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

#include <string>

#include <unistd.h>

#include <fcntl.h>

#include <termios.h>

int open_serial(const char *device_name){ //シリアル通信の初期化

    int fd1=open(device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);

    fcntl(fd1, F_SETFL,0);

    //load configuration

    struct termios conf_tio;

    tcgetattr(fd1,&conf_tio);

    //set baudrate

    speed_t BAUDRATE = B9600; //ここで通信速度を決める 9600bpsにしてある。

    cfsetispeed(&conf_tio, BAUDRATE);

    cfsetospeed(&conf_tio, BAUDRATE);

    //non canonical, non echo back

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

    //non blocking

    conf_tio.c_cc[VMIN]=0;

    conf_tio.c_cc[VTIME]=0;

    //store configuration

    tcsetattr(fd1,TCSANOW,&conf_tio);

    return fd1;

}

int main(int argc, char **argv)

{

    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("s4_comport_serialport");

    //Publisher　シリアル通信で受信したデータを"Serial_in"の名前でトピックをPublish

    auto serial_pub = node->create_publisher<std_msgs::msg::String>("Serial_in", 1000);

    char device_name[]="/dev/ttyACM0"; //接続先のデバイスをここに記載

    fd1=open_serial(device_name);

    if(fd1<0){

        RCLCPP_ERROR(node->get_logger(), "Serial Fail: cound not open %s", device_name);

        printf("Serial Fail\n");

        rclcpp::shutdown();

    }

    rclcpp::WallRate loop_rate(200); //200Hzのループ

    while (rclcpp::ok()){

        char buf[256]={0};

        std::string data;

        int len=0;//受信文字の長さチェックのため

        int flag = 0;//受信有無のフラグ

        while(true){//改行コードを見つけるまで無限ループ

            int recv_data=read(fd1, buf, sizeof(buf));//届いたデータ読み込み

            if(recv_data>0){

                flag=1;//1回目の受信用のフラグ

                len = len + recv_data;

                //buf[recv_data]='\0';

                data += std::string(buf, recv_data);

                if(data.back()=='\n'){//改行コードがあったら文字列表示

                    //printf("recv:%03d %s\n",len, data);

                    std::cout << "recv: "<< len << " " << data << std::endl;

                    if(len>1){//改行コードだけじゃないときpublish

                      auto serial_msg = std::make_unique<std_msgs::msg::String>();

                      serial_msg->data = data;//buf;

                      serial_pub->publish(std::move(serial_msg));

                    }

                    break;

                }

            }else{//1回目の受信長がゼロならブレイク

                if(flag==0) break;

            }

        }

        loop_rate.sleep();

    } 

    rclcpp::shutdown();

    return 0;

}

このコードだと、接続先が変わったときにソース書き換えの後にコンパイルが必要になるので、実行時にパラメータを引き渡せるように修正した方が良い。

ビルドして、下記のように実行すると、ArduinoからのAD変換した値が届いていれば数字が表示されると思います。

cd ~/ros2_ws/

colcon build

source instal/setup.bash

ros2 run serialsample serial_node

また、届いた値は/Serial_inのトピックを確認してくれればきちんと値が取れていることもわかると思います。

ros2 topic echo /Serial_in

トピックで受け取ったデータをシリアルでマイコンへ送信するプログラム

今回は送信だけとなります。

問題は、トピックが来たときだけ送信なので、一定周期では送信できないということになります。

困っているのは下記。やりたいことは、シリアルの送受信と一定周期publish、subcribeというプログラム。

• 一定周期でトピックを確認してマイコンにデータ送信、その直後にデータ受信という方法がわからない。

• 一定周期でマイコンからのデータ受信をしてデータをpublish、subscribeのトピックの更新があったときにシリアルへ送信というのができていない。

とりあえず、teleop_nodeなどでtwist型のデータが/cmd_velで送信されてきたときに受信して、シリアルでマイコンへデータを送るのは下記の通り。

#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  //MySubscriberクラス

{

  public:

    MySubscriber()

    : Node("my_subscriber")

   {

      subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(

      "cmd_vel", 10, std::bind(&MySubscriber::topic_callback, this, _1));//cmd_velをsubscribe

    }

  private:

    void topic_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const

    {

        char buf[256];

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

        sprintf(buf, "%7.5f,%7.5f\n",msg->linear.x, msg->angular.z); //カンマ区切りのデータ生成

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

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

        if(rec>=0)printf("Serial send:%s\n",buf);

        else{

            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Serial Fail: cound not write");

        }

        usleep(50000);

    }

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

};

int open_serial(const char *device_name){

    int fd1=open(device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);

    fcntl(fd1, F_SETFL,0);

    //load configuration

    struct termios conf_tio;

    tcgetattr(fd1,&conf_tio);

    //set baudrate

    speed_t BAUDRATE = B115200;//ボーレート115200のとき

    cfsetispeed(&conf_tio, BAUDRATE);

    cfsetospeed(&conf_tio, BAUDRATE);

    //non canonical, non echo back

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

    //non blocking

    conf_tio.c_cc[VMIN]=0;

    conf_tio.c_cc[VTIME]=0;

    //store configuration

    tcsetattr(fd1,TCSANOW,&conf_tio);

    return fd1;

}

int main(int argc, char **argv)

{

    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("Serialport");

    //Publisher

    auto serial_pub = node->create_publisher<std_msgs::msg::String>("Serial_in", 100);

    char device_name[]="/dev/ttyACM0";//接続先のポート

    fd1=open_serial(device_name);

    if(fd1<0){

        RCLCPP_ERROR(node->get_logger(), "Serial Fail: cound not open %s", device_name);

        printf("Serial Fail\n");

        rclcpp::shutdown();

    }

    //Subscriber

    rclcpp::spin(std::make_shared<MySubscriber>());

    rclcpp::shutdown();

    return 0;

}