Note: 本教程假设你已经完成了TF配置教程。所有的代码可以laser_scan_publisher_tutorialpoint_cloud_publisher_tutorial找到。.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

在ROS上发布传感器数据流

Description: 这个教程提供发布两种类型的传感器数据的例子,即 sensor_msgs/LaserScan 消息和sensor_msgs/PointCloud 消息。

Tutorial Level: BEGINNER

在ROS上发布传感器数据流

在ROS上正确地发布从传感器获取的数据对导航功能包集的安全运行很重要。 如果导航功能包集无法从机器人的传感器接收到任何信息,那么它就会盲目行事,最有可能的是发生碰撞。 有许多传感器可用于为导航功能包集提供信息:激光、摄像头、声纳、红外线、碰撞传感器等等。然而,目前导航功能包集只接受使用sensor_msgs/LaserScansensor_msgs/PointCloud消息类型发布的传感器数据。下面的教程将提供典型的设置和使用这两种类型的消息的例子。 相关: TF配置

ROS消息头

消息类型 sensor_msgs/LaserScansensor_msgs/PointCloud跟其他的消息一样,包括tf帧和与时间相关的信息。为了标准化发送这些信息,消息类型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

在ROS上发布LaserScans

LaserScan消息

对于机器人的激光扫描仪,ROS提供了一个特殊的消息类型LaserScan来存储激光信息,它位于包sensor_msgsLaserScan消息方便代码来处理任何激光,只要从扫描仪获取的数据可以格式化为这种类型的消息。我们谈论如何生成和发布这些消息之前,让我们来看看消息本身的规范:

#
# 测量的激光扫描角度,逆时针为正
# 设备坐标帧的0度面向前(沿着X轴方向)
#

Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]

正如所期望的,上面的名字/注释明确表述了消息里的各个字段。为了更具体的说明,我们来写一个简单的激光数据发布器来展示他们是如何工作的。

编写代码发布一个LaserScan消息

在ROS上发布一个LaserScan消息是相当简单的。我们先提供下面的示例代码,然后将代码分解逐行。

   1 #include <ros/ros.h>
   2 #include <sensor_msgs/LaserScan.h>
   3 
   4 int main(int argc, char** argv){
   5   ros::init(argc, argv, "laser_scan_publisher");
   6 
   7   ros::NodeHandle n;
   8   ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
   9 
  10   unsigned int num_readings = 100;
  11   double laser_frequency = 40;
  12   double ranges[num_readings];
  13   double intensities[num_readings];
  14 
  15   int count = 0;
  16   ros::Rate r(1.0);
  17   while(n.ok()){
  18     //generate some fake data for our laser scan
  19     for(unsigned int i = 0; i < num_readings; ++i){
  20       ranges[i] = count;
  21       intensities[i] = 100 + count;
  22     }
  23     ros::Time scan_time = ros::Time::now();
  24 
  25     //populate the LaserScan message
  26     sensor_msgs::LaserScan scan;
  27     scan.header.stamp = scan_time;
  28     scan.header.frame_id = "laser_frame";
  29     scan.angle_min = -1.57;
  30     scan.angle_max = 1.57;
  31     scan.angle_increment = 3.14 / num_readings;
  32     scan.time_increment = (1 / laser_frequency) / (num_readings);
  33     scan.range_min = 0.0;
  34     scan.range_max = 100.0;
  35 
  36     scan.ranges.resize(num_readings);
  37     scan.intensities.resize(num_readings);
  38     for(unsigned int i = 0; i < num_readings; ++i){
  39       scan.ranges[i] = ranges[i];
  40       scan.intensities[i] = intensities[i];
  41     }
  42 
  43     scan_pub.publish(scan);
  44     ++count;
  45     r.sleep();
  46   }
  47 }

现在我们将上面的代码一步一步分解。

   2 #include <sensor_msgs/LaserScan.h>
   3 

这里,我们include我们想要发布的sensor_msgs/LaserScan消息。

   8   ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

段代码创建了一个ros::Publisher用于使用ROS发送 LaserScan 消息。

  10   unsigned int num_readings = 100;
  11   double laser_frequency = 40;
  12   double ranges[num_readings];
  13   double intensities[num_readings];

里我们创建一组存储虚拟数据的变量,用来模拟激光扫描(其中填充所扫描到的障碍物信息)。实际的程序应从他们的激光驱动程序获取这些数据。

  18     //generate some fake data for our laser scan
  19     for(unsigned int i = 0; i < num_readings; ++i){
  20       ranges[i] = count;
  21       intensities[i] = 100 + count;
  22     }
  23     ros::Time scan_time = ros::Time::now();

