如何使用代碼發(fā)布導(dǎo)航需要的傳感器信息

在導(dǎo)航過程中，傳感器的信息至關(guān)重要，這些傳感器可以是激光雷達(dá)、攝像機(jī)、聲納、紅外線、碰撞開關(guān)，但是歸根結(jié)底，導(dǎo)航功能包要求機(jī)器人必須發(fā)布sensor_msgs/LaserScan或sensor_msgs/PointCloud格式的傳感器信息，本篇將詳細(xì)介紹如何使用代碼發(fā)布所需要的消息。

1、ROS的消息頭信息
無論是 sensor_msgs/LaserScan，還是sensor_msgs/PointCloud ，都和ROS中tf幀信息等時間相關(guān)的消息一樣，帶標(biāo)準(zhǔn)格式的頭信息。
#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

以上是標(biāo)準(zhǔn)頭信息的主要部分。seq是消息的順序標(biāo)識，不需要手動設(shè)置，發(fā)布節(jié)點(diǎn)在發(fā)布消息時，會自動累加。stamp 是消息中與數(shù)據(jù)相關(guān)聯(lián)的時間戳，例如激光數(shù)據(jù)中，時間戳對應(yīng)激光數(shù)據(jù)的采集時間點(diǎn)。frame_id 是消息中與數(shù)據(jù)相關(guān)聯(lián)的參考系id，例如在在激光數(shù)據(jù)中，frame_id對應(yīng)激光數(shù)據(jù)采集的參考系。

2、如何發(fā)布激光掃描消息
2.1、激光消息的結(jié)構(gòu)

針對激光雷達(dá)，ROS在sensor_msgs 包中定義了專用了數(shù)據(jù)結(jié)構(gòu)來存儲激光消息的相關(guān)信息，成為LaserScan。LaserScan消息的格式化定義，為虛擬的激光雷達(dá)數(shù)據(jù)采集提供了方便，在我們討論如何使用他之前，來看看該消息的結(jié)構(gòu)是什么樣的：
#
# 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]

備注中已經(jīng)詳細(xì)介紹了每個參數(shù)的意義。

2.2、通過代碼發(fā)布LaserScan消息

使用ROS發(fā)布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);

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

首先我們需要先包含Laserscan的數(shù)據(jù)結(jié)構(gòu)。
ros::Publisher scan_pub = n.advertise("scan", 50);

創(chuàng)建一個發(fā)布者，以便于后邊發(fā)布針對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();

這里的例程中我們虛擬一些激光雷達(dá)的數(shù)據(jù)，如果使用真是的激光雷達(dá)，這部分?jǐn)?shù)據(jù)需要從驅(qū)動中獲取。
//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];
}

創(chuàng)建scan_msgs::LaserScan數(shù)據(jù)類型的變量scan，把我們之前偽造的數(shù)據(jù)填入格式化的消息結(jié)構(gòu)中。
scan_pub.publish(scan);

數(shù)據(jù)填充完畢后，通過前邊定義的發(fā)布者，將數(shù)據(jù)發(fā)布。

3、如何發(fā)布點(diǎn)云數(shù)據(jù)
3.1、點(diǎn)云消息的結(jié)構(gòu)
為存儲和發(fā)布點(diǎn)云消息，ROS定義了sensor_msgs/PointCloud消息結(jié)構(gòu)：

#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

如上所示，點(diǎn)云消息的結(jié)構(gòu)支持存儲三維環(huán)境的點(diǎn)陣列，而且channels參數(shù)中，可以設(shè)置這些點(diǎn)云相關(guān)的數(shù)據(jù)，例如可以設(shè)置一個強(qiáng)度通道，存儲每個點(diǎn)的數(shù)據(jù)強(qiáng)度，還可以設(shè)置一個系數(shù)通道，存儲每個點(diǎn)的反射系數(shù)，等等。

3.2、通過代碼發(fā)布點(diǎn)云數(shù)據(jù)
ROS發(fā)布點(diǎn)云數(shù)據(jù)同樣簡潔：
#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);

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消息結(jié)構(gòu)。
ros::Publisher cloud_pub = n.advertise("cloud", 50);

定義一個發(fā)布點(diǎn)云消息的發(fā)布者。
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";

為點(diǎn)云消息填充頭信息，包括時間戳和相關(guān)的參考系id。
cloud.points.resize(num_points);

設(shè)置存儲點(diǎn)云數(shù)據(jù)的空間大小。
//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);

設(shè)置一個名為“intensity“的強(qiáng)度通道，并且設(shè)置存儲每個點(diǎn)強(qiáng)度信息的空間大小。
//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;
}

將我們偽造的數(shù)據(jù)填充到點(diǎn)云消息結(jié)構(gòu)當(dāng)中。
cloud_pub.publish(cloud);

最后，發(fā)布點(diǎn)云數(shù)據(jù)。