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


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Mon Jun 10 2019 14:19:18