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

#include
#include
#include
#include
#include

#include

#include
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 viewer;

void cloudCB(const sensor_msgs::PointCloud2& input)
{
    pcl::PointCloud cloud; // With color

    pcl::fromROSMsg(input, cloud); // sensor_msgs::PointCloud2 ----> pcl::PointCloud

    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."<
        }
        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 createViewer()
{
    boost::shared_ptr 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."<

    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包有类似的这个功能了

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