ROS Navigation-----发布传感器数据

    本文中例子将介绍如何发布2种类型的传感器数据：sensor_msgs/LaserScan messages和sensor_msgs/PointCloud messages。

1 发布传感器数据流

    正确发布传感器数据对导航功能包集的安全运行非常重要。 如果导航功能包集从机器人的传感器接收不到任何信息，那么它会盲目行事，非常可能的会撞到东西。 有许多传感器，可用于为导航功能包集提供信息：激光、摄像头、声纳、红外线、碰撞传感器等等。然而，目前导航功能包集只接受使用sensor_msgs/LaserScan或sensor_msgs/PointCloud消息类型的传感器数据。下面将会提供典型的设置和使用这两种类型消息的例子。

2 ROS消息头

    消息类型 sensor_msgs/LaserScan和 sensor_msgs/PointCloud跟其他的消息一样，包括tf帧和与时间相关的信息。为了标准化发送这些信息，所有此类消息中都有一个Header类型字段。 该Header类型字段由3个子字段组成。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 发布LaserScans

3.1 LaserScan消息

对于使用激光扫描仪的机器人,ROS提供了一个特殊的消息类型 LaserScan来存储 激光信息，它位于包 sensor_msgs。

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 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "laser_scan_publisher");

  ros::NodeHandle n;
  ros::Publisher scan_pub = n.advertise("scan", 50);

  //A real application would pull the following data from their laser driver
  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，
    //Populate the dummy laser data with values that increase by one every second.
    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，
    //Create a scan_msgs::LaserScan message and fill it with the data
    //that we've generated in preparation to send it over the wire.
    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];
    }

    //Publish the message over ROS
    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

4 发布PointClouds

4.1 点云消息

为了存储与共享用来表示物理世界中大量的点的数据, ROS 提供了sensor_msgs/PointCloud 消息。
如下所示，该消息中包含了三维点的数组以及与这些点相关的数据被保存为channel的channel数组。
例如，一个点云可以用一个"intensity"channel来发送，那么对应该channel中装的是该云中对应于每个点的intensity信息。

#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 编写代码发布点云信息

#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");

  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise("cloud", 50);
 
  //the number of points in the point cloud
  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();
  }
}

注意：以上内容翻译自ROS WIKI