project_inliers.cpp
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: project_inliers.cpp 35876 2011-02-09 01:04:36Z rusu $
35  *
36  */
37 
40 
42 void
44 {
46 
47  // ---[ Mandatory parameters
48  // The type of model to use (user given parameter).
49  int model_type;
50  if (!pnh_->getParam ("model_type", model_type))
51  {
52  NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
53  return;
54  }
55  // ---[ Optional parameters
56  // True if all data will be returned, false if only the projected inliers. Default: false.
57  bool copy_all_data = false;
58 
59  // True if all fields will be returned, false if only XYZ. Default: true.
60  bool copy_all_fields = true;
61 
62  pnh_->getParam ("copy_all_data", copy_all_data);
63  pnh_->getParam ("copy_all_fields", copy_all_fields);
64 
65  pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
66 
67  // Subscribe to the input using a filter
69 
70  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
71  " - model_type : %d\n"
72  " - copy_all_data : %s\n"
73  " - copy_all_fields : %s",
74  getName ().c_str (),
75  model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
76 
77  // Set given parameters here
78  impl_.setModelType (model_type);
79  impl_.setCopyAllFields (copy_all_fields);
80  impl_.setCopyAllData (copy_all_data);
81 
83 }
84 
86 void
88 {
89 /*
90  TODO : implement use_indices_
91  if (use_indices_)
92  {*/
93 
95 
97 
99  {
100  sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
102  sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
103  }
104  else
105  {
106  sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
108  sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
109  }
110 }
111 
113 void
115 {
116 /*
117  TODO : implement use_indices_
118  if (use_indices_)
119  {*/
120 
123 }
124 
126 void
127 pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
128  const PointIndicesConstPtr &indices,
129  const ModelCoefficientsConstPtr &model)
130 {
131  if (pub_output_.getNumSubscribers () <= 0)
132  return;
133 
134  if (!isValid (model) || !isValid (indices) || !isValid (cloud))
135  {
136  NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ());
137  return;
138  }
139 
140  NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
141  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
142  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n"
143  " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
144  getName ().c_str (),
145  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
146  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (),
147  model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ());
148 
149  tf_input_orig_frame_ = cloud->header.frame_id;
150 
151  IndicesPtr vindices;
152  if (indices)
153  vindices.reset (new std::vector<int> (indices->indices));
154 
155  model_ = model;
156  computePublish (cloud, vindices);
157 }
158 
161 
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
#define NODELET_ERROR(...)
message_filters::Subscriber< ModelCoefficients > sub_model_
The message filter subscriber for model coefficients.
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:204
pcl::ProjectInliers< pcl::PCLPointCloud2 > impl_
The PCL filter implementation used.
void unsubscribe()
Lazy transport unsubscribe routine.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud2, PointIndices, ModelCoefficients > > > sync_input_indices_model_a_
pcl::IndicesPtr IndicesPtr
Definition: filter.h:61
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:122
std::string tf_input_orig_frame_
The original data input TF frame.
Definition: filter.h:88
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud2, PointIndices, ModelCoefficients > > > sync_input_indices_model_e_
Synchronized input, indices, and model coefficients.
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
boost::shared_ptr< ros::NodeHandle > pnh_
virtual void onInit()
Nodelet initialization routine.
ModelCoefficientsConstPtr model_
A pointer to the vector of model coefficients.
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
Definition: pcl_nodelet.h:131
message_filters::Subscriber< PointCloud2 > sub_input_filter_
Definition: filter.h:70
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 input_indices_model_callback(const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model)
PointCloud2 + Indices + Model data callback.
void subscribe()
NodeletLazy connection routine.
uint32_t getNumSubscribers() const
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define NODELET_DEBUG(...)
void computePublish(const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
Call the child filter () method, optionally transform the result, and publish it. ...
Definition: filter.cpp:63
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a sepa...


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