版权声明：未经博主允许请勿转载 https://blog.csdn.net/qq_39040135/article/details/82319202
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; } }
s.average=sum/N;
for(int clock_two=N-1;clock_two>=0;clock_two--)
variance=variance+pow((double)s.array[clock_two]-(double)s.average,2);
s.mean=variance/N;
if(s.times) {
s.uwb_mean=s.mean;
s.times=false; } }
s.op_value=s.average+(s.croods-s.average)*s.mean/(s.mean+s.uwb_mean); return s; }
int main(int argc, char* argv[]) {
ros::init(argc, argv, "uwb_data_sub");
ros::NodeHandle nh;
pub_uwb_1 = nh.advertise<nav_msgs::Odometry>("uwb/Coords_333",1); //非全局参数引入
pub_uwb_2 = nh.advertise<nav_msgs::Odometry>("uwb/Coords_329",1);
pub_uwb_3 = nh.advertise<nav_msgs::Odometry>("uwb/Coords_360",1);
nav_msgs::Odometry nav_uwb;
string param_uwb_ip=listened_ip;
ros::param::get("~uwb_server_ip",param_uwb_ip); //私有参数引入
// printf("%s\n",param_uwb_ip.c_str());
// ros::Rate loop_(50);
boost::asio::io_service iosev_io;
boost::asio::ip::udp::socket udp_socket(iosev_io);
boost::asio::ip::udp::endpoint local_add(boost::asio::ip::address::from_string(param_uwb_ip),port); //listening to ip_port
udp_socket.open(local_add.protocol());
udp_socket.bind(local_add);
ROS_INFO("connect successfully");
char receive_str[bytes]={0}; // 1bytes
char coords_x[bytes]={0}; // 1bytes
char coords_y[bytes]={0}; // 1bytes
char label_id[bytes]={0}; // 1bytes
int lenx=0;
int leny=0;
int result_coords_x=0,result_coords_y=0;
// 本UWB上位机发布的数据格式形如：("$POS,54000333,600,200") <-（"$***，标签代号，横轴距，纵轴距"）;
const int fir_dall=5,sed_dall=14;
int thd_dall;
int loop_1=N,loop_2=N,loop_3=N;
uwb_data uwb_1_x,uwb_1_y,uwb_2_x,uwb_2_y,uwb_3_x,uwb_3_y;
while(ros::ok()) {
//ros::spinOnce();
boost::asio::ip::udp::endpoint sendpoint;
udp_socket.receive_from(boost::asio::buffer(receive_str,bytes),sendpoint);
int clock_thr=0,pos_first=0,pos_second=0,pos_third=0,sum_dall=0;
while(clock_thr<bytes) {
if(receive_str[clock_thr]==flag)
sum_dall++;
switch(sum_dall) {
case 2:
pos_first=clock_thr;sum_dall++;break;
case 4:
pos_second=clock_thr;sum_dall++;break;
case 6:
pos_third=clock_thr;sum_dall++;break;
default:
break; }
clock_thr++;
if(sum_dall==7)
clock_thr=bytes; }
lenx=pos_second-pos_first-1;leny=pos_third-pos_second-1;thd_dall=sed_dall+lenx+1; // 使用flag:','进行需求信息的分离
strncpy(label_id,receive_str+fir_dall,label_length);
strncpy(coords_x,receive_str+sed_dall,lenx);
strncpy(coords_y,receive_str+thd_dall,leny);
result_coords_x=atoi(coords_x);
result_coords_y=atoi(coords_y); //the test codes
printf("\033[32m");printf("%s\t",label_id);printf("%s\t",coords_x); printf("%s",coords_y); printf("\n");printf("\033[0m");
nav_uwb.header.frame_id=label_id; //name of label
nav_uwb.pose.pose.position.x=result_coords_x; //coords of 'x'
nav_uwb.pose.pose.position.y=result_coords_y; //coords of 'y'
//classification of release //label 54000333 car_position
if(strncmp(label_id,label_1,label_length)==0) {
uwb_1_x.croods=result_coords_x;
uwb_1_y.croods=result_coords_y; //using silding filter
switch(loop_1) {
case 0:
uwb_1_x=sliding_filter(uwb_1_x);
uwb_1_y=sliding_filter(uwb_1_y);
nav_uwb.pose.pose.position.x=uwb_1_x.average; //coords of 'x'
nav_uwb.pose.pose.position.y=uwb_1_y.average; //coords of 'y'
pub_uwb_1.publish(nav_uwb);
break;
default:
uwb_1_x.times=true;uwb_1_y.times=true;
uwb_1_x.array[loop_1-1]=result_coords_x;
uwb_1_y.array[loop_1-1]=result_coords_y;loop_1--;
break; } } //使用滤波器
else if(strncmp(label_id,label_2,label_length)==0) {
uwb_2_x.croods=result_coords_x;
uwb_2_y.croods=result_coords_y;
//pub_uwb_2.publish(nav_uwb);
switch(loop_2) {
case 0:
uwb_2_x=sliding_filter(uwb_2_x);
uwb_2_y=sliding_filter(uwb_2_y);
nav_uwb.pose.pose.position.x=uwb_2_x.average; //coords of 'x'
nav_uwb.pose.pose.position.y=uwb_2_y.average; //coords of 'y'
pub_uwb_2.publish(nav_uwb);break;
default:
uwb_2_x.times=true;uwb_2_y.times=true;
uwb_2_x.array[loop_2-1]=result_coords_x;
uwb_2_y.array[loop_2-1]=result_coords_y;loop_2--;
break; }
}//使用滤波器
else if(strncmp(label_id,label_3,label_length)==0) {
uwb_3_x.croods=result_coords_x;
uwb_3_y.croods=result_coords_y;
pub_uwb_3.publish(nav_uwb);
/*switch(loop_3) {
case 0:
uwb_3_x=sliding_filter(uwb_3_x);
uwb_3_y=sliding_filter(uwb_3_y);
nav_uwb.pose.pose.position.x=uwb_3_x.average; //coords of 'x'
nav_uwb.pose.pose.position.y=uwb_3_y.average; //coords of 'y'
pub_uwb_3.publish(nav_uwb);break;
default:
uwb_3_x.times=true;uwb_3_y.times=true;
uwb_3_x.array[loop_3-1]=result_coords_x;
uwb_3_y.array[loop_3-1]=result_coords_y;loop_3--;
break; }*/ } //不使用滤波器
memset(receive_str,0,bytes);
memset(label_id,0,bytes);
memset(coords_x,0,bytes);
memset(coords_y,0,bytes);
iosev_io.run();
// loop_.sleep();
}
ros::waitForShutdown();
return 0; }
##step.3:在同级目录下的CMakeLists.txt中改用以下代码
CMakeLists.txt文件是编译node时的编译器导引文件，它指定了脚本执行时的依赖项以及建立可执行文件的名称与相关的链接项,此外使用ROS框架你还可以设计自己的nodelete,该方式不使用包含入口函数的脚本,只需要定义类和相应的对象,在大数据挖掘和深度学习即多层神经网络算法上可以广泛应用
- [New Terminal]
gedit CMakeLists.txt
- [CMakeLists.txt]
cmake_minimum_required(VERSION 2.8.3)
project(uwbwork)
find_package(catkin REQUIRED COMPONENTS
roscpp
nav_msgs
)
#generate_messages(DEPENDENCIES sensor_msgs)
catkin_package()
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(uwb_data_sub uwb_data_sub.cpp)
target_link_libraries(uwb_data_sub ${catkin_LIBRARIES})
step.4:修改package.xml文件，注意只修改原文件 “TODO”以下的部分
package.xml可以看作我们编写的文件包脚本执行时具体依赖的项目，如脚本使用了哪种具体的消息格式，此外"run_depend"在ROS新版本中变成了"exe_depend"，如果在编译时有报错提示再行改动吧。
- [New Terminal]
gedit package.xml
- [package.xml]
<?xml version="1.0"?>
<package>
<name>uwbwork</name>
<version>0.0.0</version>
<description>The uwbwork package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="[email protected]">root</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
**<license>TODO</license>**
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>ffmpeg</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>ffmpeg</run_depend>
<run_depend>message_runtime</run_depend>
</package>
##step.5:自定义 uwb_test.launch文件
实际上经过以上步骤并做基本的编译后，脚本已经可以run了，但是ROS框架为我们提供了Launch接口文件的便利，我们可以利用＜ROS＿MASTER＞中的rosparam作为运行参数的动态引入桥梁，使得我们写的驱动文件具有较好的人机交互性，避免经常在底层更新参数的麻烦，例如以下代码中可以看到我们将主机的IP参数灵活引入脚本等等，但要注意要引用的参数类型“/”是全局参数还是非全局参数，所谓全局参数，顾名思义可以全局引用；“~”私有参数类型只可以在当前脚本中引用；另外就是相关参数了，相关参数想要在除当前命名空间外引用，它就要在前面加上我们自己定义的空间名称了
此外该类型的接口中还为我们提供了Group用法,为重度强迫症患者带来了福音(手动滑稽),当然它最大的用处是提高脚本执行效率,为甚麽呢卖个关子,如有兴趣＜ROS_Launch＞更多使用方法详细可参见其他大佬的博客或者我的一些学习笔记（接下来会写一写安排上），这里呢就不赘述了
- [New terminal]
cd
gedit uwb_test.launch
- [uwb_test.launch]
<?xml version="1.0"?>
<launch>
<node name="uwb_data_sub" pkg="uwbwork" type="uwb_data_sub" output="screen" launch-prefix="xterm -e">
<param name="uwb_server_ip" type="string" value="192.168.42.97" />
</node>
</launch
##step.6:cmake&run
- [New terminal]
cd
cd /root/uwb_ws
catkin_make
- [New terminal]
cd
cd source uwb_ws/devel/setup.bash
roslaunch uwb_test.launch
如果自己搭建的拓扑结构正常的话到这里已经可以看到弹出的新窗口中反馈的标签位置数据流，如果需要长期使用室内定位设备做ROS下的开发，建议在~su权限下将"source uwb_ws/devel/setup.bash"添加到.bashrc文件**（直接在终端 gedit .bashrc并添加),好了,文章比较随意，内容如有错误或不足之处还请批评指正!!! (重要事情感叹三遍—手动笑脸)