voxel_grid_downsample_decoder_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ryohei Ueda and JSK Lab
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/o2r other materials provided
16  * with the distribution.
17  * * Neither the name of the JSK Lab 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 
37 #include <pcl/point_types.h>
38 #include <pcl/filters/passthrough.h>
39 #include <pcl/filters/voxel_grid.h>
40 #include <pcl/io/io.h>
41 #include <pcl_ros/transforms.h>
42 
43 #include <string>
44 #include <iostream>
45 #include <boost/algorithm/string.hpp>
46 #include <boost/foreach.hpp>
47 #include <boost/lexical_cast.hpp>
48 
49 
50 namespace jsk_pcl_ros
51 {
52 
53  int VoxelGridDownsampleDecoder::getPointcloudID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input) {
54  return input->slice_index;
55  }
56 
57  int VoxelGridDownsampleDecoder::getPointcloudSequenceID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input) {
58  return input->sequence_id;
59  }
60 
61  std::string VoxelGridDownsampleDecoder::getPointcloudFrameId(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input) {
62  return input->point_cloud.header.frame_id;
63  }
64 
65  void VoxelGridDownsampleDecoder::pointCB(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
66  {
67  NODELET_INFO_STREAM("new pointcloud!" << input->point_cloud.header.frame_id);
68 
69  int id = getPointcloudID(input);
70  int new_sequence_id = getPointcloudSequenceID(input);
71  std::string frame_id = getPointcloudFrameId(input);
72  if (new_sequence_id != latest_sequence_id_) {
73  NODELET_INFO_STREAM("clearing pointcloud");
74  pc_buffer_.clear();
75  latest_sequence_id_ = new_sequence_id;
76  }
77 
78  // update the buffer
79  if (id >= (int)pc_buffer_.size()) {
80  // extend the buffer
81  //pc_buffer_.resize(id + 1);
82  int extend = id - pc_buffer_.size() + 1;
83  NODELET_INFO_STREAM("extend " << extend << " pointclouds");
84  for (int i = 0; i < extend; i++) {
85  NODELET_INFO_STREAM("new pointcloud allocation!");
86  pc_buffer_.push_back(jsk_recognition_msgs::SlicedPointCloudConstPtr());
87  pc_buffer_[pc_buffer_.size() - 1].reset();
88  }
89  }
90  //pc_buffer_.push_back(input);
91  NODELET_INFO_STREAM("id: " << id << " size: " << pc_buffer_.size());
92  pc_buffer_[id] = input;
93  if (pc_buffer_.size() == 1) {
94  // no need to do anything
95  }
96  else {
97  if (previous_id_ + 1 != id) { // the point cloud is not continuous
98  if (previous_id_ != (int)pc_buffer_.size() - 1) { // if not the last one
99  // make pc_buffer_ shorten
100  pc_buffer_.resize(previous_id_ + 1);
101  }
102  }
103  }
104  previous_id_ = id; // update the previous one
105  // publush the buffer
106  publishBuffer();
107 
108  }
109 
111  {
112  NODELET_INFO("publishBuffer");
113  if (pc_buffer_.size() == 0 || !pc_buffer_[0]) {
114  NODELET_WARN("no pointcloud is subscribed yet");
115  return;
116  }
117 
118  std::string result_frame_id = getPointcloudFrameId(pc_buffer_[0]);
119  pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenated_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
120  for (size_t i = 0; i < pc_buffer_.size(); i++)
121  {
122  if (!pc_buffer_[i]) {
123  NODELET_INFO_STREAM("buffer[" << i << "] is not yet available, skip it");
124  continue;
125  }
126  pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
127  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
128  fromROSMsg(pc_buffer_[i]->point_cloud, *tmp_cloud);
129  if (tmp_cloud->points.size() == 0) {
130  NODELET_INFO_STREAM("buffer[" << i << "] is not yet available, skip it");
131  continue;
132  }
133 
134 
135  tmp_cloud->header.frame_id = getPointcloudFrameId(pc_buffer_[i]);
136  // transform the pointcloud
137  pcl_ros::transformPointCloud(result_frame_id,
138  *tmp_cloud,
139  *transformed_tmp_cloud,
140  tf_listener);
141  // concatenate the tmp_cloud into concatenated_cloud
142  for (size_t j = 0; j < transformed_tmp_cloud->points.size(); j++) {
143  concatenated_cloud->points.push_back(transformed_tmp_cloud->points[j]);
144  }
145  }
146  sensor_msgs::PointCloud2 out;
147  toROSMsg(*concatenated_cloud, out);
148  out.header = pc_buffer_[0]->point_cloud.header;
149  out.header.frame_id = getPointcloudFrameId(pc_buffer_[0]);
150  pub_.publish(out);
151  }
152 
154  {
155  ConnectionBasedNodelet::onInit();
156  previous_id_ = -1;
157  // decoded output
158  pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
160  }
161 
163  {
164  sub_ = pnh_->subscribe("input", 1, &VoxelGridDownsampleDecoder::pointCB,
165  this);
166  }
167 
169  {
170  sub_.shutdown();
171  }
172 }
173 
174 
#define NODELET_INFO_STREAM(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
int getPointcloudSequenceID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
int getPointcloudID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
std::string getPointcloudFrameId(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
std::vector< jsk_recognition_msgs::SlicedPointCloudConstPtr > pc_buffer_
boost::shared_ptr< ros::NodeHandle > pnh_
void pointCB(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
#define NODELET_INFO(...)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::VoxelGridDownsampleDecoder, nodelet::Nodelet)


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