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

最新推荐文章于 2024-07-03 16:27:39 发布

yaked19

最新推荐文章于 2024-07-03 16:27:39 发布

阅读量6.2k

点赞数 2

分类专栏： PCL点云处理 ROS

本文链接：https://blog.csdn.net/yaked/article/details/52584231

版权

ROS 同时被 2 个专栏收录

42 篇文章 39 订阅

订阅专栏

PCL点云处理

8 篇文章 5 订阅

订阅专栏

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

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

====================================

PCL 1.8中利用openni2获取配准点云的示例程序，修改后空格键保存点云为inputcloud*.pcd，精度较高，值得使用。原始代码为/home/yake/ProgramFiles/pcl-pcl-1.8.0/visualization/tools/openni2_viewer.cpp修改为

/home/yake/pcl_1.8/src/xtion_openni2_topic.cpp

#define MEASURE_FUNCTION_TIME
#include <pcl/common/time.h> //fps calculations
#include <pcl/common/angles.h>
#include <pcl/io/openni2_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/boost.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

#include <boost/chrono.hpp>

#include "pcl/io/openni2/openni.h"

using std::stringstream;
using std::string;

typedef boost::chrono::high_resolution_clock HRClock;

//
template <typename PointType>
class OpenNI2Viewer
{
public:
    typedef pcl::PointCloud<PointType> Cloud;
    typedef typename Cloud::ConstPtr CloudConstPtr;

    boost::shared_ptr<pcl::visualization::PCLVisualizer> cloud_viewer_;
    boost::shared_ptr<pcl::visualization::ImageViewer> image_viewer_;

    pcl::io::OpenNI2Grabber& grabber_;
    boost::mutex cloud_mutex_;
    boost::mutex image_mutex_;

    CloudConstPtr cloud_;
    boost::shared_ptr<pcl::io::openni2::Image> image_;
    unsigned char* rgb_data_;
    unsigned rgb_data_size_;

    bool saveCloud;
    unsigned int filesNum;

    OpenNI2Viewer (pcl::io::OpenNI2Grabber& grabber)
        : cloud_viewer_ (new pcl::visualization::PCLVisualizer ("PCL OpenNI2 cloud"))
        , image_viewer_ ()
        , grabber_ (grabber)
        , rgb_data_ (0), rgb_data_size_ (0)
        ,saveCloud(false)
        ,filesNum (0)
    {
    }

    void
    cloud_callback (const CloudConstPtr& cloud)
    {
        boost::mutex::scoped_lock lock (cloud_mutex_);
        cloud_ = cloud;
    }

    void
    image_callback (const boost::shared_ptr<pcl::io::openni2::Image>& image)
    {
        boost::mutex::scoped_lock lock (image_mutex_);
        image_ = image;

        if (image->getEncoding () != pcl::io::openni2::Image::RGB)
        {
            if (rgb_data_size_ < image->getWidth () * image->getHeight ())
            {
                if (rgb_data_)
                    delete [] rgb_data_;
                rgb_data_size_ = image->getWidth () * image->getHeight ();
                rgb_data_ = new unsigned char [rgb_data_size_ * 3];
            }
            image_->fillRGB (image_->getWidth (), image_->getHeight (), rgb_data_);
        }
    }

    void
    keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
    {
        if(event.getKeySym() == "space"&& event.keyDown())
            saveCloud = true;
        //    if (event.getKeyCode ())
        //      cout << "the key \'" << event.getKeyCode () << "\' (" << event.getKeyCode () << ") was";
        //    else
        //      cout << "the special key \'" << event.getKeySym () << "\' was";
        //    if (event.keyDown ())
        //      cout << " pressed" << endl;
        //    else
        //      cout << " released" << endl;
    }

    //  void
    //  mouse_callback (const pcl::visualization::MouseEvent& mouse_event, void*)
    //  {
    //    if (mouse_event.getType () == pcl::visualization::MouseEvent::MouseButtonPress && mouse_event.getButton () == pcl::visualization::MouseEvent::LeftButton)
    //    {
    //      cout << "left button pressed @ " << mouse_event.getX () << " , " << mouse_event.getY () << endl;
    //    }
    //  }

    void
    run ()
    {
        //    cloud_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this);
        cloud_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this);
        cloud_viewer_->setCameraFieldOfView (1.02259994f);
        boost::function<void (const CloudConstPtr&) > cloud_cb = boost::bind (&OpenNI2Viewer::cloud_callback, this, _1);
        boost::signals2::connection cloud_connection = grabber_.registerCallback (cloud_cb);

