ros开发与室内定位（一）

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格式的数据分发，建立了一种动态滤波算法对序列数据流进行平滑拟合，提高定位的精度同时可能对实时性有一定牺牲，数据流可能会有０.５ｓ左右的延迟（滤波器可以关闭或打开代码中已经作了相关标记）

• [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;