pcd_to_pointcloud.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $
00035  *
00036  */
00037 
00046 // ROS core
00047 #include <ros/ros.h>
00048 #include <pcl/io/io.h>
00049 #include <pcl/io/pcd_io.h>
00050 #include <pcl/point_types.h>
00051 #include <pcl_conversions/pcl_conversions.h>
00052 
00053 #include "pcl_ros/publisher.h"
00054 
00055 using namespace std;
00056 
00057 class PCDGenerator
00058 {
00059   protected:
00060     string tf_frame_;
00061     ros::NodeHandle nh_;
00062     ros::NodeHandle private_nh_;
00063   public:
00064 
00065     // ROS messages
00066     sensor_msgs::PointCloud2 cloud_;
00067 
00068     string file_name_, cloud_topic_;
00069     double wait_;
00070 
00071     pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
00072 
00074     PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
00075     {
00076       // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00077 
00078       cloud_topic_ = "cloud_pcd";
00079       pub_.advertise (nh_, cloud_topic_.c_str (), 1);
00080       private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
00081       ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
00082     }
00083 
00085     // Start
00086     int
00087       start ()
00088     {
00089       if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
00090         return (-1);
00091       cloud_.header.frame_id = tf_frame_;
00092       return (0);
00093     }
00094 
00096     // Spin (!)
00097     bool spin ()
00098     {
00099       int nr_points      = cloud_.width * cloud_.height;
00100       string fields_list = pcl::getFieldsList (cloud_);
00101       double interval = wait_ * 1e+6;
00102       while (nh_.ok ())
00103       {
00104         ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ());
00105         cloud_.header.stamp = ros::Time::now ();
00106 
00107         if (pub_.getNumSubscribers () > 0)
00108         {
00109           ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ());
00110           pub_.publish (cloud_);
00111         }
00112         else
00113         {
00114                                         // check once a second if there is any subscriber
00115           ros::Duration (1).sleep ();
00116           continue;
00117         }
00118 
00119         usleep (interval);
00120 
00121         if (interval == 0)      // We only publish once if a 0 seconds interval is given
00122                                 {
00123                                         // Give subscribers 3 seconds until point cloud decays... a little ugly!
00124                       ros::Duration (3.0).sleep ();
00125           break;
00126                                 }
00127       }
00128       return (true);
00129     }
00130 
00131 
00132 };
00133 
00134 /* ---[ */
00135 int
00136   main (int argc, char** argv)
00137 {
00138   if (argc < 2)
00139   {
00140     std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
00141     return (-1);
00142   }
00143 
00144   ros::init (argc, argv, "pcd_to_pointcloud");
00145 
00146   PCDGenerator c;
00147   c.file_name_ = string (argv[1]);
00148   // check if publishing interval is given
00149   if (argc == 2)
00150         {
00151         c.wait_ = 0;
00152         }
00153         else
00154         {
00155                 c.wait_ = atof (argv[2]);
00156         }
00157 
00158   if (c.start () == -1)
00159   {
00160     ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
00161     return (-1);
00162   }
00163   ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.",  c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ());
00164   c.spin ();
00165 
00166   return (0);
00167 }
00168 /* ]--- */


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu May 5 2016 04:16:43