在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文件如下：
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_header</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="xxx@todo.todo">xxx</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
修改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(<dependency> 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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/signal.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <limits.h>
#include <string.h>
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 <termios.h>
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/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 <termios.h>
#include <fcntl.h>
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<std_msgs::msg::String>("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<std_msgs::msg::String>::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>("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<std_msgs::msg::String>("subscribe_and_publish",10,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);
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<TopicSuscribe01>("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<<fp<<endl;
RCLCPP_INFO(node->get_logger(), "大家好，我是subscribe1.");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
sp.ClosePort(fp);
return 0;
return 0;
}
六、编译
cd /dev_ws
colcon build
七、测试
先安装虚拟串口模拟器socat
sudo apt-get install socat
输入指令，生成虚拟串口
socat -d -d pty,raw,echo=0 pty,raw,echo=0
启动键盘按键发布节点
另开一个终端，启动话题接收串口发送节点
再开一个终端使用socat打开虚拟串口，查看虚拟串口数据
在打开serial_publisher1_node节点的终端，按动四个方向键，串口数据会发生变化。
5270
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
