ROS下UWB串口数据读取，并以话题的形式发布

这个也是用的serial功能包，看来基本都是用的serial功能包而不是rosserial

转载自：https://blog.csdn.net/qq_43511200/article/details/110298518

ROS下UWB串口数据读取，并以话题的形式发布

Xuan T 2020-11-29 00:51:26 189 收藏 3

分类专栏： UWB 文章标签： 串口通信

版权

ROS串口读取UWB定位数据，以话题的形式发布

#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <aimibot/Uwb.h> 
#include <cstdlib>

serial::Serial ser; //声明串口对象 
float uwb_x;
float uwb_y;

char data_string[2][7];//字符串形式存串口读取的x,y 在用函数转为浮点型数据
int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "uwb_serial"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
    //发布UWB话题
    ros::Publisher uwb_publisher = nh.advertise<aimibot::Uwb>("/uwb/data", 10); 
    
    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyS1"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    }
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }

    //指定循环的频率 
    ros::Rate loop_rate(100); 

    //处理从串口来的uwb数据
    while(ros::ok()) 
    { 
		//串口缓存字符数
    	size_t n = ser.available();
        if(n != 0)
        { 
            uint8_t tmpdata[30];
            ser.read(tmpdata,n);//tmpdata 参数缓存 data_size 数据长度
            //读取uwb的x,y
            for (int i = 0,j =0; i< n; i++)
            {
				j=0;
				if(tmpdata[i]=='x')
				{
					i++;//去掉首字母
					while(tmpdata[i]!=',')
					{
						data_string[0][j]=tmpdata[i];
						i++;
						j++;
					}
				}
				if(tmpdata[i]=='y')
				{
					i++;//去掉首字母
					while(tmpdata[i]!=',')
					{
						data_string[1][j]=tmpdata[i];
						i++;
						j++;
					}
				}
			}
        }
        //把字符串转化为浮点数
        uwb_x = atof(data_string[0])/100.0;//x
        uwb_y = atof(data_string[1])/100.0;//y

        aimibot::Uwb uwb_data;
        uwb_data.uwb_x = uwb_x;
        uwb_data.uwb_y = uwb_y;
        uwb_publisher.publish(uwb_data);

        //处理ROS的信息，比如订阅消息,并调用回调函数 
        ros::spinOnce(); 
        loop_rate.sleep(); 
    }
    //关闭串口
    ser.close();
    return 0;
}