来源是使用UWB开发板时,希望能够ubuntu中通过ros话题的方式获得UWB测距信息。开发板通过串口接收AT指令,来进行控制,发送也通过串口。给大家一个参考。
使用的是安信可的NodeMCU-BU01开发板,修改了固件源码,提升频率为10hz,并设置一个开机默认为标签,一个开机默认为基站,只需要使用AT指令发送开始测距即可。
#include <ros/ros.h>
#include <serial/serial.h> //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float32.h>
#include <string>
#include <cstdlib>
int main (int argc, char** argv)
{
ros::init(argc, argv, "uwb_serial");
ros::NodeHandle nh;
ros::Publisher uwb_publisher = nh.advertise<std_msgs::Float32>("/uwb", 5);
serial::Serial ser; //声明串口对象
try
{
//设置串口属性,并打开串口
ser.setPort("/dev/ttyUSB0");
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())
{
ser.write("AT+switchdis=1\r\n");//发送控制指令
ROS_INFO_STREAM("Serial Port initialized");
}
else
{
return -1;
}
ros::Rate loop_rate(10);
while(ros::ok())
{
std::string receive;
size_t n = ser.available();
if(n != 0)
{
uint8_t tmpdata[30];
receive = ser.read(n);
if(receive.size() != 0 ) {
ROS_INFO_STREAM(receive);
std::string dis;
//接收的string中包含了返回状态“OK”和换行符,需要去掉
for (char c : receive) {
if (c != '\n' && c != 'O' && c != 'K')
dis += c;
}
//如果有距离信息,我就把string转为double,然后发布。
if (dis.size() != 0) {
std_msgs::Float32 msg;
msg.data = std::stod(dis);
uwb_publisher.publish(msg);
}
}
}
//处理ROS的信息,比如订阅消息,并调用回调函数
ros::spinOnce();
loop_rate.sleep();
}
//关闭程序时,将开发板测距功能关闭。
ser.write("AT+switchdis=0\r\n");
ser.close();
return 0;
}
参考:在他基础上进行了修改和改进。