首先串口既串行接口,英语翻译为serial port,固简称为sp
在已创建好的ROS工作空间中,既在目录catkin_ws/src下输入指令`
catkin_create_pkg serial_port roscpp rospy serial //创建包及其依赖关系
cd serial_port/src //进入代码区
touch serial_port.cpp //创建代码文件
在serial_port.cpp下
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include <string>
int main(int argc, char** argv)
{
std::string buffer1;
ros::init(argc, argv, "serial_port"); //初始化节点
ros::NodeHandle n; //创建句柄
serial::Serial sp; //创建serial类
serial::Timeout to = serial::Timeout::simpleTimeout(200); //创建timeout
sp.setPort("/dev/ttyUSB0"); //设置要打开的串口名称
sp.setBaudrate(9600); //设置串口通信的波特率
sp.setTimeout(to); //串口设置timeout
try
sp.open(); //打开串口
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
if(sp.isOpen()) //判断串口是否打开成功
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
else
return -1;
ros::Rate loop_rate(500);//循环频率500HZ
while(ros::ok())
{
size_t n = sp.available();//获取串口缓存的数据字节
if(n!=0)
{
uint8_t buffer2[1024];
sp.read(buffer2,n); //buffer2读取串口缓存的数据
for(int i=0; i<n; i++)
{
uint8_t test;
test=buffer2[i];
if(test=='\n')//检测到换行符
{
buffer1+=test;
std::cout << buffer1;//终端输出
sp.write(buffer1); //串口输出
buffer1=""; //清0
}
else
buffer1+=test;
}
}
loop_rate.sleep();
}
sp.close(); //关闭串口
return 0;
}
效果图
到现在已经完成一个小目标了,那下一个目标是把这些代码作为一个订阅者,当有发布者订阅后,向其发送信息,可使串口输出该发布者发送的信息。
其中订阅者名字命名为serial_port。
注意回调函数是用ros::spinOnce();而不是使用ros::spin();
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include <string>
#include "std_msgs/String.h"
serial::Serial sp; //创建serial类
void send_msg(const std_msgs::String::ConstPtr& msg)
{
sp.write(msg->data); //串口输出发布者发布的信息
}
int main(int argc, char** argv)
{
std::string buffer1;
ros::init(argc, argv, "serial_port"); //初始化节点
ros::NodeHandle n; //创建句柄
ros::Subscriber sub = n.subscribe("SERIAL_PORT",1000,send_msg); //创建订阅者
serial::Timeout to = serial::Timeout::simpleTimeout(100); //创建timeout
sp.setPort("/dev/ttyUSB0"); //设置要打开的串口名称
sp.setBaudrate(9600); //设置串口通信的波特率
sp.setTimeout(to); //串口设置timeout
try
{
sp.open(); //打开串口
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
if(sp.isOpen()) //判断串口是否打开成功
{
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
}
else
{
return -1;
}
ros::Rate loop_rate(500);
while(ros::ok())
{
size_t n = sp.available(); //获取接收到的字节数
if(n!=0)
{
uint8_t buffer2[1024];
sp.read(buffer2,n); //buffer2被n个字节赋值
for(int i=0; i<n; i++)
{
uint8_t test;
test=buffer2[i];
if(test=='\n') //判断是否为换行符
{
buffer1+=test;
std::cout << buffer1; //界面返回
sp.write(buffer1); //串口返回
buffer1=""; //清除
}
else
buffer1+=test;
}
}
loop_rate.sleep(); //睡眠函数
ros::spinOnce(); //回调函数
}
//关闭串口
sp.close();
return 0;
}