openni_pcd_writer.cpp

Go to the documentation of this file.

/* \author Bastian Steder */

/* ---[ */
#include <iostream>
using namespace std;
#include <pcl/io/openni_grabber.h>
#include "pcl/io/pcd_io.h"
#include "pcl/common/file_io.h"
#include "pcl/common/common_headers.h"
#include "pcl/visualization/pcl_visualizer.h"
#include "pcl/console/parse.h"

using namespace pcl;
using namespace pcl::visualization;

//typedef pcl::PointXYZ PointType;
typedef pcl::PointXYZRGB PointType;

boost::mutex mutex_;
pcl::PointCloud<PointType>::ConstPtr point_cloud_ptr;
bool new_cloud = false;
bool use_binary_format = true;

string filename_prefix = "openni";

float max_fps = 5.0;


struct EventHelper
{
  void 
  cloud_cb (const pcl::PointCloud<PointType>::ConstPtr & cloud)
  {
    if (mutex_.try_lock ())
    {
      point_cloud_ptr = cloud;
      static int counter = 0;
      static double start_time = getTime(),
                    last_write_time = 0.0;
      double current_time = getTime();
      //double running_time = getTime()-start_time;
      //cout << counter/time << "fps "<<std::flush;
      if (current_time-last_write_time > 1.0f/max_fps)
      {
        ++counter;
        cout << "w" << std::flush;
        stringstream filename_stream;
        filename_stream << filename_prefix << "_" << setfill ('0') << setw (6) << counter << ".pcd";
        pcl::io::savePCDFile (filename_stream.str(), *cloud, use_binary_format);
        last_write_time = current_time;
      }
      else
      {
        cout << "s"<<std::flush;
      }
      new_cloud = true;
      mutex_.unlock ();
    }

  }
};

void printUsage(const char* progName)
{
  cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
       << "Options:\n"
       << "-------------------------------------------\n"
       << "-p <string>  filename prefix (default: "<<filename_prefix<<")\n"
       << "-h           this help\n"
       << "\n\n";
}

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::parse (argc, argv, "-p", filename_prefix) >= 0)
    cout << "Setting filename prefix to \""<<filename_prefix<<"\".\n";
  
  PCLVisualizer viewer("3D Viewer");
  viewer.addCoordinateSystem(1.0f);
  viewer.setBackgroundColor (1, 1, 1);
  
  openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
  if (driver.getNumberDevices() > 0)
  {
    for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx)
    {
      cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName(deviceIdx) << ", product: " << driver.getProductName(deviceIdx)
        << ", connected: " << (int) driver.getBus(deviceIdx) << " @ " << (int) driver.getAddress(deviceIdx) << ", serial number: \'" << driver.getSerialNumber(deviceIdx) << "\'" << endl;
    }

  }
  else
    cout << "No devices connected." << endl;
  
  std::string device_id = "#1";
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  EventHelper h;
  boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &h, _1);
  boost::signals2::connection c1 = interface->registerCallback (f);
  cout << "Starting grabber\n";
  interface->start ();
  cout << "Done\n";
  //bool first = true;
  
  while(!viewer.wasStopped()) // && (node_handle==NULL || node_handle->ok()))
  {
    viewer.spinOnce(100);
    usleep(100000);
    
    if (new_cloud && mutex_.try_lock ())
    {
      new_cloud = false;
      if (point_cloud_ptr)
      {
        cout << "v" << std::flush;
        
        
        static bool first_point_cloud = true;
        PointCloudColorHandlerCustom<PointType> color_handler_cloud(point_cloud_ptr, 0, 0, 0);
        if (first_point_cloud)
        {
          viewer.addPointCloud<PointType> (point_cloud_ptr, color_handler_cloud, "point cloud");
          first_point_cloud = false;
        }
        else
        {
          viewer.updatePointCloud<PointType> (point_cloud_ptr, color_handler_cloud, "point cloud");
        }
      }
      mutex_.unlock ();
    }
  }

  interface->stop ();
}

  // --------------------------------------
  //bool live_scenes = range_image_source >= 0;
  
  //ros::NodeHandle* node_handle=NULL;
  //ros::Subscriber* subscriber=NULL,
                 //* subscriber2=NULL;
  //if (live_scenes)
  //{
    //ros::init (argc, argv, "pointcloudobjectrecognition");
    //node_handle = new ros::NodeHandle;
    //subscriber = new ros::Subscriber;
    //if (node_handle->resolveName("input")=="/input")
      //std::cerr << "Did you forget input:=<your topic>?\n";
    //if (range_image_source==0)
      //*subscriber = node_handle->subscribe ("input", 1, cloud_msg_cb);
    //else if (range_image_source==1)
    //{
      //if (node_handle->resolveName("input2")=="/input2")
        //std::cerr << "Did you forget input2:=<your camera_info_topic>?\n";
      //*subscriber = node_handle->subscribe ("input", 1, depth_image_msg_cb);
      //subscriber2 = new ros::Subscriber;
      //*subscriber2 = node_handle->subscribe ("input2", 1, camera_info_msg_cb);
    //}
    //else if (range_image_source==2)
      //*subscriber = node_handle->subscribe ("input",  1, disparity_image_msg_cb);
  //}


    //double time_scene_range_image_creation = -getTime();
    //if (range_image_source <= 0)
    //{
      //fromROSMsg(*cloud_msg, point_cloud);
      //point_cloud.sensor_origin_ = cloud_msg_sensor_origin;  point_cloud.sensor_orientation_ = cloud_msg_sensor_orientation;
      //if (far_ranges.points.empty()) {
        //cout << "\n----------\nNo extra far ranges provided. Trying to extract them from the msg...\n";
      //}
      //RangeImage::extractFarRanges(*cloud_msg, far_ranges);
      //if (far_ranges.points.empty()) {
        //cout << "No far ranges found. You might want to use -m as a command line parameter to consider "
             //<< "all unseen areas as far ranges.\n----------\n\n";
      //}
      //Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
      //scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(
                            //point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1], point_cloud.sensor_origin_[2])) *
                          //point_cloud.sensor_orientation_;
      //scene_range_image.createFromPointCloud(point_cloud, angular_resolution, deg2rad(360.0f), deg2rad(180.0f),
                                             //scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
      //scene_range_image.integrateFarRanges(far_ranges);
    //}
    //time_scene_range_image_creation += getTime();
    
    //if (set_unseen_to_max_range)
      //scene_range_image.setUnseenToMaxRange();


/* ]--- */