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   
  2. #include   
  3. #include   
  4. #include   
  5. #include   
  6.   
  7. #include   
  8.   
  9. #include   
  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 viewer;  
  21.   
  22. void cloudCB(const sensor_msgs::PointCloud2& input)  
  23. {  
  24.     pcl::PointCloud cloud; // With color  
  25.   
  26.     pcl::fromROSMsg(input, cloud); // sensor_msgs::PointCloud2 ----> pcl::PointCloud  
  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."<
  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 createViewer()  
  61. {  
  62.     boost::shared_ptr 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."<
  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

你可能感兴趣的:(ROS)