        boost::signals2::connection image_connection;
        if (grabber_.providesCallback<void (const boost::shared_ptr<pcl::io::openni2::Image>&)>())
        {
            image_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI image"));
            //      image_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this);
            image_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this);
            boost::function<void (const boost::shared_ptr<pcl::io::openni2::Image>&) > image_cb = boost::bind (&OpenNI2Viewer::image_callback, this, _1);
            image_connection = grabber_.registerCallback (image_cb);
        }

        bool image_init = false, cloud_init = false;

        grabber_.start ();

        while (!cloud_viewer_->wasStopped () && (image_viewer_ && !image_viewer_->wasStopped ()))
        {
            boost::shared_ptr<pcl::io::openni2::Image> image;
            CloudConstPtr cloud;
            //      /home/yake/ProgramFiles/yake_pcl180_installed/include/pcl-1.8/pcl/io/pcd_io.h:563: note:   no known conversion for argument 2 from 'OpenNI2Viewer<pcl::PointXYZ>::CloudConstPtr {aka boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >}' to 'const pcl::PCLPointCloud2&'
            //       pcl::PointCloud<pcl::PointXYZRGBA> cloud;
            //      Cloud cloud;// pcl::PointCloud<PointType> Cloud

            cloud_viewer_->spinOnce ();

            // See if we can get a cloud
            if (cloud_mutex_.try_lock ())
            {
                cloud_.swap (cloud);
                cloud_mutex_.unlock ();
            }

            if (cloud)
            {
                if (!cloud_init)
                {
                    cloud_viewer_->setPosition (0, 0);
                    cloud_viewer_->setSize (cloud->width, cloud->height);
                    cloud_init = !cloud_init;
                }

                if (!cloud_viewer_->updatePointCloud (cloud, "OpenNICloud"))
                {
                    cloud_viewer_->addPointCloud (cloud, "OpenNICloud");
                    cloud_viewer_->resetCameraViewpoint ("OpenNICloud");
                    cloud_viewer_->setCameraPosition (
                                0,0,0,		// Position
                                0,0,1,		// Viewpoint
                                0,-1,0);	// Up
                }

                if(saveCloud)
                {
                    stringstream stream;
                    stream << "inputCloud"<< filesNum<< ".pcd";
                    string filename = stream.str();

                    if(pcl::io::savePCDFile(filename, *cloud, true) == 0)
                    {
                        filesNum++;
                        cout << filename<<" Saved."<<endl;
                    }
                    else PCL_ERROR("Problem saving %s.\n", filename.c_str());

                    saveCloud = false;
                }

            }

            // See if we can get an image
            if (image_mutex_.try_lock ())
            {
                image_.swap (image);
                image_mutex_.unlock ();
            }


            if (image)
            {
                if (!image_init && cloud && cloud->width != 0)
                {
                    image_viewer_->setPosition (cloud->width, 0);
                    image_viewer_->setSize (cloud->width, cloud->height);
                    image_init = !image_init;
                }

                if (image->getEncoding () == pcl::io::openni2::Image::RGB)
                    image_viewer_->addRGBImage ( (const unsigned char*)image->getData (), image->getWidth (), image->getHeight ());
                else
                    image_viewer_->addRGBImage (rgb_data_, image->getWidth (), image->getHeight ());
                image_viewer_->spinOnce ();

            }
        }

        grabber_.stop ();

        cloud_connection.disconnect ();
        image_connection.disconnect ();
        if (rgb_data_)
            delete[] rgb_data_;
    }

};
//

boost::shared_ptr<pcl::visualization::PCLVisualizer> cld;
boost::shared_ptr<pcl::visualization::ImageViewer> img;

int main (int argc, char** argv)
{
    std::string device_id ("");
    pcl::io::OpenNI2Grabber::Mode depth_mode = pcl::io::OpenNI2Grabber::OpenNI_Default_Mode;
    pcl::io::OpenNI2Grabber::Mode image_mode = pcl::io::OpenNI2Grabber::OpenNI_Default_Mode;
    bool xyz = false;

    boost::shared_ptr<pcl::io::openni2::OpenNI2DeviceManager> deviceManager = pcl::io::openni2::OpenNI2DeviceManager::getInstance ();
    if (deviceManager->getNumOfConnectedDevices () > 0)
    {
        boost::shared_ptr<pcl::io::openni2::OpenNI2Device> device = deviceManager->getAnyDevice ();
        cout << "Device ID not set, using default device: " << device->getStringID () << endl;
    }else
    {
        cout << "No devices connected." << endl;
        return -1;
    }

    unsigned mode;
    if (pcl::console::parse (argc, argv, "-depthmode", mode) != -1)
        depth_mode = pcl::io::OpenNI2Grabber::Mode (mode);

    if (pcl::console::parse (argc, argv, "-imagemode", mode) != -1)
        image_mode = pcl::io::OpenNI2Grabber::Mode (mode);

    if (pcl::console::find_argument (argc, argv, "-xyz") != -1)
        xyz = true;

    pcl::io::OpenNI2Grabber grabber (device_id, depth_mode, image_mode);

    if (xyz || !grabber.providesCallback<pcl::io::OpenNI2Grabber::sig_cb_openni_point_cloud_rgb> ())
    {
        OpenNI2Viewer<pcl::PointXYZ> openni_viewer (grabber);
        openni_viewer.run ();
    }
    else
    {
        OpenNI2Viewer<pcl::PointXYZRGBA> openni_viewer (grabber);
        openni_viewer.run ();
    }

    return 0;
}