这里订阅了的是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
- #include <iostream>
- #include <ros/ros.h>
- #include <pcl/point_cloud.h>
- #include <pcl_conversions/pcl_conversions.h>
- #include <pcl/io/pcd_io.h>
- #include <pcl/visualization/cloud_viewer.h>
- #include <sensor_msgs/PointCloud2.h>
- using std::cout;
- using std::endl;
- using std::stringstream;
- using std::string;
- using namespace pcl;
- unsigned int filesNum = 0;
- bool saveCloud(false);
- boost::shared_ptr<visualization::CloudViewer> viewer;
- void cloudCB(const sensor_msgs::PointCloud2& input)
- {
- pcl::PointCloud<pcl::PointXYZRGBA> cloud; // With color
- pcl::fromROSMsg(input, cloud); // sensor_msgs::PointCloud2 ----> pcl::PointCloud<T>
- if(! viewer->wasStopped()) viewer->showCloud(cloud.makeShared());
- if(saveCloud)
- {
- stringstream stream;
- stream << "inputCloud"<< filesNum<< ".pcd";
- string filename = stream.str();
- if(io::savePCDFile(filename, cloud, true) == 0)
- {
- filesNum++;
- cout << filename<<" Saved."<<endl;
- }
- else PCL_ERROR("Problem saving %s.\n", filename.c_str());
- saveCloud = false;
- }
- }
- void
- keyboardEventOccured(const visualization::KeyboardEvent& event, void* nothing)
- {
- if(event.getKeySym() == "space"&& event.keyDown())
- saveCloud = true;
- }
- // Creates, initializes and returns a new viewer.
- boost::shared_ptr<visualization::CloudViewer> createViewer()
- {
- boost::shared_ptr<visualization::CloudViewer> v(new visualization::CloudViewer("OpenNI viewer"));
- v->registerKeyboardCallback(keyboardEventOccured);
- return(v);
- }
- int main (int argc, char** argv)
- {
- ros::init(argc, argv, "pcl_write");
- ros::NodeHandle nh;
- cout<< "Press space to record point cloud to a file."<<endl;
- viewer = createViewer();
- ros::Subscriber pcl_sub = nh.subscribe("/camera/depth_registered/points", 1, cloudCB);
- ros::Rate rate(30.0);
- while (ros::ok() && ! viewer->wasStopped())
- {
- ros::spinOnce();
- rate.sleep();
- }
- return 0;
- }
============================2016.11=============================================
https://www.youtube.com/watch?v=MQoSDpAsqps 在评论下面看到pcl_ros包有类似的这个功能了