Qiita Teams that are logged in
You are not logged in to any team

Log in to Qiita Team
Community
OrganizationEventAdvent CalendarQiitadon (β)
Service
Qiita JobsQiita ZineQiita Blog
10
Help us understand the problem. What are the problem?

More than 1 year has passed since last update.

posted at

updated at

ROS講座44 シリアル通信とROSをつなぐ

環境

この記事は以下の環境で動いています。

項目
CPU Core i5-8250U
Ubuntu 16.04
ROS Kinetic

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

UARTなどのシリアル通信は多くのマイコンとの通信で利用されています。ROSでもrosserialという通信する相手先にもROSのスタックを載せる方法はよく使われていますが、単純にシリアル通信をするノードはないので今回作りました。

ソースコード

とりあえずシリアル通信をするノード

hard_lecture/src/hard_serialport.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"

#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 = B1000000;
    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 fd1;
void serial_callback(const std_msgs::String& serial_msg){
    int rec=write(fd1,serial_msg.data.c_str(),serial_msg.data.size());
    if(rec>=0)printf("send:%s\n",serial_msg.data.c_str());
    else{
        ROS_ERROR_ONCE("Serial Fail: cound not write");
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "s4_comport_serialport");
    ros::NodeHandle n;

    //Publisher
    ros::Publisher serial_pub = n.advertise<std_msgs::String>("Serial_in", 1000);

    //Subscriber
    ros::Subscriber serial_sub = n.subscribe("Serial_out", 10, serial_callback); 

    char device_name[]="/dev/ttyUSB0";
    fd1=open_serial(device_name);
    if(fd1<0){
        ROS_ERROR("Serial Fail: cound not open %s", device_name);
        printf("Serial Fail\n");
        ros::shutdown();
    }

    ros::Rate loop_rate(200); 
    while (ros::ok()){
        char buf[256]={0};
        int recv_data=read(fd1, buf, sizeof(buf));
        if(recv_data>0){
            printf("recv:%03d %s\n",recv_data,buf);
            std_msgs::String serial_msg;
            serial_msg.data=buf;
            serial_pub.publish(serial_msg);
        }
        ros::spinOnce();
        loop_rate.sleep();
    } 
    return 0;
}

切れた場合の再接続、Diagnosticsを付与したノード

hard_lecture/src/hard_serialport_retry.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "diagnostic_updater/diagnostic_updater.h"

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

class serial_stream{
private:
    int fd;
    std::string device_name;
    bool connected;
    int read_success;
public:
    serial_stream(void){
        connected=false;
        read_success=0;
    }
    void set_name(std::string name){
        device_name=name;

    }
    void ss_open(void){
        fd=open(device_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
        fcntl(fd, F_SETFL,0);
        //load configuration
        struct termios conf_tio;
        tcgetattr(fd,&conf_tio);
        //set baudrate
        speed_t BAUDRATE = B1000000;
        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(fd,TCSANOW,&conf_tio);
        if(fd>=0){
            connected=true;
        }
        else{
            connected=false;
        }
    }
    void ss_write(std::string data0){
        if(connected){
            int rec=write(fd,data0.c_str(),data0.size());
            if(rec<0){
                connected=false;
                ss_close();
            }
        }
    }
    std::string ss_read(void){
        if(connected){
            char buf[256]={0};
            int recv_data=read(fd, buf, sizeof(buf));
            if(recv_data>0){
                read_success++;
                std::string recv_string=buf;
                return buf;
            }
            else{
                return "";
            }
        }
    }
    void ss_close(void){
        close(fd);
        connected=false;
    }
    bool ss_connected(void){
        return connected;
    }
    bool ss_status(void){
        if(read_success>0){
            read_success=0;
            return true;
        }
        else return false;
    }
};
diagnostic_updater::Updater *p_updater;
ros::Publisher serial_pub;
std::string device_name="/dev/ttyUSB0";
serial_stream ss0;

void serial_callback(const std_msgs::String& serial_msg){
    ss0.ss_write(serial_msg.data);
}
bool first_time=true;
bool last_connected=false;
void timer_callback(const ros::TimerEvent&){
        if(!ss0.ss_connected()){
            ss0.ss_open();
            if(ss0.ss_connected())ROS_INFO("Serial Open %s", device_name.c_str());
            else{
                if     (first_time)    ROS_ERROR("Serial Fail: cound not open %s", device_name.c_str());
                else if(last_connected)ROS_ERROR("Serial Fail: Connection is broken %s", device_name.c_str());
            }
        }
        else{
            std::string recv_data=ss0.ss_read();
            if(recv_data.size()>0){
                //printf("recv:%03d %s\n",(int)recv_data.size(),recv_data.c_str());
                std_msgs::String serial_msg;
                serial_msg.data=recv_data;
                serial_pub.publish(serial_msg);
            }
        }
        first_time=false;
        last_connected=ss0.ss_connected();
        p_updater->update();        
        ros::spinOnce();
}

void diagnostic0(diagnostic_updater::DiagnosticStatusWrapper &stat){
    bool serial_c=ss0.ss_connected();
    bool serial_s=ss0.ss_status();
    if     (serial_c &&  serial_s)stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK,    "Active.");
    else if(serial_c && !serial_s)stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN,  "No Recieve.");
    else                          stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "No Connection.");
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "hard_serialport_retry");
    ros::NodeHandle n;
    ros::NodeHandle pn("~");
    //param
    pn.getParam("device_name", device_name);
    ss0.set_name(device_name);
    //Publisher
    serial_pub = n.advertise<std_msgs::String>("Serial_in", 10);
    //timer
    ros::Timer timer = n.createTimer(ros::Duration(0.01), timer_callback);
    //Diagnostic
    diagnostic_updater::Updater updater;
    p_updater=&updater;
    updater.setHardwareID("SerialPort");
    updater.add("Connect", diagnostic0);
    //Subscriber
    ros::Subscriber serial_sub = n.subscribe("Serial_out", 10, serial_callback); 

    ros::spin();
    ss0.ss_close();
    return 0;
}

