jsk_pcl_ros: voxel_grid_downsample_manager

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_manager.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 #include <boost/format.hpp>
00043 #include "jsk_recognition_msgs/SlicedPointCloud.h"
00044 
00045 namespace jsk_pcl_ros
00046 {
00047 
00048   void VoxelGridDownsampleManager::clearAll()
00049   {
00050     grid_.clear();
00051   }
00052   
00053   
00054   void VoxelGridDownsampleManager::pointCB(const sensor_msgs::PointCloud2ConstPtr &input)
00055   {
00056     try {
00057     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00058     pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00059     if (grid_.size() == 0) {
00060       JSK_NODELET_DEBUG("the number of registered grids is 0, skipping");
00061       return;
00062     }
00063     fromROSMsg(*input, *cloud);
00064     for (size_t i = 0; i < grid_.size(); i++)
00065     {
00066       visualization_msgs::Marker::ConstPtr target_grid = grid_[i];
00067       // not yet tf is supported
00068       int id = target_grid->id;
00069       // solve tf with ros::Time 0.0
00070 
00071       // transform pointcloud to the frame_id of target_grid
00072       pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00073       pcl_ros::transformPointCloud(target_grid->header.frame_id,
00074                                    *cloud,
00075                                    *transformed_cloud,
00076                                    *tf_listener);
00077       double center_x = target_grid->pose.position.x;
00078       double center_y = target_grid->pose.position.y;
00079       double center_z = target_grid->pose.position.z;
00080       double range_x = target_grid->scale.x * 1.0; // for debug
00081       double range_y = target_grid->scale.y * 1.0;
00082       double range_z = target_grid->scale.z * 1.0;
00083       double min_x = center_x - range_x / 2.0;
00084       double max_x = center_x + range_x / 2.0;
00085       double min_y = center_y - range_y / 2.0;
00086       double max_y = center_y + range_y / 2.0;
00087       double min_z = center_z - range_z / 2.0;
00088       double max_z = center_z + range_z / 2.0;
00089       double resolution = target_grid->color.r;
00090       // filter order: x -> y -> z -> downsample
00091       pcl::PassThrough<pcl::PointXYZRGB> pass_x;
00092       pass_x.setFilterFieldName("x");
00093       pass_x.setFilterLimits(min_x, max_x);
00094       
00095       pcl::PassThrough<pcl::PointXYZRGB> pass_y;
00096       pass_y.setFilterFieldName("y");
00097       pass_y.setFilterLimits(min_y, max_y);
00098       pcl::PassThrough<pcl::PointXYZRGB> pass_z;
00099       pass_z.setFilterFieldName("z");
00100       pass_z.setFilterLimits(min_z, max_z);
00101 
00102       JSK_NODELET_DEBUG_STREAM(id << " filter x: " << min_x << " - " << max_x);
00103       JSK_NODELET_DEBUG_STREAM(id << " filter y: " << min_y << " - " << max_y);
00104       JSK_NODELET_DEBUG_STREAM(id << " filter z: " << min_z << " - " << max_z);
00105       
00106       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_x (new pcl::PointCloud<pcl::PointXYZRGB>);
00107       pass_x.setInputCloud (transformed_cloud);
00108       pass_x.filter(*cloud_after_x);
00109 
00110       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_y (new pcl::PointCloud<pcl::PointXYZRGB>);
00111       pass_y.setInputCloud (cloud_after_x);
00112       pass_y.filter(*cloud_after_y);
00113 
00114       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_z (new pcl::PointCloud<pcl::PointXYZRGB>);
00115       pass_z.setInputCloud (cloud_after_y);
00116       pass_z.filter(*cloud_after_z);
00117 
00118       // downsample
00119       pcl::VoxelGrid<pcl::PointXYZRGB> sor;
00120       sor.setInputCloud (cloud_after_z);
00121       sor.setLeafSize (resolution, resolution, resolution);
00122       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
00123       sor.filter (*cloud_filtered);
00124 
00125       // reverse transform
00126       pcl::PointCloud<pcl::PointXYZRGB>::Ptr reverse_transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00127       pcl_ros::transformPointCloud(input->header.frame_id,
00128                                    *cloud_filtered,
00129                                    *reverse_transformed_cloud,
00130                                    *tf_listener);
00131       
00132       // adding the output into *output_cloud
00133       // tmp <- cloud_filtered + output_cloud
00134       // output_cloud <- tmp
00135       //pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZRGB>);
00136       //pcl::concatenatePointCloud (*cloud_filtered, *output_cloud, tmp);
00137       //output_cloud = tmp;
00138       JSK_NODELET_DEBUG_STREAM(id << " includes " << reverse_transformed_cloud->points.size() << " points");
00139       for (size_t i = 0; i < reverse_transformed_cloud->points.size(); i++) {
00140         output_cloud->points.push_back(reverse_transformed_cloud->points[i]);
00141       }
00142     }
00143     sensor_msgs::PointCloud2 out;
00144     toROSMsg(*output_cloud, out);
00145     out.header = input->header;
00146     pub_.publish(out);          // for debugging
00147 
00148     // for concatenater
00149     size_t cluster_num = output_cloud->points.size() / max_points_ + 1;
00150     JSK_NODELET_DEBUG_STREAM("encoding into " << cluster_num << " clusters");
00151     for (size_t i = 0; i < cluster_num; i++) {
00152       size_t start_index = max_points_ * i;
00153       size_t end_index = max_points_ * (i + 1) > output_cloud->points.size() ?
00154         output_cloud->points.size(): max_points_ * (i + 1);
00155       sensor_msgs::PointCloud2 cluster_out_ros;
00156       pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00157         cluster_out_pcl(new pcl::PointCloud<pcl::PointXYZRGB>);
00158       cluster_out_pcl->points.resize(end_index - start_index);
00159       // build cluster_out_pcl
00160       JSK_NODELET_DEBUG_STREAM("make cluster from " << start_index << " to " << end_index);
00161       for (size_t j = start_index; j < end_index; j++) {
00162         cluster_out_pcl->points[j - start_index] = output_cloud->points[j];
00163       }
00164       // conevrt cluster_out_pcl into ros msg
00165       toROSMsg(*cluster_out_pcl, cluster_out_ros);
00166       jsk_recognition_msgs::SlicedPointCloud publish_point_cloud;
00167       cluster_out_ros.header = input->header;
00168       publish_point_cloud.point_cloud = cluster_out_ros;
00169       publish_point_cloud.slice_index = i;
00170       publish_point_cloud.sequence_id = sequence_id_;
00171       pub_encoded_.publish(publish_point_cloud);
00172       ros::Duration(1.0 / rate_).sleep();
00173     }
00174     }
00175     catch (std::runtime_error e) { // catch any error
00176       JSK_NODELET_WARN_STREAM("error has occured in VoxelGridDownsampleManager but ignore it: " << e.what());
00177       ros::Duration(1.0 / rate_).sleep();
00178     }
00179   }
00180 
00181   void VoxelGridDownsampleManager::addGrid(const visualization_msgs::Marker::ConstPtr &new_box)
00182   {
00183     ++sequence_id_;
00184     // check we have new_box->id in our bounding_boxes_
00185     if (new_box->id == -1) {
00186       // cancel all
00187       JSK_NODELET_DEBUG("clear all pointclouds");
00188       clearAll();
00189     }
00190     else {
00191       for (size_t i = 0; i < grid_.size(); i++) {
00192         if (grid_[i]->id == new_box->id) {
00193           JSK_NODELET_DEBUG_STREAM("updating " << new_box->id << " grid");
00194           grid_[i] = new_box;
00195         }
00196       }
00197       JSK_NODELET_DEBUG_STREAM("adding new grid: " << new_box->id);
00198       grid_.push_back(new_box);
00199     }
00200   }
00201 
00202   void VoxelGridDownsampleManager::initializeGrid(void) {
00203     visualization_msgs::Marker::Ptr box (new visualization_msgs::Marker);
00204     box->header.stamp = ros::Time(0.0);
00205     box->header.frame_id = base_frame_;
00206     box->pose.position.x = 2.0;
00207     box->pose.position.y = 0.0;
00208     box->pose.position.z = -0.5;
00209     box->scale.x = 4.0;
00210     box->scale.y = 2.0;
00211     box->scale.z = 3.0;
00212     box->color.r = 0.05;
00213     box->color.g = 0.05;
00214     box->color.b = 0.05;
00215     box->color.a = 1.0;
00216     grid_.push_back(box);
00217   }
00218   
00219   void VoxelGridDownsampleManager::onInit(void)
00220   {
00221     ConnectionBasedNodelet::onInit();
00222     pnh_->param("base_frame", base_frame_, std::string("pelvis"));
00223     tf_listener = TfListenerSingleton::getInstance();
00224     initializeGrid();
00225     sequence_id_ = 0;
00226 
00227     int max_points_param;
00228     pnh_->param("max_points", max_points_param, 300);
00229     pnh_->param("rate", rate_, 1.0);
00230     max_points_  = max_points_param;
00231     
00232     pub_ = advertise<sensor_msgs::PointCloud2>(
00233       *pnh_, "output", 1);
00234     pub_encoded_ = advertise<jsk_recognition_msgs::SlicedPointCloud>(
00235       *pnh_, "output_encoded", 1);
00236     
00237   }
00238 
00239   void VoxelGridDownsampleManager::subscribe()
00240   {
00241     sub_ = pnh_->subscribe("input", 1, &VoxelGridDownsampleManager::pointCB,
00242                            this);
00243     bounding_box_sub_ = pnh_->subscribe("add_grid", 1, &VoxelGridDownsampleManager::addGrid,
00244                                         this);
00245   }
00246 
00247   void VoxelGridDownsampleManager::unsubscribe()
00248   {
00249     sub_.shutdown();
00250     bounding_box_sub_.shutdown();
00251   }
00252   
00253 }
00254 
00255 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::VoxelGridDownsampleManager,
00256                         nodelet::Nodelet);