< >
Home » ROS与C++入门教程 » ROS与C++入门教程-navigation-实现发布传感器话题

ROS与C++入门教程-navigation-实现发布传感器话题

ROS与C++入门教程-navigation-实现发布传感器话题

说明：

  • 介绍如何通过ROS发送两种类型的传感器流（sensor_msgs/LaserScan 消息和sensor_msgs/PointCloud消息）的示例。

通过ROS发布传感器流

  • 通过ROS从传感器正确发布数据对于导航包安全操作非常重要。
  • 如果导航包没有接收到来自机器人的传感器的信息，那么它将是盲目的，并且最有可能撞到物体。
  • 有许多传感器可用于向导航包提供信息：激光，照相机，声纳，红外，碰撞传感器等。
  • 然而，当前导航包仅接受使用sensor_msgs/LaserScan消息类型或 sensor_msgs/PointCloud消息类型。
  • 以下教程将提供两种类型的消息的典型设置和使用示例。

ROS消息头

  • 与通过ROS发送的许多其他消息一样，sensor_msgs/LaserScan和sensor_msgs/PointCloud消息类型包含tf坐标系和时间相关的信息。
  • 为了标准化该信息的发送方式，Header消息类型用作所有这些消息中的字段。
  • Header类型中的三个字段如下所示。
#Standard metadata for higher-level flow data types更高级别流数据类型的标准元数据
#sequence ID: consecutively increasing ID sequence ID：连续增加ID 
uint32 seq
#Two-integer timestamp that is expressed as:两整数时间戳表示为：
# * stamp.secs: seconds (stamp_secs) since epoch* stamp.secs：从epoch开始的秒（stamp_secs）
# * stamp.nsecs: nanoseconds since stamp_secs stamp.nsecs：由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字段存储TF应该与在一个消息的数据相关联的坐标系信息。在激光扫描的情况下，这将被设置为进行扫描的坐标系。

通过ROS发布LaserScans

（1）LaserScan消息

  • 对于使用激光扫描仪的机器人，ROS在sensor_msgs包中提供了一种特殊的消息类型LaserScan，用于保存有关给定扫描的信息。
  • LaserScan消息使代码可以轻松地与几乎任何激光器一起工作，只要从扫描仪返回的数据可以格式化以适应消息。
  • 在我们讨论如何生成和发布这些消息之前，让我们来看看消息规范本身：
#
# Laser scans angles are measured counter clockwise, with 0 facing forward
# 激光扫描角度逆时针测量，0面朝前
# (along the x-axis) of the device frame
# （沿x轴）

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）编写代码以发布LaserScan消息

  • 发布一个激光扫描过ROS消息是相当简单的。
  • 我们将首先提供下面的示例代码，然后逐行解释
  • 代码如下：
#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();

  • 解释： 使用每秒增加一个的值填充虚拟激光数据。

  • 代码：

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

（1）PointCloud消息

  • 为了存储和共享关于世界上的多个点的数据，ROS提供了sensor_msgs/PointCloud消息。
  • 如下所示，该消息意在支持三维点的数组以及作为通道存储的任何相关联的数据。
#This message holds a collection of 3d points, plus optional additional information about each point.
#此消息包含3d点的集合，以及有关每个点的可选附加信息
#Each Point32 should be interpreted as a 3d point in the frame given in the header
#Each Point32应该解释为标题中给出的框架中的3d点

Header header
#Array of 3d points 3D点的数组
geometry_msgs/Point32[] points   
#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
#每个通道应具有与点数组相同数量的元素，每个通道中的数据应与每个点对应1：1
ChannelFloat32[] channels
  • 例如，PointCloud可以通过“强度”通道在线上发送，该“强度”通道保存关于云中每个点的强度值的信息。
  • 我们将在下一节中探讨使用ROS 发送PointCloud的示例。

（2）编写代码以发布PointCloud消息

  • 发布一个点云与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);
  • 解释：创建ros::Publisher实例，我们将使用它通过线路发送PointCloud消息。
  • 代码:
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";
  • 解释：填写我们将发送的PointCloud消息的标题以及相关的坐标系和时间戳信息。
  • 代码：
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添加一个名为“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);
  • 解释：通过ROS发布点云消息。

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

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


标签: ROS与C++入门教程