ROS下订阅topic保存为点云

这里订阅了的是Kinect for Xbox 360或是华硕的Xtion Pro Live的topic：/camera/depth_registered/points。

Kinect 用roslaunch openni_launch openni.launch depth_registration:=true来开启，Kinect for windows 2.0目前来看是用iai_kinect这个package的，并不带有这个topic，其他很多名字都变化了，所以注意自己使用的设备。

Xtion用roslaunch openni2_launch openni.launch depth_registration:=true来开启，

当然你也可以使用rosrun rqt_reconfigure rqt_reconfigure指令通过GUI打开；

如果觉得每次修改麻烦，进到launch文件里，将那个选项设为true，以后一启动launch就可以获得这个topic了。这个topic是包含了利用相机内置的calibration参数来配准RGB图和深度图，也即它的像素点包含了颜色和深度信息。

按空格键会将当前的点云保存下来，名字为inputcloud0.pcd，inputcloud1.pcd...依次类推

pcd_saver.cpp

[cpp] view plain copy

1. #include <iostream>  
2. #include <ros/ros.h>  
3. #include <pcl/point_cloud.h>  
4. #include <pcl_conversions/pcl_conversions.h>  
5. #include <pcl/io/pcd_io.h>  
6.   
7. #include <pcl/visualization/cloud_viewer.h>  
8.   
9. #include <sensor_msgs/PointCloud2.h>  
10. using std::cout;  
11. using std::endl;  
12. using std::stringstream;  
13. using std::string;  
14.   
15. using namespace pcl;  
16.   
17. unsigned int filesNum = 0;  
18. bool saveCloud(false);  
19.   
20. boost::shared_ptr<visualization::CloudViewer> viewer;  
21.   
22. void cloudCB(const sensor_msgs::PointCloud2& input)  
23. {  
24.     pcl::PointCloud<pcl::PointXYZRGBA> cloud; // With color  
25.   
26.     pcl::fromROSMsg(input, cloud); // sensor_msgs::PointCloud2 ----> pcl::PointCloud<T>  
27.   
28.     if(! viewer->wasStopped()) viewer->showCloud(cloud.makeShared());  
29.   
30.     if(saveCloud)  
31.     {  
32.         stringstream stream;  
33.         stream << "inputCloud"<< filesNum<< ".pcd";  
34.         string filename = stream.str();  
35.   
36.         if(io::savePCDFile(filename, cloud, true) == 0)  
37.         {  
38.             filesNum++;  
39.             cout << filename<<" Saved."<<endl;  
40.         }  
41.         else PCL_ERROR("Problem saving %s.\n", filename.c_str());  
42.   
43.         saveCloud = false;  
44.   
45.     }  
46.   
47.   
48. }  
49.   
50.   
51. void  
52. keyboardEventOccured(const visualization::KeyboardEvent& event, void* nothing)  
53. {  
54.     if(event.getKeySym() == "space"&& event.keyDown())  
55.         saveCloud = true;  
56.   
57. }  
58.   
59. // Creates, initializes and returns a new viewer.  
60. boost::shared_ptr<visualization::CloudViewer> createViewer()  
61. {  
62.     boost::shared_ptr<visualization::CloudViewer> v(new visualization::CloudViewer("OpenNI viewer"));  
63.     v->registerKeyboardCallback(keyboardEventOccured);  
64.   
65.     return(v);  
66. }  
67.   
68. int main (int argc, char** argv)  
69. {  
70.     ros::init(argc, argv, "pcl_write");  
71.     ros::NodeHandle nh;  
72.     cout<< "Press space to record point cloud to a file."<<endl;  
73.   
74.     viewer = createViewer();  
75.   
76.     ros::Subscriber pcl_sub = nh.subscribe("/camera/depth_registered/points", 1, cloudCB);  
77.   
78.     ros::Rate rate(30.0);  
79.   
80.     while (ros::ok() && ! viewer->wasStopped())  
81.     {  
82.         ros::spinOnce();  
83.         rate.sleep();  
84.     }  
85.   
86.     return 0;  
87. }  

============================2016.11=============================================

https://www.youtube.com/watch?v=MQoSDpAsqps 在评论下面看到pcl_ros包有类似的这个功能了

http://wiki.ros.org/%20pcl_ros#pointcloud_to_pcd