organize_pointcloud_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 
00036 #include "jsk_pcl_ros/organize_pointcloud.h"
00037 #include <pluginlib/class_list_macros.h>
00038 
00039 #include <pcl/common/centroid.h>
00040 
00041 namespace jsk_pcl_ros
00042 {
00043   void OrganizePointCloud::extract(const sensor_msgs::PointCloud2ConstPtr &input)
00044   {
00045     // skip empty cloud
00046     JSK_ROS_INFO_STREAM("received input clouds, convert range image, resolution: " << angular_resolution << ", width(deg): " << angle_width << ", height(deg):" << angle_height << ", min_points:" << min_points);
00047 
00048     if ( input->width < min_points ) return;
00049 
00050     pcl::PointCloud<pcl::PointXYZ> pointCloud;
00051     pcl::fromROSMsg(*input, pointCloud);
00052 
00053     // We now want to create a range image from the above point cloud, with a 1deg angular resolution
00054     float angularResolution = (float) (angular_resolution * (M_PI/180.0f));  //   1.0 degree in radians
00055     float maxAngleWidth     = (float) (angle_width * (M_PI/180.0f));  // 120.0 degree in radians
00056     float maxAngleHeight    = (float) (angle_height * (M_PI/180.0f));  // 90.0 degree in radians
00057     Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
00058     pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
00059     float noiseLevel=0.00;
00060     float minRange = 0.0f;
00061     int borderSize = 1;
00062 
00063     pcl::RangeImage rangeImage;
00064     rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,
00065                                     sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
00066     JSK_ROS_INFO_STREAM("input image size " << input->width << " x " << input->height << "(=" << input->width * input->height << ")");
00067     JSK_ROS_INFO_STREAM("output image size " << rangeImage.width << " x " << rangeImage.height << "(=" << rangeImage.width * rangeImage.height << ")");
00068     JSK_ROS_DEBUG_STREAM(rangeImage);
00069 
00070     sensor_msgs::PointCloud2 out;
00071     pcl::toROSMsg(rangeImage, out);
00072     out.header = input->header;
00073     pub_.publish(out);
00074   }
00075 
00076   void OrganizePointCloud::onInit(void)
00077   {
00078     ConnectionBasedNodelet::onInit();
00079     
00080     pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00081     pnh_->param<double>("angular_resolution", angular_resolution, 1.0);
00082     pnh_->param<double>("angle_width", angle_width, 120.0);
00083     pnh_->param<double>("angle_height", angle_height, 90.0);
00084     pnh_->param<int>("min_points", min_points, 1000);
00085   }
00086 
00087   void OrganizePointCloud::subscribe()
00088   {
00089     sub_ = pnh_->subscribe("input", 1, &OrganizePointCloud::extract, this);
00090   }
00091 
00092   void OrganizePointCloud::unsubscribe()
00093   {
00094     sub_.shutdown();
00095   }
00096 }
00097 
00098 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::OrganizePointCloud, nodelet::Nodelet);
00099 


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