pointcloud_online_viewer.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: pointcloud_online_viewer.cpp 33238 2010-03-11 00:46:58Z rusu $
 *
 */

// ROS core
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
// PCL includes
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl_visualization/pcl_visualizer.h>
#include <terminal_tools/print.h>
#include <terminal_tools/parse.h>

using namespace std;
using terminal_tools::print_highlight;
using terminal_tools::parse_argument;

typedef pcl::PointXYZ Point;
typedef pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2> ColorHandler;
typedef ColorHandler::Ptr ColorHandlerPtr;
typedef ColorHandler::ConstPtr ColorHandlerConstPtr;

sensor_msgs::PointCloud2ConstPtr cloud_, cloud_old_;
boost::mutex m;

void
  cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  ROS_INFO ("PointCloud with %d data points (%s), stamp %f, and frame %s.", cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str ()); 
  m.lock ();
  cloud_ = cloud;
  m.unlock ();
}

/* ---[ */
int
  main (int argc, char** argv)
{
  ros::init (argc, argv, "pointcloud_online_viewer");
  ros::NodeHandle nh;

  // Get the queue size from the command line
  int queue_size = 1;
  parse_argument (argc, argv, "-qsize", queue_size);
  print_highlight ("Using a queue size of %d\n", queue_size);

  // Get the number of clouds to keep on screen
  int nr_clouds = 1;
  parse_argument (argc, argv, "-nclouds", nr_clouds);
  
  // Create a ROS subscriber
  ros::Subscriber sub = nh.subscribe ("input", queue_size, cloud_cb);

  pcl_visualization::PCLVisualizer p (argc, argv, "Online PointCloud2 Viewer");
  pcl::PointCloud<Point> cloud_xyz;
  ColorHandlerPtr color_handler;

  int color_handler_idx = 0;
  double psize = 0;
  while (nh.ok ())
  {
    // Spin
    ros::spinOnce ();
    ros::Duration (0.001).sleep ();
    p.spinOnce (10);

    // If no cloud received yet, continue
    if (!cloud_)
      continue;

    if (cloud_ == cloud_old_)
      continue;

    // Save the last color handler used
    color_handler_idx = p.getColorHandlerIndex ("cloud");
    // Save the last point size used
    p.getPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, psize, "cloud");

    p.removePointCloud ("cloud");
    // Convert to PointCloud<T>
    m.lock ();
    
    {
      pcl::fromROSMsg (*cloud_, cloud_xyz);
   
      // Add a random color handler
      color_handler.reset (new pcl_visualization::PointCloudColorHandlerCustom<sensor_msgs::PointCloud2> (*cloud_, 255, 255, 255));
      p.addPointCloud (cloud_xyz, color_handler, "cloud");

      // Add a color handler for each field
      for (size_t i = 0; i < cloud_->fields.size (); ++i)
      {
        if (cloud_->fields[i].name == "rgb" || cloud_->fields[i].name == "rgba")
          color_handler.reset (new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> (*cloud_));
        else
          color_handler.reset (new pcl_visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2> (*cloud_, cloud_->fields[i].name));
        p.addPointCloud (cloud_xyz, color_handler, "cloud");
      }

      // Set the point size
      p.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, psize, "cloud");

      // Set the old color handler index
      if (color_handler_idx != -1)
        p.updateColorHandlerIndex ("cloud", color_handler_idx);
      cloud_old_ = cloud_;
    }
    m.unlock ();
  }

  return (0);
}
/* ]--- */

---

pcl_tutorials
Author(s): Radu Bogdan Rusu, Bastian Steder
autogenerated on Fri Jan 11 09:12:59 2013