填充激光数据,填充值每秒加1.

  25     //populate the LaserScan message
  26     sensor_msgs::LaserScan scan;
  27     scan.header.stamp = scan_time;
  28     scan.header.frame_id = "laser_frame";
  29     scan.angle_min = -1.57;
  30     scan.angle_max = 1.57;
  31     scan.angle_increment = 3.14 / num_readings;
  32     scan.time_increment = (1 / laser_frequency) / (num_readings);
  33     scan.range_min = 0.0;
  34     scan.range_max = 100.0;
  35 
  36     scan.ranges.resize(num_readings);
  37     scan.intensities.resize(num_readings);
  38     for(unsigned int i = 0; i < num_readings; ++i){
  39       scan.ranges[i] = ranges[i];
  40       scan.intensities[i] = intensities[i];
  41     }

创建一个 scan_msgs::LaserScan 消息,并使用我们预先生成的数据填充,准备发布它。

  43     scan_pub.publish(scan);

在ROS上发布这个消息。

在ROS上发布点云 PointClouds

点云消息

为了存储与分享世界中的一系列点, ROS 提供了 sensor_msgs/PointCloud 消息。正如前文所述,这个消息支持将三维空间中的点的数组以及任何保存在一个信道中的相关数据。 例如,一条带有"intensity"信道的 PointCloud 可以保持点云数据中每一个点的强度。接下来我们使用ROS发布一个 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

编写代码发布 PointCloud 消息

使用ROS发布 PointCloud 相当简单.接下来,我们给出一个完整的例子,并详细的讨论他的每一个细节.

   1 #include <ros/ros.h>
   2 #include <sensor_msgs/PointCloud.h>
   3 
   4 int main(int argc, char** argv){
   5   ros::init(argc, argv, "point_cloud_publisher");
   6 
   7   ros::NodeHandle n;
   8   ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
   9 
  10   unsigned int num_points = 100;
  11 
  12   int count = 0;
  13   ros::Rate r(1.0);
  14   while(n.ok()){
  15     sensor_msgs::PointCloud cloud;
  16     cloud.header.stamp = ros::Time::now();
  17     cloud.header.frame_id = "sensor_frame";
  18 
  19     cloud.points.resize(num_points);
  20 
  21     //we'll also add an intensity channel to the cloud
  22     cloud.channels.resize(1);
  23     cloud.channels[0].name = "intensities";
  24     cloud.channels[0].values.resize(num_points);
  25 
  26     //generate some fake data for our point cloud
  27     for(unsigned int i = 0; i < num_points; ++i){
  28       cloud.points[i].x = 1 + count;
  29       cloud.points[i].y = 2 + count;
  30       cloud.points[i].z = 3 + count;
  31       cloud.channels[0].values[i] = 100 + count;
  32     }
  33 
  34     cloud_pub.publish(cloud);
  35     ++count;
  36     r.sleep();
  37   }
  38 }

下来,我们一句一句来看.

   2 #include <sensor_msgs/PointCloud.h>
   3 

包含 sensor_msgs/PointCloud 消息头文件.

   8   ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

创建我们用来发布Creat PointCloud 消息的 ros::Publisher .

  15     sensor_msgs::PointCloud cloud;
  16     cloud.header.stamp = ros::Time::now();
  17     cloud.header.frame_id = "sensor_frame";

填充 PointCloud 消息的头:frame 和 timestamp.

  19     cloud.points.resize(num_points);

设置点云的数量.

  21     //we'll also add an intensity channel to the cloud
  22     cloud.channels.resize(1);
  23     cloud.channels[0].name = "intensities";
  24     cloud.channels[0].values.resize(num_points);

增加信道 "intensity" 并设置其大小,使与点云数量相匹配.

  26     //generate some fake data for our point cloud
  27     for(unsigned int i = 0; i < num_points; ++i){
  28       cloud.points[i].x = 1 + count;
  29       cloud.points[i].y = 2 + count;
  30       cloud.points[i].z = 3 + count;
  31       cloud.channels[0].values[i] = 100 + count;
  32     }

使用虚拟数据填充 PointCloud 消息.同时,使用虚拟数据填充 intensity 信道.

  34     cloud_pub.publish(cloud);

使用 ROS 发布 PointCloud 消息.

Wiki: cn/navigation/Tutorials/RobotSetup/Sensors (last edited 2017-03-13 02:55:19 by SawYer)