Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00044 #ifndef PCL_NODELET_H_
00045 #define PCL_NODELET_H_
00046
00047 #include <sensor_msgs/PointCloud2.h>
00048
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
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
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
00178
00179
00180
00181
00182 return (true);
00183 }
00184
00189 inline bool
00190 isValid (const ModelCoefficientsConstPtr &model, const std::string &topic_name = "model")
00191 {
00192
00193
00194
00195
00196
00197 return (true);
00198 }
00199
00201 virtual void
00202 onInit ()
00203 {
00204 pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
00205
00206
00207 pnh_->getParam ("max_queue_size", max_queue_size_);
00208
00209
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_