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/PointIndices.h>
00050 #include <pcl/ModelCoefficients.h>
00051 #include <pcl/point_types.h>
00052 #include "pcl_ros/point_cloud.h"
00053 // ROS Nodelet includes
00054 #include <nodelet/nodelet.h>
00055 #include <message_filters/subscriber.h>
00056 #include <message_filters/synchronizer.h>
00057 #include <message_filters/sync_policies/exact_time.h>
00058 #include <message_filters/sync_policies/approximate_time.h>
00059 
00060 // Include TF
00061 #include <tf/transform_listener.h>
00062 
00063 namespace pcl_ros
00064 {
00068 
00069   class PCLNodelet : public nodelet::Nodelet
00070   {
00071     public:
00072       typedef sensor_msgs::PointCloud2 PointCloud2;
00073 
00074       typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00075       typedef PointCloud::Ptr PointCloudPtr;
00076       typedef PointCloud::ConstPtr PointCloudConstPtr;
00077 
00078       typedef pcl::PointIndices PointIndices;
00079       typedef PointIndices::Ptr PointIndicesPtr;
00080       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00081 
00082       typedef pcl::ModelCoefficients ModelCoefficients;
00083       typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
00084       typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
00085 
00086       typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
00087       typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
00088 
00090       PCLNodelet () : use_indices_ (false), latched_indices_ (false),
00091                       max_queue_size_ (3), approximate_sync_ (false) {};
00092 
00093     protected:
00095       boost::shared_ptr<ros::NodeHandle> pnh_;
00096 
00107       bool use_indices_;
00115       bool latched_indices_;
00116 
00118       message_filters::Subscriber<PointCloud> sub_input_filter_;
00119 
00121       message_filters::Subscriber<PointIndices> sub_indices_filter_;
00122 
00124       ros::Publisher pub_output_;
00125 
00127       int max_queue_size_;
00128 
00130       bool approximate_sync_;
00131 
00133       tf::TransformListener tf_listener_;
00134 
00139       inline bool
00140       isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input")
00141       {
00142         if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
00143         {
00144           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 ());
00145 
00146           return (false);
00147         }
00148         return (true);
00149       }
00150 
00155       inline bool
00156       isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input")
00157       {
00158         if (cloud->width * cloud->height != cloud->points.size ())
00159         {
00160           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, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ());
00161 
00162           return (false);
00163         }
00164         return (true);
00165       }
00166 
00171       inline bool
00172       isValid (const PointIndicesConstPtr &indices, const std::string &topic_name = "indices")
00173       {
00174         /*if (indices->indices.empty ())
00175         {
00176           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 ());
00177           return (true);
00178         }*/
00179         return (true);
00180       }
00181 
00186       inline bool
00187       isValid (const ModelCoefficientsConstPtr &model, const std::string &topic_name = "model")
00188       {
00189         /*if (model->values.empty ())
00190         {
00191           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 ());
00192           return (false);
00193         }*/
00194         return (true);
00195       }
00196 
00198       virtual void
00199       onInit ()
00200       {
00201         pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
00202         
00203         // Parameters that we care about only at startup
00204         pnh_->getParam ("max_queue_size", max_queue_size_);
00205         
00206         // ---[ Optional parameters
00207         pnh_->getParam ("use_indices", use_indices_);
00208         pnh_->getParam ("latched_indices", latched_indices_);
00209         pnh_->getParam ("approximate_sync", approximate_sync_);
00210 
00211         NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
00212             " - approximate_sync : %s\n"
00213             " - use_indices      : %s\n"
00214             " - latched_indices  : %s\n"
00215             " - max_queue_size   : %d",
00216             getName ().c_str (), 
00217             (approximate_sync_) ? "true" : "false",
00218             (use_indices_) ? "true" : "false", 
00219             (latched_indices_) ? "true" : "false", 
00220             max_queue_size_);
00221       }
00222 
00223     public:
00224       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00225   };
00226 }
00227 
00228 #endif  //#ifndef PCL_NODELET_H_


pcl_ros
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:22:23