feature.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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 OWNERff 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: feature.cpp 35422 2011-01-24 20:04:44Z rusu $
35  *
36  */
37 
38 //#include <pluginlib/class_list_macros.hpp>
39 // Include the implementations here instead of compiling them separately to speed up compile time
40 //#include "normal_3d.cpp"
41 //#include "boundary.cpp"
42 //#include "fpfh.cpp"
43 //#include "fpfh_omp.cpp"
44 //#include "moment_invariants.cpp"
45 //#include "normal_3d_omp.cpp"
46 //#include "normal_3d_tbb.cpp"
47 //#include "pfh.cpp"
48 //#include "principal_curvatures.cpp"
49 //#include "vfh.cpp"
51 
53 void
55 {
56  // Call the super onInit ()
58 
59  // Call the child init
60  childInit (*pnh_);
61 
62  // Allow each individual class that inherits from us to declare their own Publisher
63  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
64  //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
65 
66  // ---[ Mandatory parameters
67  if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
68  {
69  NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
70  return;
71  }
72  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
73  {
74  NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
75  return;
76  }
77 
78  // ---[ Optional parameters
79  pnh_->getParam ("use_surface", use_surface_);
80 
81  // Enable the dynamic reconfigure service
82  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
83  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
84  srv_->setCallback (f);
85 
86  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
87  " - use_surface : %s\n"
88  " - k_search : %d\n"
89  " - radius_search : %f\n"
90  " - spatial_locator: %d",
91  getName ().c_str (),
92  (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
93 
95 }
96 
98 void
100 {
101  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
102  if (use_indices_ || use_surface_)
103  {
104  // Create the objects here
105  if (approximate_sync_)
106  sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
107  else
108  sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
109 
110  // Subscribe to the input using a filter
111  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
112  if (use_indices_)
113  {
114  // If indices are enabled, subscribe to the indices
115  sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
116  if (use_surface_) // Use both indices and surface
117  {
118  // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
119  sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
120  if (approximate_sync_)
121  sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
122  else
123  sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
124  }
125  else // Use only indices
126  {
127  sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
128  // surface not enabled, connect the input-indices duo and register
129  if (approximate_sync_)
130  sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
131  else
132  sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
133  }
134  }
135  else // Use only surface
136  {
137  sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
138  // indices not enabled, connect the input-surface duo and register
139  sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
140  if (approximate_sync_)
141  sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
142  else
143  sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
144  }
145  // Register callbacks
146  if (approximate_sync_)
147  sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
148  else
149  sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
150  }
151  else
152  // Subscribe in an old fashion to input only (no filters)
153  sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
154 }
155 
157 void
159 {
160  if (use_indices_ || use_surface_)
161  {
162  sub_input_filter_.unsubscribe ();
163  if (use_indices_)
164  {
165  sub_indices_filter_.unsubscribe ();
166  if (use_surface_)
167  sub_surface_filter_.unsubscribe ();
168  }
169  else
170  sub_surface_filter_.unsubscribe ();
171  }
172  else
173  sub_input_.shutdown ();
174 }
175 
177 void
178 pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t /*level*/)
179 {
180  if (k_ != config.k_search)
181  {
182  k_ = config.k_search;
183  NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
184  }
185  if (search_radius_ != config.radius_search)
186  {
187  search_radius_ = config.radius_search;
188  NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
189  }
190 }
191 
193 void
195  const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
196 {
197  // No subscribers, no work
198  if (pub_output_.getNumSubscribers () <= 0)
199  return;
200 
201  // If cloud is given, check if it's valid
202  if (!isValid (cloud))
203  {
204  NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
205  emptyPublish (cloud);
206  return;
207  }
208 
209  // If surface is given, check if it's valid
210  if (cloud_surface && !isValid (cloud_surface, "surface"))
211  {
212  NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
213  emptyPublish (cloud);
214  return;
215  }
216 
217  // If indices are given, check if they are valid
218  if (indices && !isValid (indices))
219  {
220  NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
221  emptyPublish (cloud);
222  return;
223  }
224 
226  if (cloud_surface)
227  if (indices)
228  NODELET_DEBUG ("[input_surface_indices_callback]\n"
229  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
230  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
231  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
232  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
233  cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
234  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
235  else
236  NODELET_DEBUG ("[input_surface_indices_callback]\n"
237  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
238  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
239  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
240  cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
241 
242  else
243  if (indices)
244  NODELET_DEBUG ("[input_surface_indices_callback]\n"
245  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
246  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
247  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
248  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
249  else
250  NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
252 
253 
254  if ((int)(cloud->width * cloud->height) < k_)
255  {
256  NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height));
257  emptyPublish (cloud);
258  return;
259  }
260 
261  // If indices given...
262  IndicesPtr vindices;
263  if (indices && !indices->header.frame_id.empty ())
264  vindices.reset (new std::vector<int> (indices->indices));
265 
266  computePublish (cloud, cloud_surface, vindices);
267 }
268 
272 
274 void
276 {
277  // Call the super onInit ()
279 
280  // Call the child init
281  childInit (*pnh_);
282 
283  // Allow each individual class that inherits from us to declare their own Publisher
284  // This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
285  //pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
286 
287  // ---[ Mandatory parameters
288  if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
289  {
290  NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
291  return;
292  }
293  if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
294  {
295  NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
296  return;
297  }
298  // ---[ Optional parameters
299  pnh_->getParam ("use_surface", use_surface_);
300 
301  // Enable the dynamic reconfigure service
302  srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
303  dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
304  srv_->setCallback (f);
305 
306  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
307  " - use_surface : %s\n"
308  " - k_search : %d\n"
309  " - radius_search : %f\n"
310  " - spatial_locator: %d",
311  getName ().c_str (),
312  (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
313 
314  onInitPostProcess ();
315 }
316 
318 void
320 {
321  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
322  sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
323 
324  // Create the objects here
325  if (approximate_sync_)
326  sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
327  else
328  sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
329 
330  // If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
331  if (use_indices_ || use_surface_)
332  {
333  if (use_indices_)
334  {
335  // If indices are enabled, subscribe to the indices
336  sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
337  if (use_surface_) // Use both indices and surface
338  {
339  // If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
340  sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
341  if (approximate_sync_)
342  sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
343  else
344  sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
345  }
346  else // Use only indices
347  {
348  sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
349  if (approximate_sync_)
350  // surface not enabled, connect the input-indices duo and register
351  sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
352  else
353  // surface not enabled, connect the input-indices duo and register
354  sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
355  }
356  }
357  else // Use only surface
358  {
359  // indices not enabled, connect the input-surface duo and register
360  sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
361 
362  sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
363  if (approximate_sync_)
364  sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
365  else
366  sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
367  }
368  }
369  else
370  {
371  sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
372 
373  if (approximate_sync_)
374  sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
375  else
376  sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
377  }
378 
379  // Register callbacks
380  if (approximate_sync_)
381  sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
382  else
383  sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
384 }
385 
387 void
389 {
390  sub_input_filter_.unsubscribe ();
391  sub_normals_filter_.unsubscribe ();
392  if (use_indices_ || use_surface_)
393  {
394  if (use_indices_)
395  {
396  sub_indices_filter_.unsubscribe ();
397  if (use_surface_)
398  sub_surface_filter_.unsubscribe ();
399  }
400  else
401  sub_surface_filter_.unsubscribe ();
402  }
403 }
404 
406 void
408  const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
409  const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
410 {
411  // No subscribers, no work
412  if (pub_output_.getNumSubscribers () <= 0)
413  return;
414 
415  // If cloud+normals is given, check if it's valid
416  if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
417  {
418  NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
419  emptyPublish (cloud);
420  return;
421  }
422 
423  // If surface is given, check if it's valid
424  if (cloud_surface && !isValid (cloud_surface, "surface"))
425  {
426  NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
427  emptyPublish (cloud);
428  return;
429  }
430 
431  // If indices are given, check if they are valid
432  if (indices && !isValid (indices))
433  {
434  NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
435  emptyPublish (cloud);
436  return;
437  }
438 
440  if (cloud_surface)
441  if (indices)
442  NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
443  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
444  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
445  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
446  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
447  getName ().c_str (),
448  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
449  cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
450  cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
451  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
452  else
453  NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
454  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
455  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
456  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
457  getName ().c_str (),
458  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
459  cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
460  cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
461  else
462  if (indices)
463  NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
464  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
465  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
466  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
467  getName ().c_str (),
468  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
469  cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
470  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
471  else
472  NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
473  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
474  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
475  getName ().c_str (),
476  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
477  cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
479 
480  if ((int)(cloud->width * cloud->height) < k_)
481  {
482  NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
483  emptyPublish (cloud);
484  return;
485  }
486 
487  // If indices given...
488  IndicesPtr vindices;
489  if (indices && !indices->header.frame_id.empty ())
490  vindices.reset (new std::vector<int> (indices->indices));
491 
492  computePublish (cloud, cloud_normals, cloud_surface, vindices);
493 }
494 
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
pcl_ros::Feature::spatial_locator_type_
int spatial_locator_type_
Parameter for the spatial locator tree. By convention, the values represent: 0: ANN (Approximate Near...
Definition: feature.h:117
pcl_ros::Feature::use_surface_
bool use_surface_
Set to true if the nodelet needs to listen for incoming point clouds representing the search surface.
Definition: feature.h:110
pcl_ros::Feature::srv_
boost::shared_ptr< dynamic_reconfigure::Server< FeatureConfig > > srv_
Pointer to a dynamic reconfigure service.
Definition: feature.h:120
boost::shared_ptr< const PointCloudIn >
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
pcl_ros::Feature::input_surface_indices_callback
void input_surface_indices_callback(const PointCloudInConstPtr &cloud, const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices and use_surface are set.
Definition: feature.cpp:194
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback
void input_normals_surface_indices_callback(const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices and use_surface are set.
Definition: feature.cpp:407
pcl_ros::FeatureFromNormals::unsubscribe
virtual void unsubscribe()
Definition: feature.cpp:388
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
pcl_ros::Feature::IndicesPtr
pcl::IndicesPtr IndicesPtr
Definition: feature.h:75
f
f
feature.h
fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
pcl_ros::PCLNodelet::onInit
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
pcl_ros::Feature::PointCloudIn
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
Definition: feature.h:71
pcl_ros::Feature::unsubscribe
virtual void unsubscribe()
Definition: feature.cpp:158
pcl_ros::Feature::k_
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:97
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
pcl_ros::Feature::onInit
virtual void onInit()
Nodelet initialization routine.
Definition: feature.cpp:54
pcl_ros::Feature::subscribe
virtual void subscribe()
NodeletLazy connection routine.
Definition: feature.cpp:99
pcl_ros::FeatureFromNormals::subscribe
virtual void subscribe()
NodeletLazy connection routine.
Definition: feature.cpp:319
nodelet::Nodelet::getName
const std::string & getName() const
pcl_ros::Feature::childInit
virtual bool childInit(ros::NodeHandle &nh)=0
Child initialization routine. Internal method.
pcl_ros::Feature::input_callback
void input_callback(const PointCloudInConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
Definition: feature.h:149
pcl_ros::Feature::search_radius_
double search_radius_
The nearest neighbors search radius for each point.
Definition: feature.h:100
pcl_ros::FeatureFromNormals::onInit
virtual void onInit()
Nodelet initialization routine.
Definition: feature.cpp:275
NODELET_DEBUG
#define NODELET_DEBUG(...)
pcl_ros::Feature::config_callback
void config_callback(FeatureConfig &config, uint32_t level)
Dynamic reconfigure callback.
Definition: feature.cpp:178


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Fri Jul 12 2024 02:54:40