ROS下订阅topic保存为点云（1）

来源：互联网发布：西安三星项目瘫痪知乎编辑：程序博客网时间：2024/05/17 01:52

这里订阅了的是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;    }}voidkeyboardEventOccured(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包有类似的这个功能了

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

0 0