ROS下搭建 UWB 下行数据解析驱动
Linux内核版本:ubuntu 15.05 ROS版本:indigo
搭建局域网,向网内的主机提供室内标签实时的定位信息,该Demo可以利用UDP协议将UWB系统上位机的报文转发到网内主机并解析为ROS分布式架构中定制的消息,发布在话题uwb/Coords;nav_msgs/Odometry消息类型,格式如下(部分):
- **std_msgs/Header header **
- ** uint32 seq**
- ** time stamp **
- ** string frame_id **
- **geometry_msgs/PoseWithCovariance pose **
- **geometry_msgs/Pose pose **
- **** geometry_msgs/Point position ****
- ** float64 x **
- ** float64 y**
- ** float64 z**
position.x为定位标签的橫轴坐标,单位/cm; position.y为定位标签的纵轴坐标,单位/cm; position.z为定位标签的垂轴坐标,单位/cm; header.frame_id 为定位标签的id号,如”54000360”
Step.1:创建一个工作空间/uwb_ws,并建立相应的pkg:/uwbwork
- [New Terminal]
mkdir -p /root/uwb_ws/src
source /opt/ros/indigo/setup.bash
catkin_create_pkg uwbwork
cd /root/dronework
catkin_make
step.2:在/uwbwork路径下添加uwb_data_sub.cpp
- [New Terminal]
cd src/uwbwork
gedit uwb_data_sub.cpp ~没有安装 Gedit 插件可以用 $~vi uwb_data_sub.cpp
使用下面的Demo添加进文件并保存,该脚本的功能是利用UDP实时接受网络报文,进行解析、ROS格式的数据分发,建立了一种动态滤波算法对序列数据流进行平滑拟合,提高定位的精度同时可能对实时性有一定牺牲,数据流可能会有0.5s左右的延迟(滤波器可以关闭或打开代码中已经作了相关标记)
- [uwb_data_sub.cpp]
#include <ros/ros.h>
#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <math.h>
#include <stdlib.h>
#include <nav_msgs/Odometry.h>
#include <stdio.h>
#define label_1 "54000333"
#define label_2 "54000329"
#define label_3 "54000360"
#define label_length 8
#define N 5
#define bytes 1024
#define listened_ip "192.168.0.0" //default ip: reset it form "uwb_test.launch"
#define port 8000
#define flag ','
using namespace std;
using namespace boost::asio;
ros::Publisher pub_uwb_1;
ros::Publisher pub_uwb_2;
ros::Publisher pub_uwb_3;
typedef struct {
float average;
float op_value;
double uwb_mean;
double mean;
int array[N];
int croods; bool times; }uwb_data;
//@Function: sliding filter 由于各类外部或内部因素干扰,定位数据序列会产生较为剧烈的时间抖动,这里利用动态均值滤波消除离散数据的毛刺平滑数据,达到更好的定位测量效果
uwb_data sliding_filter(uwb_data s) {
float sum=0;
double variance=0;
if(!s.croods) {
for(int clock_one=N-1;clock_one>=0;clock_one--)
{
sum=sum+(float)s.array[clock_one];
s.array[clock_one]=s.array[clock_one-1];
if(clock_one==1) {
sum=sum+s.croods;
s.array[clock_one-1]=s.croods;