convex_hull.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: convex_hull.hpp 32993 2010-09-30 23:08:57Z rusu $
35  *
36  */
37 
40 #include <geometry_msgs/PolygonStamped.h>
41 
43 void
45 {
47 
48  pub_output_ = advertise<PointCloud> (*pnh_, "output", max_queue_size_);
49  pub_plane_ = advertise<geometry_msgs::PolygonStamped> (*pnh_, "output_polygon", max_queue_size_);
50 
51  // ---[ Optional parameters
52  pnh_->getParam ("use_indices", use_indices_);
53 
54  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
55  " - use_indices : %s",
56  getName ().c_str (),
57  (use_indices_) ? "true" : "false");
58 
60 }
61 
63 void
65 {
66  // If we're supposed to look for PointIndices (indices)
67  if (use_indices_)
68  {
69  // Subscribe to the input using a filter
70  sub_input_filter_.subscribe (*pnh_, "input", 1);
71  // If indices are enabled, subscribe to the indices
72  sub_indices_filter_.subscribe (*pnh_, "indices", 1);
73 
74  if (approximate_sync_)
75  {
76  sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
77  // surface not enabled, connect the input-indices duo and register
78  sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
79  sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
80  }
81  else
82  {
83  sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
84  // surface not enabled, connect the input-indices duo and register
85  sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
86  sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
87  }
88  }
89  else
90  // Subscribe in an old fashion to input only (no filters)
91  sub_input_ = pnh_->subscribe<PointCloud> ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
92 }
93 
95 void
97 {
98  if (use_indices_)
99  {
100  sub_input_filter_.unsubscribe();
101  sub_indices_filter_.unsubscribe();
102  }
103  else
104  sub_input_.shutdown();
105 }
106 
108 void
110  const PointIndicesConstPtr &indices)
111 {
112  // No subscribers, no work
113  if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
114  return;
115 
116  PointCloud output;
117 
118  // If cloud is given, check if it's valid
119  if (!isValid (cloud))
120  {
121  NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
122  // Publish an empty message
123  output.header = cloud->header;
124  pub_output_.publish (ros_ptr(output.makeShared ()));
125  return;
126  }
127  // If indices are given, check if they are valid
128  if (indices && !isValid (indices, "indices"))
129  {
130  NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
131  // Publish an empty message
132  output.header = cloud->header;
133  pub_output_.publish (ros_ptr(output.makeShared ()));
134  return;
135  }
136 
138  if (indices)
139  NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
140  " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
141  " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
142  getName ().c_str (),
143  cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
144  indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
145  else
146  NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
147 
148  // Reset the indices and surface pointers
149  IndicesPtr indices_ptr;
150  if (indices)
151  indices_ptr.reset (new std::vector<int> (indices->indices));
152 
153  impl_.setInputCloud (pcl_ptr(cloud));
154  impl_.setIndices (indices_ptr);
155 
156  // Estimate the feature
157  impl_.reconstruct (output);
158 
159  // If more than 3 points are present, send a PolygonStamped hull too
160  if (output.points.size () >= 3)
161  {
162  geometry_msgs::PolygonStamped poly;
163  poly.header = fromPCL(output.header);
164  poly.polygon.points.resize (output.points.size ());
165  // Get three consecutive points (without copying)
166  pcl::Vector4fMap O = output.points[1].getVector4fMap ();
167  pcl::Vector4fMap B = output.points[0].getVector4fMap ();
168  pcl::Vector4fMap A = output.points[2].getVector4fMap ();
169  // Check the direction of points -- polygon must have CCW direction
170  Eigen::Vector4f OA = A - O;
171  Eigen::Vector4f OB = B - O;
172  Eigen::Vector4f N = OA.cross3 (OB);
173  double theta = N.dot (O);
174  bool reversed = false;
175  if (theta < (M_PI / 2.0))
176  reversed = true;
177  for (size_t i = 0; i < output.points.size (); ++i)
178  {
179  if (reversed)
180  {
181  size_t j = output.points.size () - i - 1;
182  poly.polygon.points[i].x = output.points[j].x;
183  poly.polygon.points[i].y = output.points[j].y;
184  poly.polygon.points[i].z = output.points[j].z;
185  }
186  else
187  {
188  poly.polygon.points[i].x = output.points[i].x;
189  poly.polygon.points[i].y = output.points[i].y;
190  poly.polygon.points[i].z = output.points[i].z;
191  }
192  }
193  pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
194  }
195  // Publish a Boost shared ptr const data
196  output.header = cloud->header;
197  pub_output_.publish (ros_ptr(output.makeShared ()));
198 }
199 
202 
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet_topic_tools::NodeletLazy::pnh_
boost::shared_ptr< ros::NodeHandle > pnh_
convex_hull.h
boost::shared_ptr
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
pcl_ros::PCLNodelet::use_indices_
bool use_indices_
Set to true if point indices are used.
Definition: pcl_nodelet.h:95
pcl_ros::ConvexHull2D::subscribe
virtual void subscribe()
LazyNodelet connection routine.
Definition: convex_hull.cpp:64
ConvexHull2D
pcl_ros::ConvexHull2D ConvexHull2D
Definition: convex_hull.cpp:200
pcl_ros::PCLNodelet::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_nodelet.h:84
pcl_ros::PCLNodelet::PointCloud
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: pcl_nodelet.h:78
pcl_ros::ConvexHull2D::onInit
virtual void onInit()
Nodelet initialization routine.
Definition: convex_hull.cpp:44
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::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::ConvexHull2D::input_indices_callback
void input_indices_callback(const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback.
Definition: convex_hull.cpp:109
nodelet::Nodelet
nodelet_topic_tools::NodeletLazy::onInitPostProcess
virtual void onInitPostProcess()
pcl::pcl_ptr
boost::shared_ptr< T > pcl_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:392
class_list_macros.hpp
nodelet::Nodelet::getName
const std::string & getName() const
pcl_ros::ConvexHull2D::unsubscribe
virtual void unsubscribe()
Definition: convex_hull.cpp:96
pcl_ros::ConvexHull2D
ConvexHull2D represents a 2D ConvexHull implementation.
Definition: convex_hull.h:53
pcl_ros::ConvexHull2D::pub_plane_
ros::Publisher pub_plane_
Publisher for PolygonStamped.
Definition: convex_hull.h:82
NODELET_DEBUG
#define NODELET_DEBUG(...)
pcl::ros_ptr
boost::shared_ptr< T > ros_ptr(const boost::shared_ptr< T > &p)
Definition: point_cloud.h:354


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