< >
Home » ROS与navigation教程 » ROS与navigation教程-发布传感器数据

ROS与navigation教程-发布传感器数据

ROS与navigation教程-发布传感器数据

说明：

  • 介绍如何通过ROS发送两种类型的传感器数据， 分别是sensor_msgs/LaserScan消息和sensor_msgs/PointCloud消息。

发布传感器数据

通过ROS从传感器正确发布数据对于导航包安全运行非常重要。如果导航包没有收到来自机器人传感器的信息，那么它将被盲目地驾驶，并且很有可能会撞到东西。有许多传感器可为导航包提供信息：激光器，相机，声纳，红外线，碰撞传感器等。但是，当前的导航包只接受使用sensor_msgs/LaserScan消息类型或sensor_msgs/PointCloud消息类型。以下教程将提供两种类型消息的典型设置和使用示例。

ROS消息头

像其他类型消息一样， sensor_msgs/LaserScan和sensor_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

发布激光消息

  • LaserScan消息

对于具有激光扫描仪的机器人，ROS在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]

上面的注释就能明白字段表达的意思，为了更直观认识，写一个简单的激光扫描发布器来进行演示。

编写代码发布激光/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>

这里包含sensor_msgs/LaserScan的头文件

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

此代码创建一个ros::Publisher，用于使用ROS发送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();

用每秒增加1的值填充虚拟激光数据

//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发布消息

通过ROS发布点云/PointClouds

  • PointCloud消息

为了存储和共享关于空间上多个点的数据，ROS提供了一个sensor_msgs/PointCloud消息。如下所示，此消息旨在支持三维点阵列以及作为通道存储的任何相关数据。例如，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是非常简单的。我们将在下面详细介绍一个简单的例子，然后详细讨论重要的部分。

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

创建ros::Publisher, 用于发送PointCloud消息

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

填写PointCloud消息的header，分别是发布信息的相关坐标系和时间戳

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

向PointCloud添加一个称为“强度”的频道，并将其大小设置为与云中我们将拥有的点数相匹配。

//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;
}

用一些虚拟数据填充PointCloud消息。另外，用伪数据填充强度信道。

cloud_pub.publish(cloud);

使用ROS 发布PointCloud

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros与navigation教程