cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_header
cd ~/dev_ws/src/cpp_header/include/cpp_header
touch minimal_publisher.hpp
代码如下：
#include "cpp_header/minimal_publisher.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/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实现虚拟串口通信
cd ~/dev_ws/src/cpp_header/src
touch minimal_publisher.cpp
代码如下：
#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);
int ReadData(int fd,unsigned char *data,int datalength);
void ClosePort(int fd);
int BaudRate( int baudrate);
};
#endif // PUB_SERIALPORT_HPP
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<rclcpp::Node>("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<<fp<<endl;
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是我自己定义的一个话题，一方面是为了避免与其他人的话题相同而造成数据乱传，另一方面我的发布节点用到的话题也是这个名字。
在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>rclcpp</depend>
<depend>std_msgs</depend>
在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
参照我的另一篇文章：
链接: ROS2实现虚拟串口通信
cat < /dev/pts/4
ros2 run example_topic_rclcpp topic_publisher_01
这里可以参考我的另一篇博客：
链接: 利用RCLCPP实现话题的发布与订阅
ros2 run cpp_header minimal_publisher_node
