ROS探索总结-20.发布导航需要的传感器信息

发布导航需要的传感器信息

在导航过程中，传感器的信息至关重要，这些传感器可以是激光雷达、摄像机、声纳、红外线、碰撞开关，但是归根结底，导航功能包要求机器人必须发布sensor_msgs/LaserScan或sensor_msgs/PointCloud格式的传感器信息，本篇将详细介绍如何使用代码发布所需要的消息。

1、ROS的消息头信息

无论是 sensor_msgs/LaserScan，还是sensor_msgs/PointCloud ，都和ROS中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

以上是标准头信息的主要部分。seq是消息的顺序标识，不需要手动设置，发布节点在发布消息时，会自动累加。stamp 是消息中与数据相关联的时间戳，例如激光数据中，时间戳对应激光数据的采集时间点。frame_id 是消息中与数据相关联的参考系id，例如在在激光数据中，frame_id对应激光数据采集的参考系。

2、如何发布激光扫描消息

2.1、激光消息的结构

针对激光雷达，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]

备注中已经详细介绍了每个参数的意义。

2.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>

首先我们需要先包含Laserscan的数据结构。

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

创建一个发布者，以便于后边发布针对scan主题的Laserscan消息。

  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_msgs::LaserScan数据类型的变量scan，把我们之前伪造的数据填入格式化的消息结构中。

   scan_pub.publish(scan);

数据填充完毕后，通过前边定义的发布者，将数据发布。

3、如何发布点云数据

3.1、点云消息的结构

为存储和发布点云消息，ROS定义了sensor_msgs/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

如上所示，点云消息的结构支持存储三维环境的点阵列，而且channels参数中，可以设置这些点云相关的数据，例如可以设置一个强度通道，存储每个点的数据强度，还可以设置一个系数通道，存储每个点的反射系数，等等。

3.2、通过代码发布点云数据

ROS发布点云数据同样简洁：

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

首先也是要包含sensor_msgs/PointCloud消息结构。

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

定义一个发布点云消息的发布者。

    sensor_msgs::PointCloud cloud;

    cloud.header.stamp = ros::Time::now();

    cloud.header.frame_id = "sensor_frame";

为点云消息填充头信息，包括时间戳和相关的参考系id。

    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);

最后，发布点云数据。

参考连接：

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors