在ros2-humble/src/ros/ros_tutorials/turtlesim/tutorials文件夹下有个teleop_turtle_key.cpp文件。小乌龟例程就是这个cpp文件读取键盘按键指令，并发送到话题上。

在此启发下，准备用键盘方向键控制ros小车，串口数据采用16进制方式发送。系统Ubuntu22.04，ros2humble.

参考文章：

(134条消息) 利用ROS2实现串口通信_ros2 串口_摆烂女侠的博客-CSDN博客

ROS2通过话题的发布与订阅进行串口通信_ff925的博客-CSDN博客

ROS2实现虚拟串口通信_ros 虚拟串口_ff925的博客-CSDN博客

一、新建一个cpp_header 功能包

cd ~/dev_ws/src  //进入工作空间

ros2 pkg create --build-type ament_cmake cpp_header  //新建功能包

修改package.xml文件如下：

  cpp_header   0.0.0   TODO: Package description   xxx   TODO: License declaration

  ament_cmake      rclcpp   std_msgs

  ament_lint_auto   ament_lint_common

      ament_cmake  

修改CMakeLists.txt文件如下：

cmake_minimum_required(VERSION 3.8) project(cpp_header)

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) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED)

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() add_executable(serial_subscribe1_node src/serial_subscribe1_node.cpp src/pub_serialport.cpp)  ament_target_dependencies(serial_subscribe1_node rclcpp std_msgs) add_executable(serial_publisher1_node src/serial_publisher1_node.cpp) ament_target_dependencies(serial_publisher1_node rclcpp std_msgs)

install(TARGETS serial_subscribe1_node serial_publisher1_node DESTINATION lib/${PROJECT_NAME})

二、在/dev_ws/src/cpp_header/include/cpp_header文件夹下新建pub_serialport.hpp文件，这是串口控制的头文件，写入如下内容：

#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);

    void ClosePort(int fd);     int BaudRate( int baudrate); };

#endif // PUB_SERIALPORT_HPP

三、在/dev_ws/src/cpp_header/src文件夹中新建pub_serialport.cpp文件，这是串口控制函数定义文件，写入以下内容，其中  case COM3:  device="/dev/pts/2";  break;根据实际情况填写"/dev/pts/2"

#include "pub_serialport.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             total_len += len;         }         else if(len        if(fd               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/ttyS3";  break;     case COM3:  device="/dev/pts/2";  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; }

四、在/dev_ws/src/cpp_header/src文件夹中新建serial_publisher1_node.cpp文件，用于读取键盘方向键并在话题发布，写入以下内容：

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

class Publisher : public rclcpp::Node { public:     Publisher(std::string name) : Node(name)     {         RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());         subscribe_and_publish_publisher_ = this->create_publisher("subscribe_and_publish", 10);

        //保存标准输入（stdin）的属性，以恢复键盘输入的阻塞行为         tcgetattr(STDIN_FILENO, &initial_settings_);         //将标准输入设置为非阻塞状态         tcgetattr(STDIN_FILENO, &new_settings_);         new_settings_.c_lflag &= ~(ICANON);         new_settings_.c_lflag &= ~(ECHO);         new_settings_.c_cc[VMIN] = 0;         new_settings_.c_cc[VTIME] = 0;         tcsetattr(STDIN_FILENO, TCSANOW, &new_settings_);

        //将标准输入（stdin）设置为O_NONBLOCK         int flags = fcntl(STDIN_FILENO, F_GETFL, 0);         if (flags != -1) {             fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);         }

        //发布数据到话题的定时器         timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&Publisher::timer_callback, this));     }

