ROS串口读取UWB定位数据,以话题的形式发布
#include <ros/ros.h>
#include <serial/serial.h>
#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];
int main (int argc, char** argv)
{
ros::init(argc, argv, "uwb_serial");
ros::NodeHandle nh;
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);
while(ros::ok())
{
size_t n = ser.available();
if(n != 0)
{
uint8_t tmpdata[30];
ser.read(tmpdata,n);
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;
uwb_y = atof(data_string[1])/100.0;
aimibot::Uwb uwb_data;
uwb_data.uwb_x = uwb_x;
uwb_data.uwb_y = uwb_y;
uwb_publisher.publish(uwb_data);
ros::spinOnce();
loop_rate.sleep();
}
ser.close();
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)