前段时间在用大疆的UWB,在WINDOWS上按照说明书很顺利的就完成了标签的配置以及坐标角度的通过COM口的读取。
于是准备移植到ubuntu上加入ROS中,但是就遇上了一些不太理解的BUG就放弃了。
然后今天又捡起来研究了一下,最后还是成功把数据读出来了。
串口的数据读取程序总体接口参考这位老哥:https://www.cnblogs.com/TIANHUAHUA/p/7990923.html
按照大疆UWB标签说明书我们发现数据格式是11*2字节
在linux里使用串口调试助手,测试发现数据重复的规律是每38字节一轮循环
于是我就把数据位设了38字节读取。再用low + high * 256
的方式查找数据发现,从第14字节开始才是我们需要的数据内容,从14字节开始的后面6个字节是X,Y,THETA
。
完成数据查询后,我又发现坐标数据会有跳变的BUG,这个就是高八位和低八位相加的问题,修改了坐标X,Y
的计算方法后修复了跳变BUG。
下面贴代码,主要就是遇上了两个Linux专属BUG:一个就是他这个数据格式和说明书对不上,再就是计算坐标不能用char型的直接加,解决了就很简单了,本质上是两个超级low的问题
#include <string>
#include <ros/ros.h>
#include "boost/asio.hpp"
#include "boost/bind.hpp"
#include <iostream>
#include <fstream>
using namespace std;
using namespace boost::asio;
int main(int argc, char** argv) {
ros::init(argc, argv, "usb");
ros::NodeHandle n;
//std::ofstream out("test.txt");
//ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(100);
io_service iosev;
//std::cout << "de11" << std::endl;
serial_port sp(iosev, "/dev/ttyACM0");
//std::cout << "de12" << std::endl;
sp.set_option(serial_port::baud_rate(115200));
sp.set_option(serial_port::flow_control());
sp.set_option(serial_port::parity());
sp.set_option(serial_port::stop_bits());
sp.set_option(serial_port::character_size(8));
std::cout << "de0" << std::endl;
while(n.ok()){
//数据串口一组是38字节(38 * 8位)
char rxData[38] = {0};//38 * 8bit
boost::system::error_code err;
//scanf("%s\n",rxData);
//std::cout << "de1" << std::endl;
sp.read_some(buffer(rxData,38),err);
if (err)
{
ROS_INFO("Serial port read_some Error!");
return -1;
}
//std_msgs::String msg;
//std::stringstream ss;
//测试发现数据从第14个字节开始,前6字节分别是X,Y,Theta
//计算X,Y需要先将低八位转化成无符号8位整形,将高八位转化成有符号16位整形
uint8_t xl = rxData[14];
int16_t xh = rxData[15];
uint8_t yl = rxData[16];
int16_t yh = rxData[17];
int16_t x = xl + xh * 256;
int16_t y = yl + yh * 256;
uint16_t theta = rxData[18] + rxData[19] * 256;//角度可以直接如此计算
//角度范围为0-36000,除100
theta /= 100;
//ROS_INFO("UWB X: %d, %d, Y: %d, %d, Theta: %d", xl, xh, yl, yh, theta);
ROS_INFO("UWB X: %d, Y: %d, Theta: %d", x, y, theta);
/*
for(int i = 0; i < 38; i+=2) {
uint16_t out = rxData[i] + rxData[i + 1] * 256;
std::cout << out << ", ";
}
std::cout << std::endl;
*/
loop_rate.sleep();
}
return 0;
}