譯：基於ROS發佈傳感器流數據

基於ROS發佈傳感器流數據

1. 基於ROS發佈傳感器流數據

---

要想安全操作Navigation Stack，在ROS上從傳感器處正確的發佈數據是很重要的。如果Navigation Stack不能從傳感器處接收消息，那麼機器人就相當於盲目運行了，很有可能撞上什麼東西。有很多傳感器都能用來爲Navigation Stack提供信息：雷達、攝像頭、聲吶、紅外線、碰撞傳感器等等。但是，閒雜的Navigation Stack只能接收使用sensor_msgs/LaserScan或sensor_msgs/PointCloud消息類型發佈的傳感器數據。以下內容將介紹一個典型的構建以及使用上述兩種消息類型的例子。

2. ROS消息頭

---

和其他ROS上發佈的消息一樣，sensor_msgs/LaserScan 和 sensor_msgs/PointCloud 消息中都包含了tf frame和time相關的信息。爲了標準化信息的發送，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

3. 在ROS上發佈LaserScans

---

3.1 LaserScan消息

對於有雷達的機器人，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]

從這些變量名以及註釋我們大概能夠了解這些字段的含義，爲了理解地更清楚，讓我們來寫一個簡單的雷達掃描發佈器來表明具體是如何工作的。

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

這裏我們include了sensor_msgs/LaserScan，因爲我們需要發佈該消息。

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

創建一個發佈器用於之後發佈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上發佈該消息。

4. 在ROS上發佈PointClouds

---

4.1 PointCloud消息

對於如何存儲以及分發大量點的數據，ROS提供了sensor_msgs/PointCloud消息。如下所示，該消息是用來存儲一組3d點數據以及一組通道（每個通道都存儲了點數據的相關信息）。例如，一個發佈的PointCloud消息中可能包含了一個“intensity”通道，該通道包含了點雲中每個點的強度信息。下文將介紹一個發送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

4.2 編寫發佈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>

include sensor_msgs/PointCloud消息

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

創建一個發佈PointCloud消息的發佈器。

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

添加一個名爲“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);

發佈該消息。

參考資料

1. navigationTutorialsRobotSetupSensors