pcd_io.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_io.cpp 35812 2011-02-08 00:05:03Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include <pcl_ros/io/pcd_io.h>
00040 
00042 void
00043 pcl_ros::PCDReader::onInit ()
00044 {
00045   ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
00046   // Provide a latched topic
00047   ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true);
00048 
00049   private_nh.getParam ("publish_rate", publish_rate_);
00050   private_nh.getParam ("tf_frame", tf_frame_);
00051 
00052   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00053                  " - publish_rate : %f\n"
00054                  " - tf_frame     : %s",
00055                  getName ().c_str (),
00056                  publish_rate_, tf_frame_.c_str ());
00057 
00058   PointCloud2Ptr output_new;
00059   output_ = boost::make_shared<PointCloud2> ();
00060   output_new = boost::make_shared<PointCloud2> ();
00061 
00062   // Wait in a loop until someone connects
00063   do
00064   {
00065     ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
00066     ros::spinOnce ();
00067     ros::Duration (0.01).sleep ();
00068   }
00069   while (private_nh.ok () && pub_output.getNumSubscribers () == 0);
00070 
00071   std::string file_name;
00072 
00073   while (private_nh.ok ())
00074   {
00075     // Get the current filename parameter. If no filename set, loop
00076     if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ())
00077     {
00078       ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
00079       ros::Duration (0.01).sleep ();
00080       ros::spinOnce ();
00081       continue;
00082     }
00083 
00084     // If the filename parameter holds a different value than the last one we read
00085     if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
00086     {
00087       NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
00088       file_name = file_name_;
00089       pcl::PCLPointCloud2 cloud;
00090       if (impl_.read (file_name_, cloud) < 0)
00091       {
00092         NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
00093         return;
00094       }
00095       pcl_conversions::moveFromPCL(cloud, *(output_));
00096       output_->header.stamp    = ros::Time::now ();
00097       output_->header.frame_id = tf_frame_;
00098     }
00099 
00100     // We do not publish empty data
00101     if (output_->data.size () == 0)
00102       continue;
00103 
00104     if (publish_rate_ == 0)
00105     {
00106       if (output_ != output_new)
00107       {
00108         NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
00109         pub_output.publish (output_);
00110         output_new = output_;
00111       }
00112       ros::Duration (0.01).sleep ();
00113     }
00114     else
00115     {
00116       NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
00117       output_->header.stamp = ros::Time::now ();
00118       pub_output.publish (output_);
00119 
00120       ros::Duration (publish_rate_).sleep ();
00121     }
00122 
00123     ros::spinOnce ();
00124     // Update parameters from server
00125     private_nh.getParam ("publish_rate", publish_rate_);
00126     private_nh.getParam ("tf_frame", tf_frame_);
00127   }
00128 }
00129 
00131 
00133 void
00134 pcl_ros::PCDWriter::onInit ()
00135 {
00136   ros::NodeHandle pnh = getMTPrivateNodeHandle ();
00137   sub_input_ = pnh.subscribe ("input", 1,  &PCDWriter::input_callback, this);
00138   // ---[ Optional parameters
00139   pnh.getParam ("filename", file_name_);
00140   pnh.getParam ("binary_mode", binary_mode_);
00141 
00142   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00143                  " - filename     : %s\n"
00144                  " - binary_mode  : %s", 
00145                  getName ().c_str (),
00146                  file_name_.c_str (), (binary_mode_) ? "true" : "false");
00147 
00148 }
00149 
00151 void
00152 pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
00153 {
00154   if (!isValid (cloud))
00155     return;
00156   
00157   getMTPrivateNodeHandle ().getParam ("filename", file_name_);
00158 
00159   NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
00160  
00161   std::string fname;
00162   if (file_name_.empty ())
00163     fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
00164   else
00165     fname = file_name_;
00166   pcl::PCLPointCloud2 pcl_cloud;
00167   // It is safe to remove the const here because we are the only subscriber callback.
00168   pcl_conversions::moveToPCL(*(const_cast<PointCloud2*>(cloud.get())), pcl_cloud);
00169   impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
00170 
00171   NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
00172 }
00173 
00174 typedef pcl_ros::PCDReader PCDReader;
00175 typedef pcl_ros::PCDWriter PCDWriter;
00176 PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet);
00177 PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet);
00178 


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