环境: Ubuntu16.04 ROS kinetic C++ 1.新建ROS工作空间
mkdir -p PointCloundShow_ws/src
cd PointCloundShow_ws/src
catkin_init_workspace
cd ..
catkin_make
将工作空间下的setup.bash文件路径添加到Home路径下的.bashrc文件中:
这样就不用每次都source一下了. 2.创建功能包
cd src
catkin_create_pkg pointcloundshow pcl_ros roscpp rospy sensor_msgs std_msgs
在功能包的src文件夹下新建cpp文件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;
}
然后将下面的编译规则写到功能包的CMakeLists.txt文件中
find_package(PCL REQUIRED)
include_directories(include${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_executable(pcl_create src/pcl_create.cpp)
target_link_libraries(pcl_create ${catkin_LIBRARIES} ${PCL_LIBRARIES})
回到工作空间路径下输入catkin_make进行编译 依次打开新的终端分别运行下面的指令:
roscore
rosrun pointcloundshow pcl_create
rviz
打开rviz 在rviz中增加PointCloud2d topic 选 /pcl_output fixed Frame 输入odom 如图
手动创建正方体八个角点,理解点云的空间意义: 代码:
#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 = 8;
cloud.height = 1; //此处也可以为cloud.width = 4; cloud.height = 2;
cloud.points.resize(cloud.width * cloud.height);
cloud.points[0].x = 1;
cloud.points[0].y = 1;
cloud.points[0].z = 0;
cloud.points[1].x = -1;
cloud.points[1].y = 1;
cloud.points[1].z = 0;
cloud.points[2].x = 1;
cloud.points[2].y = -1;
cloud.points[2].z = 0;
cloud.points[3].x = -1;
cloud.points[3].y = -1;
cloud.points[3].z = 0;
cloud.points[4].x = 1;
cloud.points[4].y = 1;
cloud.points[4].z = 2;
cloud.points[5].x = -1;
cloud.points[5].y = 1;
cloud.points[5].z = 2;
cloud.points[6].x = 1;
cloud.points[6].y = -1;
cloud.points[6].z = 2;
cloud.points[7].x = -1;
cloud.points[7].y = -1;
cloud.points[7].z = 2;
//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;
}
利用循环嵌套,创建一个正方体
#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)
{
float table[]={-2,-1.5,-1,-0.5,0.5,1,1.5,2};
int point_num;
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 = 512;
cloud.height = 1;
cloud.points.resize(cloud.width * cloud.height);
for(int a=0;a<(sizeof(table)/sizeof(int));++a)
{
float width = table[a];
for(int i=0;i<8;++i)
{
float length = table[i];
for(int c=0;c<8;++c)
{
point_num = a*64+i*8+c;
cloud.points[point_num].x = width;
cloud.points[point_num].y = length;
cloud.points[point_num].z = table[c];
}
}
}
//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;
}
创建一个圆柱体:
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <cmath>
#define PI 3.1415926535
main (int argc, char **argv)
{
long point_num;
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 = 360;
cloud.height = 21;
cloud.points.resize(cloud.width * cloud.height);
for(int z=0;z<20;++z)
{
for(int loop=0;loop<360;++loop)
{
point_num=z*360 + loop;
float hudu = 180 *loop/PI;
cloud.points[point_num].x = cos(hudu);
cloud.points[point_num].y = sin(hudu);
cloud.points[point_num].z = 0.3*(z+1.0f);//或者为0.3*static_cast<float>(z);目地在于int转换为float
}
}
//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;
}