pcl_nodelet.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $
35  *
36  */
37 
44 #ifndef PCL_NODELET_H_
45 #define PCL_NODELET_H_
46 
47 #include <sensor_msgs/PointCloud2.h>
48 // PCL includes
49 #include <pcl_msgs/PointIndices.h>
50 #include <pcl_msgs/ModelCoefficients.h>
51 #include <pcl/pcl_base.h>
52 #include <pcl/point_types.h>
54 #include "pcl_ros/point_cloud.h"
55 // ROS Nodelet includes
61 
62 // Include TF
63 #include <tf/transform_listener.h>
64 
66 
67 namespace pcl_ros
68 {
72 
74  {
75  public:
76  typedef sensor_msgs::PointCloud2 PointCloud2;
77 
78  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
81 
82  typedef pcl_msgs::PointIndices PointIndices;
83  typedef PointIndices::Ptr PointIndicesPtr;
84  typedef PointIndices::ConstPtr PointIndicesConstPtr;
85 
86  typedef pcl_msgs::ModelCoefficients ModelCoefficients;
87  typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
88  typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
89 
90  typedef pcl::IndicesPtr IndicesPtr;
91  typedef pcl::IndicesConstPtr IndicesConstPtr;
92 
94  PCLNodelet () : use_indices_ (false), latched_indices_ (false),
96 
97  protected:
108  bool use_indices_;
117 
120 
123 
126 
129 
132 
135 
140  inline bool
141  isValid (const PointCloud2::ConstPtr &cloud, const std::string &topic_name = "input")
142  {
143  if (cloud->width * cloud->height * cloud->point_step != cloud->data.size ())
144  {
145  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 ());
146 
147  return (false);
148  }
149  return (true);
150  }
151 
156  inline bool
157  isValid (const PointCloudConstPtr &cloud, const std::string &topic_name = "input")
158  {
159  if (cloud->width * cloud->height != cloud->points.size ())
160  {
161  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 ());
162 
163  return (false);
164  }
165  return (true);
166  }
167 
172  inline bool
173  isValid (const PointIndicesConstPtr &/*indices*/, const std::string &/*topic_name*/ = "indices")
174  {
175  /*if (indices->indices.empty ())
176  {
177  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 ());
178  return (true);
179  }*/
180  return (true);
181  }
182 
187  inline bool
188  isValid (const ModelCoefficientsConstPtr &/*model*/, const std::string &/*topic_name*/ = "model")
189  {
190  /*if (model->values.empty ())
191  {
192  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 ());
193  return (false);
194  }*/
195  return (true);
196  }
197 
199  virtual void subscribe () {}
200  virtual void unsubscribe () {}
201 
203  virtual void
205  {
207 
208  // Parameters that we care about only at startup
209  pnh_->getParam ("max_queue_size", max_queue_size_);
210 
211  // ---[ Optional parameters
212  pnh_->getParam ("use_indices", use_indices_);
213  pnh_->getParam ("latched_indices", latched_indices_);
214  pnh_->getParam ("approximate_sync", approximate_sync_);
215 
216  NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
217  " - approximate_sync : %s\n"
218  " - use_indices : %s\n"
219  " - latched_indices : %s\n"
220  " - max_queue_size : %d",
221  getName ().c_str (),
222  (approximate_sync_) ? "true" : "false",
223  (use_indices_) ? "true" : "false",
224  (latched_indices_) ? "true" : "false",
225  max_queue_size_);
226  }
227 
228  public:
229  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230  };
231 }
232 
233 #endif //#ifndef PCL_NODELET_H_
pcl_msgs::ModelCoefficients ModelCoefficients
Definition: pcl_nodelet.h:86
PointIndices::Ptr PointIndicesPtr
Definition: pcl_nodelet.h:83
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
#define NODELET_WARN(...)
ModelCoefficients::Ptr ModelCoefficientsPtr
Definition: pcl_nodelet.h:87
PCLNodelet()
Empty constructor.
Definition: pcl_nodelet.h:94
bool isValid(const PointCloudConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
Definition: pcl_nodelet.h:157
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:122
pcl::IndicesConstPtr IndicesConstPtr
Definition: pcl_nodelet.h:91
bool isValid(const ModelCoefficientsConstPtr &, const std::string &="model")
Test whether a given ModelCoefficients message is "valid" (i.e., has values).
Definition: pcl_nodelet.h:188
bool latched_indices_
Set to true if the indices topic is latched.
Definition: pcl_nodelet.h:116
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
Definition: pcl_nodelet.h:141
const std::string & getName() const
bool isValid(const PointIndicesConstPtr &, const std::string &="indices")
Test whether a given PointIndices message is "valid" (i.e., has values).
Definition: pcl_nodelet.h:173
boost::shared_ptr< ros::NodeHandle > pnh_
sensor_msgs::PointCloud2 PointCloud2
Definition: pcl_nodelet.h:76
boost::shared_ptr< const PointCloud > PointCloudConstPtr
Definition: pcl_nodelet.h:80
PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should inherit from this class...
Definition: pcl_nodelet.h:73
pcl_msgs::PointIndices PointIndices
Definition: pcl_nodelet.h:82
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: pcl_nodelet.h:79
virtual void unsubscribe()
Definition: pcl_nodelet.h:200
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: pcl_nodelet.h:78
tf::TransformListener tf_listener_
TF listener object.
Definition: pcl_nodelet.h:134
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: pcl_nodelet.h:131
pcl::IndicesPtr IndicesPtr
Definition: pcl_nodelet.h:90
virtual void subscribe()
Lazy transport subscribe/unsubscribe routine. It is optional for backward compatibility.
Definition: pcl_nodelet.h:199
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:128
ModelCoefficients::ConstPtr ModelCoefficientsConstPtr
Definition: pcl_nodelet.h:88
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:125
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
#define NODELET_DEBUG(...)
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:95
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.
Definition: pcl_nodelet.h:119


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Thu Feb 16 2023 03:08:02