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 #include <pcl/io/io.h>
41 
43 void
45 {
47 
48  // ---[ Mandatory parameters
49  // The type of model to use (user given parameter).
50  int model_type;
51  if (!pnh_->getParam ("model_type", model_type))
52  {
53  NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
54  return;
55  }
56  // ---[ Optional parameters
57  // True if all data will be returned, false if only the projected inliers. Default: false.
58  bool copy_all_data = false;
59 
60  // True if all fields will be returned, false if only XYZ. Default: true.
61  bool copy_all_fields = true;
62 
63  pnh_->getParam ("copy_all_data", copy_all_data);
64  pnh_->getParam ("copy_all_fields", copy_all_fields);
65 
66  pub_output_ = advertise<PointCloud2> (*pnh_, "output", max_queue_size_);
67 
68  // Subscribe to the input using a filter
70 
71  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
72  " - model_type : %d\n"
73  " - copy_all_data : %s\n"
74  " - copy_all_fields : %s",
75  getName ().c_str (),
76  model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
77 
78  // Set given parameters here
79  impl_.setModelType (model_type);
80  impl_.setCopyAllFields (copy_all_fields);
81  impl_.setCopyAllData (copy_all_data);
82 
84 }
85 
87 void
89 {
90 /*
91  TODO : implement use_indices_
92  if (use_indices_)
93  {*/
94 
96 
98 
100  {
101  sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
103  sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
104  }
105  else
106  {
107  sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
109  sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
110  }
111 }
112 
114 void
116 {
117 /*
118  TODO : implement use_indices_
119  if (use_indices_)
120  {*/
121 
124 }
125 
127 void
128 pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
129  const PointIndicesConstPtr &indices,
130  const ModelCoefficientsConstPtr &model)
131 {
132  if (pub_output_.getNumSubscribers () <= 0)
133  return;
134 
135  if (!isValid (model) || !isValid (indices) || !isValid (cloud))
136  {
137  NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ());
138  return;
139  }
140 
141  NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
142  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
143  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n"
144  " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
145  getName ().c_str (),
146  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
147  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (),
148  model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ());
149 
150  tf_input_orig_frame_ = cloud->header.frame_id;
151 
152  IndicesPtr vindices;
153  if (indices)
154  vindices.reset (new std::vector<int> (indices->indices));
155 
156  model_ = model;
157  computePublish (cloud, vindices);
158 }
159 
162 
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:203
pcl::ProjectInliers< pcl::PCLPointCloud2 > impl_
The PCL filter implementation used.
void unsubscribe()
Lazy transport unsubscribe routine.
const std::string & getName() const
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud2, PointIndices, ModelCoefficients > > > sync_input_indices_model_a_
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
Definition: pcl_nodelet.h:121
std::string tf_input_orig_frame_
The original data input TF frame.
Definition: filter.h:89
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:140
boost::shared_ptr< ros::NodeHandle > pnh_
virtual void onInit()
Nodelet initialization routine.
ModelCoefficientsConstPtr model_
A pointer to the vector of model coefficients.
uint32_t getNumSubscribers() const
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:130
message_filters::Subscriber< PointCloud2 > sub_input_filter_
Definition: filter.h:71
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:127
ModelCoefficients::ConstPtr ModelCoefficientsConstPtr
Definition: pcl_nodelet.h:87
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:124
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.
#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:64
PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet)
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:83
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 Mon Jun 10 2019 14:19:18