voxel_grid_downsample_decoder_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "jsk_pcl_ros/voxel_grid_downsample_decoder.h"
00036 #include <pluginlib/class_list_macros.h>
00037 #include <pcl/point_types.h>
00038 #include <pcl/filters/passthrough.h>
00039 #include <pcl/filters/voxel_grid.h>
00040 #include <pcl/io/io.h>
00041 #include <pcl_ros/transforms.h>
00042 
00043 #include <string>
00044 #include <iostream>
00045 #include <boost/algorithm/string.hpp>
00046 #include <boost/foreach.hpp>
00047 #include <boost/lexical_cast.hpp>
00048 
00049 
00050 namespace jsk_pcl_ros
00051 {
00052 
00053   int VoxelGridDownsampleDecoder::getPointcloudID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input) {
00054     return input->slice_index;
00055   }
00056   
00057   int VoxelGridDownsampleDecoder::getPointcloudSequenceID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input) {
00058     return input->sequence_id;
00059   }
00060 
00061   std::string VoxelGridDownsampleDecoder::getPointcloudFrameId(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input) {
00062     return input->point_cloud.header.frame_id;
00063   }
00064   
00065   void VoxelGridDownsampleDecoder::pointCB(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
00066   {
00067     JSK_NODELET_INFO_STREAM("new pointcloud!" << input->point_cloud.header.frame_id);
00068     
00069     int id = getPointcloudID(input);
00070     int new_sequence_id = getPointcloudSequenceID(input);
00071     std::string frame_id = getPointcloudFrameId(input);
00072     if (new_sequence_id != latest_sequence_id_) {
00073       JSK_NODELET_INFO_STREAM("clearing pointcloud");
00074       pc_buffer_.clear();
00075       latest_sequence_id_ = new_sequence_id;
00076     }
00077     
00078     // update the buffer
00079     if (id >= (int)pc_buffer_.size()) {
00080       // extend the buffer
00081       //pc_buffer_.resize(id + 1);
00082       int extend = id - pc_buffer_.size() + 1;
00083       JSK_NODELET_INFO_STREAM("extend " << extend << " pointclouds");
00084       for (int i = 0; i < extend; i++) {
00085         JSK_NODELET_INFO_STREAM("new pointcloud allocation!");
00086         pc_buffer_.push_back(jsk_recognition_msgs::SlicedPointCloudConstPtr());
00087         pc_buffer_[pc_buffer_.size() - 1].reset();
00088       }
00089     }
00090     //pc_buffer_.push_back(input);
00091     JSK_NODELET_INFO_STREAM("id: " << id << " size: " << pc_buffer_.size());
00092     pc_buffer_[id] = input;
00093     if (pc_buffer_.size() == 1) {
00094       // no need to do anything
00095     }
00096     else {
00097       if (previous_id_ + 1 != id) { // the point cloud is not continuous
00098         if (previous_id_ != (int)pc_buffer_.size() - 1) { // if not the last one
00099           // make pc_buffer_ shorten
00100           pc_buffer_.resize(previous_id_ + 1);
00101         }
00102       }
00103     }
00104     previous_id_ = id;          // update the previous one
00105     // publush the buffer
00106     publishBuffer();
00107     
00108   }
00109 
00110   void VoxelGridDownsampleDecoder::publishBuffer(void)
00111   {
00112     JSK_NODELET_INFO("publishBuffer");
00113     if (pc_buffer_.size() == 0 || !pc_buffer_[0]) {
00114       JSK_NODELET_WARN("no pointcloud is subscribed yet");
00115       return;
00116     }
00117     
00118     std::string result_frame_id = getPointcloudFrameId(pc_buffer_[0]);
00119     pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenated_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00120     for (size_t i = 0; i < pc_buffer_.size(); i++)
00121     {
00122       if (!pc_buffer_[i]) {
00123         JSK_NODELET_INFO_STREAM("buffer[" << i << "] is not yet available, skip it");
00124         continue;
00125       }
00126       pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00127       pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00128       fromROSMsg(pc_buffer_[i]->point_cloud, *tmp_cloud);
00129       if (tmp_cloud->points.size() == 0) {
00130         JSK_NODELET_INFO_STREAM("buffer[" << i << "] is not yet available, skip it");
00131         continue;
00132       }
00133 
00134       
00135       tmp_cloud->header.frame_id = getPointcloudFrameId(pc_buffer_[i]);
00136       // transform the pointcloud
00137       pcl_ros::transformPointCloud(result_frame_id,
00138                                    *tmp_cloud,
00139                                    *transformed_tmp_cloud,
00140                                    tf_listener);
00141       // concatenate the tmp_cloud into concatenated_cloud
00142       for (size_t j = 0; j < transformed_tmp_cloud->points.size(); j++) {
00143         concatenated_cloud->points.push_back(transformed_tmp_cloud->points[j]);
00144       }
00145     }
00146     sensor_msgs::PointCloud2 out;
00147     toROSMsg(*concatenated_cloud, out);
00148     out.header = pc_buffer_[0]->point_cloud.header;
00149     out.header.frame_id = getPointcloudFrameId(pc_buffer_[0]);
00150     pub_.publish(out);
00151   }
00152   
00153   void VoxelGridDownsampleDecoder::onInit(void)
00154   {
00155     ConnectionBasedNodelet::onInit();
00156     previous_id_ = -1;
00157     // decoded output
00158     pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00159   }
00160 
00161   void VoxelGridDownsampleDecoder::subscribe()
00162   {
00163     sub_ = pnh_->subscribe("input", 1, &VoxelGridDownsampleDecoder::pointCB,
00164                            this);
00165   }
00166 
00167   void VoxelGridDownsampleDecoder::unsubscribe()
00168   {
00169     sub_.shutdown();
00170   }
00171 }
00172 
00173 
00174 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::VoxelGridDownsampleDecoder,
00175                         nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:48