grid_sampler_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
37 #include <pcl/common/common.h>
38 
40 
41 namespace jsk_pcl_ros
42 {
44  {
45  ConnectionBasedNodelet::onInit();
46  pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output", 1);
47  dynamic_reconfigure::Server<Config>::CallbackType f =
48  boost::bind (&GridSampler::configCallback, this, _1, _2);
49  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
50  srv_->setCallback (f);
52  }
53 
55  {
56  sub_ = pnh_->subscribe("input", 1, &GridSampler::sample, this);
57  }
58 
60  {
61  sub_.shutdown();
62  }
63 
64  void GridSampler::configCallback(Config &config, uint32_t level)
65  {
66  boost::mutex::scoped_lock(mutex_);
67  if (config.grid_size == 0.0) {
68  NODELET_WARN("grid_size == 0.0 is prohibited");
69  return;
70  }
71  else {
72  grid_size_ = config.grid_size;
73  min_indices_ = config.min_indices;
74  }
75  }
76 
77  void GridSampler::sample(const sensor_msgs::PointCloud2::ConstPtr& msg)
78  {
79  boost::mutex::scoped_lock(mutex_);
80  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
81  pcl::fromROSMsg(*msg, *pcl_cloud);
82  Eigen::Vector4f minpt, maxpt;
83  pcl::getMinMax3D<pcl::PointXYZRGB>(*pcl_cloud, minpt, maxpt);
84  int x_bin_num = ceil((maxpt[0] - minpt[0]) / grid_size_);
85  int y_bin_num = ceil((maxpt[1] - minpt[1]) / grid_size_);
86  int z_bin_num = ceil((maxpt[2] - minpt[2]) / grid_size_);
87 
88  // x y z
89  std::map<int, std::map<int, std::map<int, std::vector<size_t > > > > grid;
90  for (size_t i = 0; i < pcl_cloud->points.size(); i++) {
91  pcl::PointXYZRGB point = pcl_cloud->points[i];
92  if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) {
93  // skip nan
94  continue;
95  }
96  int xbin = int((point.x - minpt[0]) / grid_size_);
97  int ybin = int((point.y - minpt[1]) / grid_size_);
98  int zbin = int((point.z - minpt[2]) / grid_size_);
99  std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit
100  = grid.find(xbin);
101  if (xit == grid.end()) { // cannot find x bin
102  NODELET_DEBUG_STREAM("no x bin" << xbin);
103  std::map<int, std::vector<size_t> > new_z;
104  std::vector<size_t> new_indices;
105  new_indices.push_back(i);
106  new_z[zbin] = new_indices;
107  std::map<int, std::map<int, std::vector<size_t> > > new_y;
108  new_y[ybin] = new_z;
109  grid[xbin] = new_y;
110  }
111  else {
112  NODELET_DEBUG_STREAM("found x bin" << xbin);
113  std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
114  std::map<int, std::map<int, std::vector<size_t> > >::iterator yit
115  = ybins.find(ybin);
116  if (yit == ybins.end()) { // cannot find y bin
117  NODELET_DEBUG_STREAM("no y bin" << ybin);
118  std::map<int, std::vector<size_t> > new_z;
119  std::vector<size_t> new_indices;
120  new_indices.push_back(i);
121  new_z[zbin] = new_indices;
122  xit->second[ybin] = new_z;
123  }
124  else {
125  NODELET_DEBUG_STREAM("found y bin" << ybin);
126  std::map<int, std::vector<size_t> > zbins = yit->second;
127  std::map<int, std::vector<size_t> >::iterator zit
128  = zbins.find(zbin);
129  if (zit == zbins.end()) {
130  NODELET_DEBUG_STREAM("no z bin" << zbin);
131  std::vector<size_t> new_indices;
132  new_indices.push_back(i);
133  xit->second[ybin][zbin] = new_indices;
134  }
135  else {
136  NODELET_DEBUG_STREAM("found z bin" << zbin);
137  xit->second[ybin][zbin].push_back(i);
138  }
139  }
140  }
141  }
142  // publish the result
143  jsk_recognition_msgs::ClusterPointIndices output;
144  output.header = msg->header;
145  for (std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit = grid.begin();
146  xit != grid.end();
147  xit++) {
148  std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
149  for (std::map<int, std::map<int, std::vector<size_t> > >::iterator yit = ybins.begin();
150  yit != ybins.end();
151  yit++) {
152  std::map<int, std::vector<size_t> > zbins = yit->second;
153  for (std::map<int, std::vector<size_t> >::iterator zit = zbins.begin();
154  zit != zbins.end();
155  zit++) {
156  std::vector<size_t> indices = zit->second;
157  NODELET_DEBUG_STREAM("size: " << indices.size());
158  if (indices.size() > min_indices_) {
159  PCLIndicesMsg ros_indices;
160  ros_indices.header = msg->header;
161  for (size_t j = 0; j < indices.size(); j++) {
162  ros_indices.indices.push_back(indices[j]);
163  }
164  output.cluster_indices.push_back(ros_indices);
165  }
166  }
167  }
168  }
169  pub_.publish(output);
170  }
171 }
172 
174 
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: grid_sampler.h:64
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
void output(int index, double value)
boost::shared_ptr< ros::NodeHandle > pnh_
#define NODELET_DEBUG_STREAM(...)
ros::Subscriber sub_
Definition: grid_sampler.h:62
virtual void configCallback(Config &config, uint32_t level)
virtual void sample(const sensor_msgs::PointCloud2::ConstPtr &msg)
jsk_pcl_ros::GridSamplerConfig Config
Definition: grid_sampler.h:52
pcl::PointIndices PCLIndicesMsg
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::GridSampler, nodelet::Nodelet)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46