ROS创建点云数据并在rviz中显示

àì夳堔傛蜴生んèń 2022-03-16 16:22 1959阅读 0赞

实验环境ROS(kinetic)  
**1.新建工程**

mkdir -p catkin_ws/src
    cd catkin_ws/src
    catkin_create_pkg chapter6_tutorials pcl_ros roscpp rospy sensor_msgs std_msgs 
    cd ..
    catkin_make

**2.编辑主函数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; 
    }

**3.编辑CMakeLists.txt**  
编辑/chapter6\_tutorials/src/chapter6\_tutorials路径下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})

**4.编译和在rviz中显示**  
**编译**

cd ~/catkin_ws | catkin_make

**刷新环境**

source ~/catkin_ws/devel/setup.bash

**运行**

roscore
    rosrun chapter6_tutorials pcl_create

**打开rviz**  
在rviz中增加PointCloud2d  
topic 选 /pcl\_output  
fixed Frame 输入odom  
如图  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 1]**手动创建正方体八个角点，理解点云的空间意义：**  
代码：

#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; 
    }

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 2]利用循环嵌套，创建一个正方体

#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[8]={ -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<8;++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; 
    }

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 3]  
**创建一个圆柱体：**

#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; 
    }

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 4]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20190228113510312.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 1]: /images/20220316/06274e4e579d4a92aee7a10baed98438.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 2]: /images/20220316/b5cf128d952d4d4e9f0d451da9c717f7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 3]: /images/20220316/1ce318c7d87042e28adac9bda720a82b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70 4]: /images/20220316/bcaeb3f4290a411a82ffc6ca49f23e5d.png