基于ROS发布传感器流数据
1. 基于ROS发布传感器流数据
要想安全操作Navigation Stack,在ROS上从传感器处正确的发布数据是很重要的。如果Navigation Stack不能从传感器处接收消息,那么机器人就相当于盲目运行了,很有可能撞上什么东西。有很多传感器都能用来为Navigation Stack提供信息:雷达、摄像头、声呐、红外线、碰撞传感器等等。但是,闲杂的Navigation Stack只能接收使用sensor_msgs/LaserScan
或sensor_msgs/PointCloud
消息类型发布的传感器数据。以下内容将介绍一个典型的构建以及使用上述两种消息类型的例子。
2. ROS消息头
和其他ROS上发布的消息一样,sensor_msgs/LaserScan 和 sensor_msgs/PointCloud 消息中都包含了tf frame和time相关的信息。为了标准化信息的发送,Header类型都作为这些信息的一个字段包含其中。
下面展示了Header中的三个字段。seq字段对应一个标识符(随着发布者发送的信息自增),stamp字段存储时间信息(例如在雷达扫描时,stamp对应的时间应该是扫描的时刻信息),frame_id存储的是tf 帧信息。
#Standard metadata for higher-level flow data types
#sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.secs: seconds (stamp_secs) since epoch
# * stamp.nsecs: nanoseconds since stamp_secs
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
3. 在ROS上发布LaserScans
3.1 LaserScan消息
对于有雷达的机器人,ROS在sensor_msgs包中提供了一特殊的消息类型LaserScan来保存一次扫描的信息。使用LaserScan消息能很容易地在代码中与任何雷达进行交互,只要雷达扫描器返回的数据能标准化存入该消息中。在我们讨论如何生成以及发布这些消息时,让我们先来看看消息本身的内容:
#
# Laser scans angles are measured counter clockwise, with 0 facing forward
# (along the x-axis) of the device frame
#
Header header
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds]
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]
从这些变量名以及注释我们大概能够了解这些字段的含义,为了理解地更清楚,让我们来写一个简单的雷达扫描发布器来表明具体是如何工作的。
3.2 编写发布LaserScan消息的代码
在ROS上发布LaserScan消息是很直接的,来看一下下面这段简单代码:
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
int main(int argc, char** argv){
ros::init(argc, argv, "laser_scan_publisher");
ros::NodeHandle n;
ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
unsigned int num_readings = 100;
double laser_frequency = 40;
double ranges[num_readings];
double intensities[num_readings];
int count = 0;
ros::Rate r(1.0);
while(n.ok()){
//generate some fake data for our laser scan
for(unsigned int i = 0; i < num_readings; ++i){
ranges[i] = count;
intensities[i] = 100 + count;
}
ros::Time scan_time = ros::Time::now();
//populate the LaserScan message
sensor_msgs::LaserScan scan;
scan.header.stamp = scan_time;
scan.header.frame_id = "laser_frame";
scan.angle_min = -1.57;
scan.angle_max = 1.57;
scan.angle_increment = 3.14 / num_readings;
scan.time_increment = (1 / laser_frequency) / (num_readings);
scan.range_min = 0.0;
scan.range_max = 100.0;
scan.ranges.resize(num_readings);
scan.intensities.resize(num_readings);
for(unsigned int i = 0; i < num_readings; ++i){
scan.ranges[i] = ranges[i];
scan.intensities[i] = intensities[i];
}
scan_pub.publish(scan);
++count;
r.sleep();
}
}
然后我们逐步来分解这段代码。
#include <sensor_msgs/LaserScan.h>
这里我们include了sensor_msgs/LaserScan,因为我们需要发布该消息。
ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
创建一个发布器用于之后发布LaserScan消息。
unsigned int num_readings = 100;
double laser_frequency = 40;
double ranges[num_readings];
double intensities[num_readings];
这里我们创建了一些假的雷达配置信息,在真实的应用中,这些信息可以从雷达驱动器处获得。
//generate some fake data for our laser scan
for(unsigned int i = 0; i < num_readings; ++i){
ranges[i] = count;
intensities[i] = 100 + count;
}
ros::Time scan_time = ros::Time::now();
为假的雷达数据填充一些每秒增加一的值。
//populate the LaserScan message
sensor_msgs::LaserScan scan;
scan.header.stamp = scan_time;
scan.header.frame_id = "laser_frame";
scan.angle_min = -1.57;
scan.angle_max = 1.57;
scan.angle_increment = 3.14 / num_readings;
scan.time_increment = (1 / laser_frequency) / (num_readings);
scan.range_min = 0.0;
scan.range_max = 100.0;
scan.ranges.resize(num_readings);
scan.intensities.resize(num_readings);
for(unsigned int i = 0; i < num_readings; ++i){
scan.ranges[i] = ranges[i];
scan.intensities[i] = intensities[i];
}
创建一个scan_msgs::LaserScan消息,并将我们生成的雷达数据填充到该消息中。
scan_pub.publish(scan);
在ROS上发布该消息。
4. 在ROS上发布PointClouds
4.1 PointCloud消息
对于如何存储以及分发大量点的数据,ROS提供了sensor_msgs/PointCloud消息。如下所示,该消息是用来存储一组3d点数据以及一组通道(每个通道都存储了点数据的相关信息)。例如,一个发布的PointCloud消息中可能包含了一个“intensity”通道,该通道包含了点云中每个点的强度信息。下文将介绍一个发送PointCloud消息的简单例子。
#This message holds a collection of 3d points, plus optional additional information about each point.
#Each Point32 should be interpreted as a 3d point in the frame given in the header
Header header
geometry_msgs/Point32[] points #Array of 3d points
ChannelFloat32[] channels #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point
4.2 编写发布PointCloud消息的代码
在ROS上发布PointCloud也是很容易的,看一下这段代码:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
int main(int argc, char** argv){
ros::init(argc, argv, "point_cloud_publisher");
ros::NodeHandle n;
ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
unsigned int num_points = 100;
int count = 0;
ros::Rate r(1.0);
while(n.ok()){
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";
cloud.points.resize(num_points);
//we'll also add an intensity channel to the cloud
cloud.channels.resize(1);
cloud.channels[0].name = "intensities";
cloud.channels[0].values.resize(num_points);
//generate some fake data for our point cloud
for(unsigned int i = 0; i < num_points; ++i){
cloud.points[i].x = 1 + count;
cloud.points[i].y = 2 + count;
cloud.points[i].z = 3 + count;
cloud.channels[0].values[i] = 100 + count;
}
cloud_pub.publish(cloud);
++count;
r.sleep();
}
}
下面我们逐一解释这段代码。
#include <sensor_msgs/PointCloud.h>
include sensor_msgs/PointCloud消息
ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
创建一个发布PointCloud消息的发布器。
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";
填充消息头。
cloud.points.resize(num_points);
设置点云中点的数量以填充假的数据。
//we'll also add an intensity channel to the cloud
cloud.channels.resize(1);
cloud.channels[0].name = "intensities";
cloud.channels[0].values.resize(num_points);
添加一个名为“intensity”的通道,并将起大小设为与点云数据相同的大小。
//generate some fake data for our point cloud
for(unsigned int i = 0; i < num_points; ++i){
cloud.points[i].x = 1 + count;
cloud.points[i].y = 2 + count;
cloud.points[i].z = 3 + count;
cloud.channels[0].values[i] = 100 + count;
}
填充假数据。
cloud_pub.publish(cloud);
发布该消息。