concatenate_data.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: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include <pcl/io/io.h>
00040 #include "pcl_ros/transforms.h"
00041 #include "pcl_ros/io/concatenate_data.h"
00042 
00043 #include <pcl_conversions/pcl_conversions.h>
00044 
00046 void
00047 pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
00048 {
00049   private_nh_ = getMTPrivateNodeHandle ();
00050   // ---[ Mandatory parameters
00051   private_nh_.getParam ("output_frame", output_frame_);
00052   private_nh_.getParam ("approximate_sync", approximate_sync_);
00053 
00054   if (output_frame_.empty ())
00055   {
00056     NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
00057     return;
00058   }
00059 
00060   // ---[ Optional parameters
00061   private_nh_.getParam ("max_queue_size", maximum_queue_size_);
00062 
00063   // Output
00064   pub_output_ = private_nh_.advertise<PointCloud2> ("output", maximum_queue_size_);
00065 
00066   XmlRpc::XmlRpcValue input_topics;
00067   if (!private_nh_.getParam ("input_topics", input_topics))
00068   {
00069     NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
00070     return; 
00071   }
00072   // Check the type
00073   switch (input_topics.getType ())
00074   {
00075     case XmlRpc::XmlRpcValue::TypeArray:
00076     {
00077       if (input_topics.size () == 1)
00078       {
00079         NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
00080         return;
00081       }
00082 
00083       if (input_topics.size () > 8)
00084       {
00085         NODELET_ERROR ("[onInit] More than 8 topics passed!");
00086         return;
00087        }
00088 
00089       ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:");
00090       for (int d = 0; d < input_topics.size (); ++d)
00091         ROS_INFO_STREAM (" - " << (std::string)(input_topics[d]));
00092 
00093       // Subscribe to the filters
00094       filters_.resize (input_topics.size ());
00095 
00096       // 8 topics
00097       if (approximate_sync_)
00098         ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
00099                                                                                       PointCloud2, PointCloud2, PointCloud2,
00100                                                                                       PointCloud2, PointCloud2> 
00101                                                       > (maximum_queue_size_));
00102       else
00103         ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
00104                                                                                 PointCloud2, PointCloud2, PointCloud2,
00105                                                                                 PointCloud2, PointCloud2> 
00106                                                       > (maximum_queue_size_));
00107 
00108       // First input_topics.size () filters are valid
00109       for (int d = 0; d < input_topics.size (); ++d)
00110       {
00111         filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
00112         filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_);
00113       }
00114 
00115       // Bogus null filter
00116       filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
00117 
00118       switch (input_topics.size ())
00119       {
00120         case 2:
00121         {
00122           if (approximate_sync_)
00123             ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
00124           else
00125             ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
00126           break;
00127         }
00128         case 3:
00129         {
00130           if (approximate_sync_)
00131             ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
00132           else
00133             ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
00134           break;
00135         }
00136         case 4:
00137         {
00138           if (approximate_sync_)
00139             ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
00140           else
00141             ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
00142           break;
00143         }
00144         case 5:
00145         {
00146           if (approximate_sync_)
00147             ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
00148           else
00149             ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
00150           break;
00151         }
00152         case 6:
00153         {
00154           if (approximate_sync_)
00155             ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
00156           else
00157             ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
00158           break;
00159         }
00160         case 7:
00161         {
00162           if (approximate_sync_)
00163             ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
00164           else
00165             ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
00166           break;
00167         }
00168         case 8:
00169         {
00170           if (approximate_sync_)
00171             ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
00172           else
00173             ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
00174           break;
00175         }
00176         default:
00177         {
00178           NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
00179           return;
00180         }
00181       }
00182       break;
00183     }
00184     default:
00185     {
00186       NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
00187       return;
00188     }
00189   }
00190 
00191   if (approximate_sync_)
00192     ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
00193   else
00194     ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
00195 }
00196 
00198 void 
00199 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
00200 {
00201   //ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
00202   PointCloud2::Ptr in1_t (new PointCloud2 ());
00203   PointCloud2::Ptr in2_t (new PointCloud2 ());
00204 
00205   // Transform the point clouds into the specified output frame
00206   if (output_frame_ != in1.header.frame_id)
00207     pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_);
00208   else
00209     in1_t = boost::make_shared<PointCloud2> (in1);
00210 
00211   if (output_frame_ != in2.header.frame_id)
00212     pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_);
00213   else
00214     in2_t = boost::make_shared<PointCloud2> (in2);
00215 
00216   // Concatenate the results
00217   pcl::concatenatePointCloud (*in1_t, *in2_t, out);
00218   // Copy header
00219   out.header.stamp = in1.header.stamp;
00220 }
00221 
00223 void 
00224 pcl_ros::PointCloudConcatenateDataSynchronizer::input (
00225     const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2, 
00226     const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4, 
00227     const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6, 
00228     const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
00229 {
00230   PointCloud2::Ptr out1 (new PointCloud2 ());
00231   PointCloud2::Ptr out2 (new PointCloud2 ());
00232   pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1);
00233   if (in3 && in3->width * in3->height > 0)
00234   {
00235     pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
00236     out1 = out2;
00237     if (in4 && in4->width * in4->height > 0)
00238     {
00239       pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1);
00240       if (in5 && in5->width * in5->height > 0)
00241       {
00242         pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
00243         out1 = out2;
00244         if (in6 && in6->width * in6->height > 0)
00245         {
00246           pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1);
00247           if (in7 && in7->width * in7->height > 0)
00248           {
00249             pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
00250             out1 = out2;
00251           } 
00252         }
00253       }
00254     }
00255   }
00256   pub_output_.publish (boost::make_shared<PointCloud2> (*out1)); 
00257 }
00258 
00259 typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
00260 PLUGINLIB_EXPORT_CLASS(PointCloudConcatenateDataSynchronizer,nodelet::Nodelet);
00261 


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