/home/laradmin/lar/perception/pointcloud_segmentation/src/laser_to_pc.cpp
Go to the documentation of this file.00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without modification, are permitted 00008 provided that the following conditions are met: 00009 00010 *Redistributions of source code must retain the above copyright notice, this list of 00011 conditions and the following disclaimer. 00012 *Redistributions in binary form must reproduce the above copyright notice, this list of 00013 conditions and the following disclaimer in the documentation and/or other materials provided 00014 with the distribution. 00015 *Neither the name of the University of Aveiro nor the names of its contributors may be used to 00016 endorse or promote products derived from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 00019 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 00020 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 00021 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00022 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00023 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 00024 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 00025 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 ***************************************************************************************************/ 00032 #include <pointcloud_segmentation/laser_to_pc.h> 00033 00034 void laser_to_pc::filter (const sensor_msgs::LaserScanPtr& msg) 00035 { 00036 PointCloud<PointXYZ> laser_cloud; 00037 sensor_msgs::PointCloud2 laser; // laser pointcloud out 00038 laser.header.frame_id=msg->header.frame_id; 00039 tf::TransformListener listener; // not sure why i need this but it is ok :-) 00040 00041 tf::StampedTransform transform; 00042 try 00043 { 00044 listener_ptr->lookupTransform(frame_id, msg->header.frame_id, ros::Time(0), transform); 00045 } 00046 catch (tf::TransformException ex) 00047 { 00048 ROS_ERROR("%s",ex.what()); 00049 } 00050 00051 laser_geometry::LaserProjection projector_; 00052 projector_.transformLaserScanToPointCloud(msg->header.frame_id,*msg,laser,listener); 00053 00054 fromROSMsg(laser,laser_cloud); 00055 laser_cloud.header.frame_id=msg->header.frame_id; 00056 00057 // Transform laser scan 00058 PointCloud<PointXYZ> laser_cloud_transformed; 00059 pcl_ros::transformPointCloud (laser_cloud,laser_cloud_transformed,transform); 00060 00061 // Cut the cloud 00062 points_from_volume<PointXYZ> pfv; 00063 PointCloud<PointXYZ> cloud_cut; 00064 cloud_cut.header.frame_id=frame_id; 00065 convex_hull.header.frame_id=frame_id; 00066 laser_cloud_transformed.header.frame_id=frame_id; 00067 pfv.set_convex_hull(convex_hull); 00068 pfv.convexhull_function(laser_cloud_transformed, max_z,min_z, false); 00069 cloud_cut=pfv.get_pc_in_volume(); 00070 00071 sensor_msgs::PointCloud2 msg_laser; 00072 toROSMsg(cloud_cut, msg_laser); 00073 00074 // Publish the result 00075 pub_ptr->publish(msg_laser); 00076 00077 };