pcl_nodelet.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $
00035  *
00036  */
00037 
00044 #ifndef PCL_NODELET_H_
00045 #define PCL_NODELET_H_
00046 
00047 #include <sensor_msgs/PointCloud2.h>
00048 // PCL includes
00049 #include <pcl_msgs/PointIndices.h>
00050 #include <pcl_msgs/ModelCoefficients.h>
00051 #include <pcl/point_types.h>
00052 #include <pcl_conversions/pcl_conversions.h>
00053 #include "pcl_ros/point_cloud.h"
00054 // ROS Nodelet includes
00055 #include <nodelet/nodelet.h>
00056 #include <message_filters/subscriber.h>
00057 #include <message_filters/synchronizer.h>
00058 #include <message_filters/sync_policies/exact_time.h>
00059 #include <message_filters/sync_policies/approximate_time.h>
00060 
00061 // Include TF
00062 #include <tf/transform_listener.h>
00063 
00064 using pcl_conversions::fromPCL;
00065 
00066 namespace pcl_ros
00067 {
00071 
00072   class PCLNodelet : public nodelet::Nodelet
00073   {
00074     public:
00075       typedef sensor_msgs::PointCloud2 PointCloud2;
00076 
00077       typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00078       typedef PointCloud::Ptr PointCloudPtr;
00079       typedef PointCloud::ConstPtr PointCloudConstPtr;
00080 
00081       typedef pcl_msgs::PointIndices PointIndices;
00082       typedef PointIndices::Ptr PointIndicesPtr;
00083       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00084 
00085       typedef pcl_msgs::ModelCoefficients ModelCoefficients;
00086       typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
00087       typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
00088 
00089       typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
00090       typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
00091 
00093       PCLNodelet () : use_indices_ (false), latched_indices_ (false),
00094                       max_queue_size_ (3), approximate_sync_ (false) {};
00095 
00096     protected:
00098       boost::shared_ptr<ros::NodeHandle> pnh_;
00099 
00110       bool use_indices_;
00118       bool latched_indices_;
00119 
00121       message_filters::Subscriber<PointCloud> sub_input_filter_;
00122 
00124       message_filters::Subscriber<PointIndices> sub_indices_filter_;
00125 
00127       ros::Publisher pub_output_;
00128 
00130       int max_queue_size_;
00131 
00133       bool approximate_sync_;
00134 
00136       tf::TransformListener tf_listener_;
00137 
00142       inline bool
00143       isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input")
00144       {
00145         if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
00146         {
00147           NODELET_WARN ("[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->data.size (), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
00148 
00149           return (false);
00150         }
00151         return (true);
00152       }
00153 
00158       inline bool
00159       isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input")
00160       {
00161         if (cloud->width * cloud->height != cloud->points.size ())
00162         {
00163           NODELET_WARN ("[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), cloud->points.size (), cloud->width, cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
00164 
00165           return (false);
00166         }
00167         return (true);
00168       }
00169 
00174       inline bool
00175       isValid (const PointIndicesConstPtr &indices, const std::string &topic_name = "indices")
00176       {
00177         /*if (indices->indices.empty ())
00178         {
00179           NODELET_WARN ("[%s] Empty indices (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
00180           return (true);
00181         }*/
00182         return (true);
00183       }
00184 
00189       inline bool
00190       isValid (const ModelCoefficientsConstPtr &model, const std::string &topic_name = "model")
00191       {
00192         /*if (model->values.empty ())
00193         {
00194           NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, and frame %s on topic %s received!", getName ().c_str (), model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
00195           return (false);
00196         }*/
00197         return (true);
00198       }
00199 
00201       virtual void
00202       onInit ()
00203       {
00204         pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
00205         
00206         // Parameters that we care about only at startup
00207         pnh_->getParam ("max_queue_size", max_queue_size_);
00208         
00209         // ---[ Optional parameters
00210         pnh_->getParam ("use_indices", use_indices_);
00211         pnh_->getParam ("latched_indices", latched_indices_);
00212         pnh_->getParam ("approximate_sync", approximate_sync_);
00213 
00214         NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
00215             " - approximate_sync : %s\n"
00216             " - use_indices      : %s\n"
00217             " - latched_indices  : %s\n"
00218             " - max_queue_size   : %d",
00219             getName ().c_str (), 
00220             (approximate_sync_) ? "true" : "false",
00221             (use_indices_) ? "true" : "false", 
00222             (latched_indices_) ? "true" : "false", 
00223             max_queue_size_);
00224       }
00225 
00226     public:
00227       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00228   };
00229 }
00230 
00231 #endif  //#ifndef PCL_NODELET_H_


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Aug 26 2015 15:25:31