LoginSignup
26
17

More than 5 years have passed since last update.

@srs(project_srs)

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

Last updated at Posted at 2018-08-17

環境

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

項目 値
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
ＵＳＢ－シリアル変換ケーブル　スケルトンより画像を拝借

hard-シリアルケーブル2.JPG
ＦＴＤＩ　ＵＳＢ・シリアル変換ケーブルより画像を拝借 電源が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講座の目次へのリンク

26
17
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
26
17