extract_polygonal_prism_data.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: extract_polygonal_prism_data.hpp 32996 2010-09-30 23:42:11Z rusu $
35  *
36  */
37 
39 #include "pcl_ros/transforms.h"
41 
43 
46 
48 void
50 {
51  // Call the super onInit ()
53 
54  // Enable the dynamic reconfigure service
55  srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
56  dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
57  srv_->setCallback (f);
58 
59  // Advertise the output topics
60  pub_output_ = advertise<PointIndices> (*pnh_, "output", max_queue_size_);
61 
63 }
64 
66 void
68 {
69  sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
70  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
71 
72  // Create the objects here
73  if (approximate_sync_)
74  sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
75  else
76  sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
77 
78  if (use_indices_)
79  {
80  sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
81  if (approximate_sync_)
82  sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
83  else
84  sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
85  }
86  else
87  {
88  sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
89 
90  if (approximate_sync_)
91  sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
92  else
93  sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
94  }
95  // Register callbacks
96  if (approximate_sync_)
97  sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
98  else
99  sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
100 }
101 
103 void
105 {
106  sub_hull_filter_.unsubscribe ();
107  sub_input_filter_.unsubscribe ();
108 
109  if (use_indices_)
110  sub_indices_filter_.unsubscribe ();
111 }
112 
114 void
115 pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t /*level*/)
116 {
117  double height_min, height_max;
118  impl_.getHeightLimits (height_min, height_max);
119  if (height_min != config.height_min)
120  {
121  height_min = config.height_min;
122  NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
123  impl_.setHeightLimits (height_min, height_max);
124  }
125  if (height_max != config.height_max)
126  {
127  height_max = config.height_max;
128  NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
129  impl_.setHeightLimits (height_min, height_max);
130  }
131 }
132 
134 void
136  const PointCloudConstPtr &cloud,
137  const PointCloudConstPtr &hull,
138  const PointIndicesConstPtr &indices)
139 {
140  // No subscribers, no work
141  if (pub_output_.getNumSubscribers () <= 0)
142  return;
143 
144  // Copy the header (stamp + frame_id)
145  pcl_msgs::PointIndices inliers;
146  inliers.header = fromPCL(cloud->header);
147 
148  // If cloud is given, check if it's valid
149  if (!isValid (cloud) || !isValid (hull, "planar_hull"))
150  {
151  NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
152  pub_output_.publish (inliers);
153  return;
154  }
155  // If indices are given, check if they are valid
156  if (indices && !isValid (indices))
157  {
158  NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
159  pub_output_.publish (inliers);
160  return;
161  }
162 
164  if (indices)
165  NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
166  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
167  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
168  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
169  getName ().c_str (),
170  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 (),
171  hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
172  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
173  else
174  NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
175  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
176  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
177  getName ().c_str (),
178  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 (),
179  hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
181 
182  if (cloud->header.frame_id != hull->header.frame_id)
183  {
184  NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
185  PointCloud planar_hull;
186  if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
187  {
188  // Publish empty before return
189  pub_output_.publish (inliers);
190  return;
191  }
192  impl_.setInputPlanarHull (pcl_ptr(planar_hull.makeShared ()));
193  }
194  else
195  impl_.setInputPlanarHull (pcl_ptr(hull));
196 
197  IndicesPtr indices_ptr;
198  if (indices && !indices->header.frame_id.empty ())
199  indices_ptr.reset (new std::vector<int> (indices->indices));
200 
201  impl_.setInputCloud (pcl_ptr(cloud));
202  impl_.setIndices (indices_ptr);
203 
204  // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
205  if (!cloud->points.empty ()) {
206  pcl::PointIndices pcl_inliers;
207  moveToPCL(inliers, pcl_inliers);
208  impl_.segment (pcl_inliers);
209  moveFromPCL(pcl_inliers, inliers);
210  }
211  // Enforce that the TF frame and the timestamp are copied
212  inliers.header = fromPCL(cloud->header);
213  pub_output_.publish (inliers);
214  NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
215 }
216 
219 
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
boost::shared_ptr
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
extract_polygonal_prism_data.h
pcl_ros::ExtractPolygonalPrismData::unsubscribe
void unsubscribe()
Definition: extract_polygonal_prism_data.cpp:104
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::ExtractPolygonalPrismData::onInit
void onInit()
Nodelet initialization routine.
Definition: extract_polygonal_prism_data.cpp:49
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
f
f
pcl_ros::PCLNodelet::PointCloud
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: pcl_nodelet.h:78
transforms.h
fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
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::IndicesPtr
pcl::IndicesPtr IndicesPtr
Definition: pcl_nodelet.h:90
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::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Apply a rigid transform defined by a 3D offset and a quaternion.
Definition: transforms.hpp:85
pcl_conversions::moveToPCL
void moveToPCL(pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
pcl_ros::ExtractPolygonalPrismData::pub_output_
ros::Publisher pub_output_
The output PointIndices publisher.
Definition: extract_polygonal_prism_data.h:72
pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback
void input_hull_indices_callback(const PointCloudConstPtr &cloud, const PointCloudConstPtr &hull, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices is set.
Definition: extract_polygonal_prism_data.cpp:135
pcl_ros::ExtractPolygonalPrismData::srv_
boost::shared_ptr< dynamic_reconfigure::Server< ExtractPolygonalPrismDataConfig > > srv_
Pointer to a dynamic reconfigure service.
Definition: extract_polygonal_prism_data.h:82
pcl_ros::ExtractPolygonalPrismData::input_callback
void input_callback(const PointCloudConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
Definition: extract_polygonal_prism_data.h:93
nodelet::Nodelet
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
ExtractPolygonalPrismData
pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData
Definition: extract_polygonal_prism_data.cpp:217
pcl::pcl_ptr
boost::shared_ptr< T > pcl_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:392
class_list_macros.hpp
pcl_ros::ExtractPolygonalPrismData::subscribe
void subscribe()
LazyNodelet connection routine.
Definition: extract_polygonal_prism_data.cpp:67
pcl_ros::ExtractPolygonalPrismData::config_callback
void config_callback(ExtractPolygonalPrismDataConfig &config, uint32_t level)
Dynamic reconfigure callback.
Definition: extract_polygonal_prism_data.cpp:115
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
NODELET_DEBUG
#define NODELET_DEBUG(...)
pcl_ros::ExtractPolygonalPrismData
ExtractPolygonalPrismData uses a set of point indices that represent a planar model,...
Definition: extract_polygonal_prism_data.h:64
pcl_conversions.h


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