1.pcl_load.cpp

1. #include<ros/ros.h>
2. #include<pcl/point_cloud.h>
3. #include<pcl_conversions/pcl_conversions.h>
4. #include<sensor_msgs/PointCloud2.h>
5. #include<pcl/io/pcd_io.h>//which contains the required definitions to load and store point clouds to PCD and other file formats.  
6. main (int argc, char **argv)  
7. {  
8.   ros::init (argc, argv, "UandBdetect");  
9.   ros::NodeHandle nh;  
10.   ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);  
11.   pcl::PointCloud<pcl::PointXYZ> cloud;  
12.   sensor_msgs::PointCloud2 output;  
13.   pcl::io::loadPCDFile ("/home/shuning/catkin_ws/src/imgpcl/data/test_pcd.pcd", cloud);  
14.   //Convert the cloud to ROS message  
15.   pcl::toROSMsg(cloud, output);  
16.   output.header.frame_id = "odom";//this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer  
17.   ros::Rate loop_rate(1);  
18.   while (ros::ok())  
19.   {  
20.     pcl_pub.publish(output);  
21.     ros::spinOnce();  
22.     loop_rate.sleep();  
23.   }  
24.   return 0;  
25. }  

1. #include<ros/ros.h>
2. #include<pcl/point_cloud.h>
3. #include<pcl_conversions/pcl_conversions.h>
4. #include<sensor_msgs/PointCloud2.h>
5. #include<pcl/io/pcd_io.h>
6. void cloudCB(const sensor_msgs::PointCloud2 &input)  
7. {  
8.   pcl::PointCloud<pcl::PointXYZ> cloud;  
9.   pcl::fromROSMsg(input, cloud);//從ROS型別訊息轉為PCL型別訊息
10.   pcl::io::savePCDFileASCII ("/home/shuning/catkin_ws/src/imgpcl/data/write_pcd_test.pcd", cloud);//儲存pcd
11. }  
12. main (int argc, char **argv)  
13. {  
14.   ros::init (argc, argv, "pcl_write");  
15.   ros::NodeHandle nh;  
16.   ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);//接收點雲
17.   ros::spin();  
18.   return 0;  
19. }  

2.test_pcd.pcd"

部分資料如下

FIELDS x y z intensity distance sid
SIZE 4 4 4 4 4 4
TYPE F F F F F F
COUNT 1 1 1 1 1 1
WIDTH 460400
HEIGHT 1
POINTS 460400
DATA ascii
-0.93387 -0.6825 -1.1865 12 1.485 7
-0.93173 -0.68323 -1.1878 10 1.485 8
-0.92185 -0.68054 -1.1831 10 1.475 10
-0.91748 -0.67961 -1.1815 10 1.471 11
-0.91479 -0.6799 -1.182 12 1.47 12
-0.90031 -0.68289 -1.1872 10 1.467 18
-0.89271 -0.67941 -1.1811 12 1.457 19
-0.87805 -0.67048 -1.1656 8 1.434 20

3.編譯

cd catkin_ws

source devel/setup.bash

catkin_make

roscore

rosrun imgpcl pcl_load

rosrun imgpcl pcl_write

rosrun rviz rviz

5.Rviz的配置

點選Add,新增pointcloud2,

Fixed  Frame設定為odom.

Topic 設定為/pcl_output