$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 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/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage 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 00037 #include <ros/ros.h> 00038 #include <sstream> 00039 #include "robot_self_filter_color/self_see_filter_color.h" 00040 #include <tf/message_filter.h> 00041 #include <message_filters/subscriber.h> 00042 00043 #include <pcl/ros/conversions.h> 00044 #include <pcl/filters/voxel_grid.h> 00045 00046 class SelfFilter 00047 { 00048 public: 00049 SelfFilter (void): nh_ ("~") 00050 { 00051 nh_.param<std::string> ("sensor_frame", sensor_frame_, std::string ()); 00052 nh_.param<double> ("subsample_value", subsample_param_, 0.01); 00053 self_filter_ = new filters::SelfFilter<pcl::PointCloud<pcl::PointXYZRGB> > (nh_); 00054 00055 sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (root_handle_, "cloud_in", 1); 00056 mn_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*sub_, tf_, "", 1); 00057 00058 //mn_ = new tf::MessageNotifier<sensor_msgs::PointCloud2>(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1); 00059 pointCloudPublisher_ = root_handle_.advertise<sensor_msgs::PointCloud2>("cloud_out", 1); 00060 std::vector<std::string> frames; 00061 self_filter_->getSelfMask()->getLinkNames(frames); 00062 if (frames.empty()) 00063 { 00064 ROS_DEBUG ("No valid frames have been passed into the self filter. Using a callback that will just forward scans on."); 00065 no_filter_sub_ = root_handle_.subscribe<sensor_msgs::PointCloud2> ("cloud_in", 1, boost::bind(&SelfFilter::noFilterCallback, this, _1)); 00066 } 00067 else 00068 { 00069 ROS_DEBUG ("Valid frames were passed in. We'll filter them."); 00070 mn_->setTargetFrames (frames); 00071 mn_->registerCallback (boost::bind (&SelfFilter::cloudCallback, this, _1)); 00072 } 00073 } 00074 00075 ~SelfFilter (void) 00076 { 00077 delete self_filter_; 00078 delete mn_; 00079 delete sub_; 00080 } 00081 00082 private: 00083 void 00084 noFilterCallback (const sensor_msgs::PointCloud2ConstPtr &cloud) 00085 { 00086 pointCloudPublisher_.publish (cloud); 00087 ROS_DEBUG ("Self filter publishing unfiltered frame"); 00088 } 00089 00090 void cloudCallback (const sensor_msgs::PointCloud2ConstPtr &cloud2) 00091 { 00092 ROS_DEBUG ("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud2->header.stamp).toSec ()); 00093 std::vector<int> mask; 00094 ros::WallTime tm = ros::WallTime::now (); 00095 00096 pcl::PointCloud<pcl::PointXYZRGB> cloud, cloud_filtered; 00097 pcl::fromROSMsg (*cloud2, cloud); 00098 00099 if (subsample_param_ != 0) 00100 { 00101 pcl::PointCloud<pcl::PointXYZRGB> cloud_downsampled; 00102 // Set up the downsampling filter 00103 grid_.setLeafSize (subsample_param_, subsample_param_, subsample_param_); // 1cm leaf size 00104 grid_.setInputCloud (boost::make_shared <pcl::PointCloud<pcl::PointXYZRGB> > (cloud)); 00105 grid_.filter (cloud_downsampled); 00106 00107 self_filter_->updateWithSensorFrame (cloud_downsampled, cloud_filtered, sensor_frame_); 00108 } 00109 else 00110 { 00111 self_filter_->updateWithSensorFrame (cloud, cloud_filtered, sensor_frame_); 00112 } 00113 00114 double sec = (ros::WallTime::now() - tm).toSec (); 00115 00116 sensor_msgs::PointCloud2 out; 00117 pcl::toROSMsg (cloud_filtered, out); 00118 pointCloudPublisher_.publish (out); 00119 ROS_DEBUG ("Self filter: reduced %d points to %d points in %f seconds", (int)cloud.points.size(), (int)cloud_filtered.points.size (), sec); 00120 } 00121 00122 tf::TransformListener tf_; 00123 //tf::MessageNotifier<sensor_msgs::PointCloud> *mn_; 00124 ros::NodeHandle nh_, root_handle_; 00125 00126 tf::MessageFilter<sensor_msgs::PointCloud2> *mn_; 00127 message_filters::Subscriber<sensor_msgs::PointCloud2> *sub_; 00128 00129 filters::SelfFilter<pcl::PointCloud<pcl::PointXYZRGB> > *self_filter_; 00130 std::string sensor_frame_; 00131 double subsample_param_; 00132 00133 ros::Publisher pointCloudPublisher_; 00134 ros::Subscriber no_filter_sub_; 00135 00136 pcl::VoxelGrid<pcl::PointXYZRGB> grid_; 00137 }; 00138 00139 int 00140 main (int argc, char **argv) 00141 { 00142 ros::init (argc, argv, "self_filter"); 00143 00144 SelfFilter s; 00145 ros::spin (); 00146 00147 return (0); 00148 }