创建点云 - 爱码网

目的

创建仅由随机的点组成的PCL点云，并且这个PCL点云最终会通过一个/pcl_output的主题发布出去。

代码

源文件pcl_create.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    // Fill in the cloud data
    cloud.width  = 100;
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);

    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }

    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

解析

1.包含适当的头文件

在本例中，包含PCL指定的头文件，标准的ROS头文件，声明PointCloud2消息的头文件：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

初始化节点后，创建一个ROS发布者PointCloud2并进行广播。

ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
pcl::PointCloud<pcl::PointXYZ> cloud;
sensor_msgs::PointCloud2 output;

2.产生带有相关数据的点云

在本例中，创建一个大小为100的点云：

// Fill in the cloud data
cloud.width  = 100;
cloud.height = 1;
cloud.points.resize(cloud.width * cloud.height);

分配好空间，设置好合适的字段后，点云将填满0和1024之间的随机数：

 for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }

3.将PCL点云类型转换为ROS消息类型并发布出去

使用toROSMsg函数完成转换，为了能够有一个恒定的信息源，会以1Hz的频率周期性发布PointCloud2消息。

//Convert the cloud to ROS message
pcl::toROSMsg(cloud, output);
output.header.frame_id = "odom";

ros::Rate loop_rate(1);
while (ros::ok())
{
	pcl_pub.publish(output);
	ros::spinOnce();
	loop_rate.sleep();
}

output.header.frame_id = "odom"是为了能够在rviz中可视化PointCloud2消息。

运行

1.首先打开一个终端，运行命令：

$ roscore

2.打开另一个终端，运行：

$ rosrun chapter6_tutorials pcl_create

3.可视化点云，运行：

$ rosrun rviz rviz

rviz加载好后，点击Add并添加pcl_output主题来增加一个PointCloud2的对象。
在Global Option中将odom设置为固定的坐标系。
效果图如下：创建点云