头文件包含一些常用的点云处理文件,以及ROS 的传感器类文件
#include"ros/ros.h"
#include<pcl/point_types.h>
#include<pcl/common/transforms.h>
#include<pcl/ModelCoefficients.h>
#include<sensor_msgs/PointCloud2.h>
#include<eigen3/Eigen/Core>
#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<iostream>
初始化ros系统,命名节点名称为“cylinder_node",实例化ros节点管理类Nodehandle,创建发布者向ros系统发布传感器消息,消息类型是PointCloud2点云信息,创建的output对象用于接收生成的圆柱点云并作为载体发布出去。
ros::init(argc,argv,"cylinder_node");
ros::NodeHandle nh;
ros::Publisher cylinder_pub=nh.advertise<sensor_msgs::PointCloud2>("cylinder_output",1);
sensor_msgs::PointCloud2 output;
下面开始利用pcl库生成圆柱点云。
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
创建cloud对象用于储存生成的点云(PS:不用new创建的画会导致核心转储,能编译成功但是运行不起来)
下面就是通过圆柱的几何特征生成点云,通过控制x、y、z的表达式可以改变圆柱轴线方向,改变radius可以更改圆柱半径等等。
int Num=1200;
float inter=2.0*M_PI/Num;
double radius=2.0;
double length=4.0;
double len=length/Num;
for(int j= 0;j<Num;j++)
{
for(int i=0;i<Num;i++)
{
double z=radius*sin(i*inter);
double x=radius*cos(i*inter);
double y=j*len;
pcl::PointXYZ point;
point.x=x;
point.y=y;
point.z=z;
cloud->push_back(point);
}
}
最后通过pcl::toROSMsg函数将pcl的PointXYZ点云信息转变为ros的cloud2点云信息,设置frame_id为“odom2”
pcl::toROSMsg(*cloud,output);
output.header.frame_id="odom2";
ros::Rate loop_rate(1);
while(ros::ok())
{
cylinder_pub.publish(output);
ros::spinOnce();
loop_rate.sleep();
}
打开rviz查看生成的点云(PS:将fixed frame改为odom2)