ROS2通过话题的发布与订阅进行串口通信

步骤

新建一个cpp_header的包

cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_header

进入include，新建头文件minimal_publisher.hpp

cd ~/dev_ws/src/cpp_header/include/cpp_header
touch minimal_publisher.hpp

代码如下：

#include "cpp_header/minimal_publisher.hpp"

#include  

Serial::Serial()
{

}
Serial::~Serial()
{

}

int Serial::BaudRate( int baudrate)
{
    if(7 == baudrate)
        return B460800;
    else if(6 == baudrate)
        return B115200;
    else if(5 == baudrate)
        return B57600;
    else if(4 == baudrate)
        return B38400;
    else if(3 == baudrate)
        return B19200;
    else if(2 == baudrate)
        return B9600;
    else if(1 == baudrate)
        return B4800;
    else if(0 == baudrate)
        return B2400;
    else
        return B9600;

}

int Serial::SetPara(int serialfd,int speed,int databits , int stopbits ,int parity )
{
    struct termios termios_new;
    bzero( &termios_new, sizeof(termios_new));//等价于memset(&termios_new,sizeof(termios_new));
    cfmakeraw(&termios_new);//就是将终端设置为原始模式
    termios_new.c_cflag=BaudRate(speed);
    termios_new.c_cflag |= CLOCAL | CREAD;
  //  termios_new.c_iflag = IGNPAR | IGNBRK;

    termios_new.c_cflag &= ~CSIZE;
    switch (databits)
    {
    case 0:
        termios_new.c_cflag |= CS5;
        break;
    case 1:
        termios_new.c_cflag |= CS6;
        break;
    case 2:
        termios_new.c_cflag |= CS7;
        break;
    case 3:
        termios_new.c_cflag |= CS8;
        break;
    default:
        termios_new.c_cflag |= CS8;
        break;
    }

    switch (parity)
    {
    case 0:  				//as no parity
        termios_new.c_cflag &= ~PARENB;    //Clear parity enable
      //  termios_new.c_iflag &= ~INPCK; /* Enable parity checking */  //add by fu
        break;
    case 1:
        termios_new.c_cflag |= PARENB;     // Enable parity
        termios_new.c_cflag &= ~PARODD;
        break;
    case 2:
        termios_new.c_cflag |= PARENB;
        termios_new.c_cflag |= ~PARODD;
        break;
    default:
        termios_new.c_cflag &= ~PARENB;   // Clear parity enable
        break;
    }
    switch (stopbits)// set Stop Bit
    {
    case 1:
        termios_new.c_cflag &= ~CSTOPB;
        break;
    case 2:
        termios_new.c_cflag |= CSTOPB;
        break;
    default:
        termios_new.c_cflag &= ~CSTOPB;
        break;
    }
    tcflush(serialfd,TCIFLUSH); // 清除输入缓存
   tcflush(serialfd,TCOFLUSH); // 清除输出缓存
    termios_new.c_cc[VTIME] = 1;   // MIN与 TIME组合有以下四种：1.MIN = 0 , TIME =0  有READ立即回传 否则传回 0 ,不读取任何字元
    termios_new.c_cc[VMIN] = 1;  //    2、 MIN = 0 , TIME >0  READ 传回读到的字元,或在十分之一秒后传回TIME 若来不及读到任何字元,则传回0
    tcflush (serialfd, TCIFLUSH);  //    3、 MIN > 0 , TIME =0  READ 会等待,直到MIN字元可读
    return tcsetattr(serialfd,TCSANOW,&termios_new);  //    4、 MIN > 0 , TIME > 0 每一格字元之间计时器即会被启动 READ 会在读到MIN字元,传回值或
}

int Serial::WriteData(int fd,const char *data, int datalength )//index 代表串口号 0 串口/dev/ttyAMA1 ......
{
    if(fd <0){ return -1;}
    int len = 0, total_len = 0;//modify8.
    for (total_len = 0 ; total_len < datalength;)
    {
        len = 0;
        len = write(fd, &data[total_len], datalength - total_len);
        printf("WriteData fd = %d ,len =%d,data = %s\n",fd,len,data);
        if (len > 0)
        {
            total_len += len;
        }
        else if(len <= 0)
        {
            len = -1;
            break;
        }
     }
       return len;
}

int Serial::ReadData(int fd,unsigned char *data, int datalength)
{
       if(fd <0){ return -1;}
       int len = 0;
       memset(data,0,datalength);

       int max_fd = 0;
       fd_set readset ={0};
       struct timeval tv ={0};

       FD_ZERO(&readset);
       FD_SET((unsigned int)fd, &readset);
       max_fd = fd +1;
       tv.tv_sec=0;
       tv.tv_usec=1000;
       if (select(max_fd, &readset, NULL, NULL,&tv ) < 0)
       {
               printf("ReadData: select error\n");
       }
       int nRet = FD_ISSET(fd, &readset);
      if (nRet)
       {
              len = read(fd, data, datalength);
       }
       return len;
}

void Serial::ClosePort(int fd)
{
    struct termios termios_old;
    if(fd > 0)
    {
        tcsetattr (fd, TCSADRAIN, &termios_old);
        ::close (fd);
    }
}