    ~Publisher()     {         //恢复标准输入的阻塞行为         tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings_);         //将标准输入（stdin）恢复为阻塞状态         int flags = fcntl(STDIN_FILENO, F_GETFL, 0);         if (flags != -1) {             fcntl(STDIN_FILENO, F_SETFL, flags & (~O_NONBLOCK));         }     }

private:     void timer_callback()     {         std_msgs::msg::String message;         std::string data = "0000";         constexpr int bufferSize = 512;         char buffer[bufferSize];         int bytesRead = read(STDIN_FILENO, buffer, bufferSize);

        if (bytesRead > 0) {             RCLCPP_INFO(this->get_logger(), "%d bytes read from stdin", bytesRead);

            for (int i = 0; i < bytesRead; i++) {                 int keyCode = int(buffer[i]);                 if (keyCode == 27) {  //指令以 ESC 开头                     int next1 = i + 1, next2 = i + 2;                     if (next1 < bytesRead && next2 < bytesRead && buffer[next1] == '[') {                         char dir = buffer[next2];                         if (dir == 'A') {  //箭头向上                             arrow_up_pressed_ = true;                         } else if (dir == 'B') {  //箭头向下                             arrow_down_pressed_ = true;                         } else if (dir == 'C') {  //箭头向右                             arrow_right_pressed_ = true;                         } else if (dir == 'D') {  //箭头向左                             arrow_left_pressed_ = true;                         }                      }                 }              }         }

    //    if (arrow_up_pressed_ || arrow_down_pressed_ || arrow_right_pressed_ || arrow_left_pressed_ ) {             if (arrow_up_pressed_) {                 data[0] = '1';             } else {                 data[0] = '0';             }             if (arrow_down_pressed_) {                 data[1] = '1';             } else {                 data[1] = '0';             }             if (arrow_right_pressed_) {                 data[2] = '1';             } else {                 data[2] = '0';             }             if (arrow_left_pressed_) {                 data[3] = '1';             } else {                 data[3] = '0';             }             message.data = data;             RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());             subscribe_and_publish_publisher_->publish(message);             arrow_up_pressed_ = false;             arrow_down_pressed_ = false;             arrow_right_pressed_ = false;             arrow_left_pressed_ = false; //        }     }

    rclcpp::TimerBase::SharedPtr timer_;     rclcpp::Publisher::SharedPtr subscribe_and_publish_publisher_;     termios initial_settings_;     termios new_settings_;     bool arrow_up_pressed_ = false;     bool arrow_down_pressed_ = false;     bool arrow_right_pressed_ = false;     bool arrow_left_pressed_ = false; };

int main(int argc, char **argv) {     rclcpp::init(argc, argv);     auto node = std::make_shared("publisher");     rclcpp::spin(node);     rclcpp::shutdown();     return 0; }

五、在/dev_ws/src/cpp_header/src文件夹中新建serial_subscribe1_node.cpp文件，用于接收话题数据，并从串口以16进制发送最终控制指令，写入以下内容：

#include "pub_serialport.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp"

int fp; unsigned char Msg[128] = { 0 }; const char* constc = nullptr; unsigned char byteArray[10] = { 0xFF,0x32,0x01,0x01,0x00,0x32,0x00,0x01,0x00,0xFC }; // "Hello World!" 的字节数组表示

Serial sp; class TopicSuscribe01 : public rclcpp::Node { public:     TopicSuscribe01(std::string name) : Node(name)     {         RCLCPP_INFO(this->get_logger(),"我是%s，订阅话题为：/subscribe_and_publish.",name.c_str());         command_subscribe_ = this->create_subscription("subscribe_and_publish",10,std::bind(&TopicSuscribe01::command_callback,this,std::placeholders::_1));     } private:     rclcpp::Subscription::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);                   std::string data = msg->data;                                         //     std::string hex_number = "FF3101000031000000FC";

    if (data == "1000") {                          byteArray[0] = 0xFF;         byteArray[1] = 0x31;         byteArray[2] = 0x01;         byteArray[3] = 0x00;         byteArray[4] = 0x00;         byteArray[5] = 0x31;         byteArray[6] = 0x00;         byteArray[7] = 0x00;         byteArray[8] = 0x00;         byteArray[9] = 0xFC;                       } else if (data == "0100") {         // byteArray[10] = { 0xFF,31,00,00,00,31,01,00,00,0xFC };         byteArray[0] = 0xFF;         byteArray[1] = 0x31;         byteArray[2] = 0x00;         byteArray[3] = 0x00;         byteArray[4] = 0x00;         byteArray[5] = 0x31;         byteArray[6] = 0x01;         byteArray[7] = 0x00;         byteArray[8] = 0x00;         byteArray[9] = 0xFC;     } else if (data == "0010") {         // byteArray[10] = { 0xFF,32,01,00,00,31,00,00,00,0xFC };         byteArray[0] = 0xFF;         byteArray[1] = 0x32;         byteArray[2] = 0x01;         byteArray[3] = 0x00;         byteArray[4] = 0x00;         byteArray[5] = 0x31;         byteArray[6] = 0x00;         byteArray[7] = 0x00;         byteArray[8] = 0x00;         byteArray[9] = 0xFC;     } else if (data == "0001") {         // byteArray[10] = { 0xFF,31,01,00,00,32,00,00,00,0xFC };                 byteArray[0] = 0xFF;         byteArray[1] = 0x31;         byteArray[2] = 0x01;         byteArray[3] = 0x00;         byteArray[4] = 0x00;         byteArray[5] = 0x32;         byteArray[6] = 0x00;         byteArray[7] = 0x00;         byteArray[8] = 0x00;         byteArray[9] = 0xFC;     } else {                       byteArray[0] = 0xFF;         byteArray[1] = 0x32;         byteArray[2] = 0x01;         byteArray[3] = 0x01;         byteArray[4] = 0x00;         byteArray[5] = 0x32;         byteArray[6] = 0x00;         byteArray[7] = 0x01;         byteArray[8] = 0x00;         byteArray[9] = 0xFC;     }                          //  std::string str(byteArray, byteArray + sizeof(byteArray) / sizeof(unsigned char)); // 使用 string 构造函数将字节数组转换为字符串       //  str += '\0'; // 添加 NULL 字符       //  constc = str.c_str();                //  int len = strlen(constc);       //  sp.WriteData(fp,constc,len);             // 将数组以十六进制方式发送到串口     for (int i = 0; i < 10; i++) {         char hex[5];         sprintf(hex, "%02X ", byteArray[i]);         write(fp, hex, strlen(hex));     }     write(fp, "\n", 1); // 发送换行符作为结束标记

           }

};

int main(int argc, char **argv) {     /* code */     rclcpp::init(argc, argv);     auto node = std::make_shared("subscribe1");     // 打印一句自我介绍     // int fp;     // Serial sp;     sp.BaudRate(115200);     fp = sp.OpenPort(COM3);    // fp = sp.OpenPort(/dev/pts/3);     if(fp>0){       RCLCPP_INFO(node->get_logger(), "serial success!.");     }   //  sp.WriteData(fp,"1024",20);

         // cout