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
#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消息结构。
ros::Publisher cloud_pub = n.advertise("cloud", 50);
定义一个发布点云消息的发布者。
sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";