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 
94  sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
95 
96  sub_model_.subscribe (*pnh_, "model", max_queue_size_);
97 
98  if (approximate_sync_)
99  {
100  sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
101  sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_);
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_);
107  sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_);
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 
121  sub_input_filter_.unsubscribe ();
122  sub_model_.unsubscribe ();
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 
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::Filter::IndicesPtr
pcl::IndicesPtr IndicesPtr
Definition: filter.h:61
pcl_ros::ProjectInliers::unsubscribe
void unsubscribe()
Lazy transport unsubscribe routine.
Definition: project_inliers.cpp:114
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
ProjectInliers
pcl_ros::ProjectInliers ProjectInliers
Definition: project_inliers.cpp:159
project_inliers.h
pcl_ros::PCLNodelet::max_queue_size_
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:128
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
pcl_ros::PCLNodelet::pub_output_
ros::Publisher pub_output_
The output PointCloud publisher.
Definition: pcl_nodelet.h:125
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::PCLNodelet::ModelCoefficientsConstPtr
ModelCoefficients::ConstPtr ModelCoefficientsConstPtr
Definition: pcl_nodelet.h:88
pcl_ros::ProjectInliers
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a sepa...
Definition: project_inliers.h:56
pcl_ros::ProjectInliers::impl_
pcl::ProjectInliers< pcl::PCLPointCloud2 > impl_
The PCL filter implementation used.
Definition: project_inliers.h:94
pcl_ros::Filter::sub_input_filter_
message_filters::Subscriber< PointCloud2 > sub_input_filter_
Definition: filter.h:70
message_filters::Subscriber::subscribe
void subscribe()
pcl_ros::ProjectInliers::subscribe
void subscribe()
NodeletLazy connection routine.
Definition: project_inliers.cpp:87
nodelet::Nodelet
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
class_list_macros.hpp
nodelet::Nodelet::getName
const std::string & getName() const
pcl_ros::ProjectInliers::onInit
virtual void onInit()
Nodelet initialization routine.
Definition: project_inliers.cpp:43
pcl_ros::ProjectInliers::input_indices_model_callback
void input_indices_model_callback(const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model)
PointCloud2 + Indices + Model data callback.
Definition: project_inliers.cpp:127
NODELET_DEBUG
#define NODELET_DEBUG(...)


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