int  Serial::OpenPort(int index)
{
    char *device;
    struct termios termios_old;
    int fd;
    switch(index)
    {
    case COM0:  device="/dev/ttyS0";  break;
    case COM1:  device="/dev/ttyS1";  break;
    case COM2:  device="/dev/ttyS2";  break;
    case COM3:  device="/dev/pts/3";  break;
    case ttyUSB0:  device="/dev/ttyUSB0";  break;
    case ttyUSB1:  device="/dev/ttyUSB1";  break;
    case ttyUSB2:  device="/dev/ttyUSB2";  break;
    default: device="/dev/ttyAMA2"; break;
    }

    fd = open( device, O_RDWR | O_NOCTTY |O_NONBLOCK);//O_RDWR | O_NOCTTY | O_NDELAY   //O_NONBLOCK
    if (fd < 0)
    { return -1;}
    tcgetattr(fd , &termios_old);
    return fd;
}

注意：case COM3: device=“/dev/pts/3”; break;
这里的端口号需要打开socat进行查看，选择其中一个作为这里面的端口。
可以看我另一篇文章：链接: ROS2实现虚拟串口通信

进入src目录，新建文件minimal_publisher.cpp

cd ~/dev_ws/src/cpp_header/src
touch minimal_publisher.cpp

代码如下：

#ifndef PUB_SERIALPORT_HPP
#define PUB_SERIALPORT_HPP

#include  
#include  
#include  
#include  
#include  
#include  
#include  
#include  
#include  
#include  


enum sp_dev_e
{
    COM0 = 0,
    COM1,
    COM2,
    COM3,
    ttyUSB0,
    ttyUSB1,
    ttyUSB2
};
class Serial
{
    public:
    Serial();
    ~Serial();

    int OpenPort(int index);
    int SetPara(int serialfd,int speed=2,int databits=8,int stopbits=1,int parity=0);
    int WriteData(int fd,const char *data,int datalength);
    int ReadData(int fd,unsigned char *data,int datalength);
    void ClosePort(int fd);
    int BaudRate( int baudrate);
};


#endif // PUB_SERIALPORT_HPP

进入src目录，新建文件minimal_publisher_node.cpp

cd ~/dev_ws/src/cpp_header/src
touch minimal_publisher_node.cpp

代码如下：

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


int fp;
unsigned char Msg[128] = { 0 };
const char* constc = nullptr;
Serial sp;
class TopicSuscribe01 : public rclcpp::Node
{
public:
    TopicSuscribe01(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(),"我是%s，订阅话题为：/rostopic.",name.c_str());
        command_subscribe_ = this->create_subscription<std_msgs::msg::String>("rostopic",11,std::bind(&TopicSuscribe01::command_callback,this,std::placeholders::_1));
    }
private:
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_subscribe_;
    void command_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        double speed = 0.0f;
        if(msg->data == "9999")
        {
            speed = 0.2f;

        }
        RCLCPP_INFO(this->get_logger(),"收到的[%s]指令，发送速度%f",msg->data.c_str(),speed);
        
        constc = msg->data.c_str();
        
        sp.WriteData(fp,constc,11);
    }

};


int main(int argc, char **argv)
{
    /* code */
    rclcpp::init(argc, argv);
    /*产生一个Wang2的节点*/
    // auto node = std::make_shared("pub_serial");
    auto node = std::make_shared<TopicSuscribe01>("topic_subscribe_01");
    // 打印一句自我介绍
    // int fp;
    // Serial sp;
    sp.BaudRate(115200);
    fp = sp.OpenPort(COM3);
    if(fp>0){
      RCLCPP_INFO(node->get_logger(), "serial success!.");
    }
    sp.WriteData(fp,"1024",11);
    // cout<

    RCLCPP_INFO(node->get_logger(), "大家好，我是topic_subscribe_01.");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    sp.ClosePort(fp);
    return 0;
    
    return 0;
}

**注意：**RCLCPP_INFO(this->get_logger(),“我是%s，订阅话题为：/rostopic”,name.c_str());
这里面的rostopic是我自己定义的一个话题，一方面是为了避免与其他人的话题相同而造成数据乱传，另一方面我的发布节点用到的话题也是这个名字。

编辑package.xml

在ament_cmake后增加

<depend>rclcpp</depend>
<depend>std_msgs</depend>

编辑 CMakelist.txt

在find_package(ament_cmake REQUIRED)后增加

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

再增加可执行文件，ros2 run能够调用的名称

include_directories(include)
add_executable(minimal_publisher_node src/minimal_publisher_node.cpp src/minimal_publisher.cpp) 
ament_target_dependencies(minimal_publisher_node rclcpp std_msgs)

增加可执行文件位置，ros2 run可以找到这个可执行文件

install(TARGETS
  minimal_publisher_node
  DESTINATION lib/${PROJECT_NAME})
ament_package()

编译包

colcon build --symlink-install --packages-select cpp_header

加载工作空间

. install/setup.bash

执行

ros2 run cpp_header minimal_publisher_node

开启虚拟串口/dev/pts/4进行监听

参照我的另一篇文章：
链接: ROS2实现虚拟串口通信

cat < /dev/pts/4

开启发布节点

ros2 run example_topic_rclcpp topic_publisher_01 

这里可以参考我的另一篇博客：
链接: 利用RCLCPP实现话题的发布与订阅

开启串口通信

ros2 run cpp_header minimal_publisher_node 

结果