CMakeList

hard_lecture/CMakeLists.txt
add_executable(hard_serialport src/hard_serialport.cpp)
add_executable(hard_serialport_retry src/hard_serialport_retry.cpp)

target_link_libraries(hard_serialport
  ${catkin_LIBRARIES}
)
target_link_libraries(hard_serialport_retry
  ${catkin_LIBRARIES}
)

ビルド

cd ~/catkin_ws
catkin_make

シリアルポートの設定

シリアルポートの接続

シリアルポートはubuntuでシリアルポートとして認識されるものなら何でもできます。昔のPCにはシリアルポートが直接ついていますが、今のPCではほぼありません。USBシリアル変換を使うのが普通です。例えば秋月で売っているシリアル通信ケーブルが使えます。例えば以下のようなものが使えます。

hard-シリアルケーブル1.jpg
USB-シリアル変換ケーブル スケルトンより画像を拝借

hard-シリアルケーブル2.JPG
FTDI USB・シリアル変換ケーブルより画像を拝借 電源が3.3Vと5Vの2種類があるので注意
またFTDIのチップなら大体正常にシリアルポートとして認識されます。

デバイス名

Ubuntuではシリアルポートにはデバイスファイルを経由してアクセスします。標準的なシリアルポートでは/dev/ttyUSB0という名前になります。また2つUSBシリアル変換をつなげると2つ目は/dev/ttyUSB1という名前になります。
物によっては名前が違ってArduinoのUSBシリアル変換は/dev/ttyACM0という名前になります。

シリアルポートへの権限付与

USBシリアル変換(arduino)をつないだ状態で以下のコマンドで権限の確認をします。

シリアルポートの権限の確認
ll /dev/ttyACM0 
結果
crw-rw---- 1 root dialout 166, 0 Aug 17 23:55 /dev/ttyACM0

ルートユーザーdialoutグループには権限がありますが、シリアルポートの権限が一般のユーザーにはありません。そこで以下の対策が必要です。

  • sudo ユーザーで実行する
  • chmodで権限を付ける。ただしこの方法ではUSBシリアルを抜き差しするたびに実行する必要があります。
    sudo chmod 666 /dev/ttyACM0
  • ユーザーをdialoutグループに追加する。この方法だと永続的に効果が付きます。
    sudo gpasswd --add $USER dialout
    コマンド実行後に再ログインが必要です。

実行

1つ目のターミナル
roscore
2つ目のターミナル
rosrun hard_lecture hard_serialport_retry
送信する場合
rostopic pub -r 1 /Serial_out std_msgs/String "data: 'hello'" 
受信する場合
rostopic echo /Serial_in 

目次ページへのリンク

ROS講座の目次へのリンク

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
10
Help us understand the problem. What are the problem?