$search
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_