$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/self_see_filter.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: 00050 typedef pcl::PointCloud<pcl::PointXYZ> tCloudXYZ; 00052 typedef pcl::PointCloud<pcl::PointXYZRGB> tCloudRGB; 00053 00055 typedef filters::SelfFilter<tCloudXYZ> tSelfFilterXYZ; 00056 typedef filters::SelfFilter<tCloudRGB> tSelfFilterRGB; 00057 00058 public: 00059 SelfFilter (void): nh_ ("~") 00060 { 00061 // ROS_ERROR( "MY FILTER VERSION"); 00062 nh_.param<std::string> ("sensor_frame", sensor_frame_, std::string ()); 00063 nh_.param<double> ("subsample_value", subsample_param_, 0.01); 00064 00065 self_filter_xyz = new tSelfFilterXYZ(nh_); 00066 self_filter_rgb = new tSelfFilterRGB(nh_); 00067 00068 sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (root_handle_, "cloud_in", 1); 00069 mn_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*sub_, tf_, "", 1); 00070 00071 //mn_ = new tf::MessageNotifier<sensor_msgs::PointCloud2>(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1); 00072 pointCloudPublisher_ = root_handle_.advertise<sensor_msgs::PointCloud2>("cloud_out", 1); 00073 std::vector<std::string> frames; 00074 self_filter_xyz->getSelfMask()->getLinkNames(frames); 00075 self_filter_rgb->getSelfMask()->getLinkNames(frames); 00076 if (frames.empty()) 00077 { 00078 ROS_DEBUG ("No valid frames have been passed into the self filter. Using a callback that will just forward scans on."); 00079 no_filter_sub_ = root_handle_.subscribe<sensor_msgs::PointCloud2> ("cloud_in", 1, boost::bind(&SelfFilter::noFilterCallback, this, _1)); 00080 } 00081 else 00082 { 00083 ROS_DEBUG ("Valid frames were passed in. We'll filter them."); 00084 mn_->setTargetFrames (frames); 00085 mn_->registerCallback (boost::bind (&SelfFilter::cloudCallback, this, _1)); 00086 } 00087 } 00088 00089 ~SelfFilter (void) 00090 { 00091 delete self_filter_xyz; 00092 delete self_filter_rgb; 00093 delete mn_; 00094 delete sub_; 00095 } 00096 00100 bool isRGBCloud( const sensor_msgs::PointCloud2ConstPtr& cloud ) 00101 { 00102 sensor_msgs::PointCloud2::_fields_type::const_iterator i, end; 00103 00104 for( i = cloud->fields.begin(), end = cloud->fields.end(); i != end; ++i ) 00105 { 00106 if( i->name == "rgb" ) 00107 { 00108 return true; 00109 } 00110 } 00111 return false; 00112 } 00113 00114 private: 00115 void 00116 noFilterCallback (const sensor_msgs::PointCloud2ConstPtr &cloud) 00117 { 00118 pointCloudPublisher_.publish (cloud); 00119 ROS_DEBUG ("Self filter publishing unfiltered frame"); 00120 } 00121 00122 void cloudCallback (const sensor_msgs::PointCloud2ConstPtr &cloud2) 00123 { 00124 ROS_DEBUG ("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud2->header.stamp).toSec ()); 00125 std::vector<int> mask; 00126 ros::WallTime tm = ros::WallTime::now (); 00127 00128 sensor_msgs::PointCloud2 out; 00129 00130 if( !isRGBCloud(cloud2) ) 00131 { 00132 tCloudXYZ cloud, cloud_filtered; 00133 pcl::fromROSMsg (*cloud2, cloud); 00134 00135 if (subsample_param_ != 0) 00136 { 00137 tCloudXYZ cloud_downsampled; 00138 // Set up the downsampling filter 00139 grid_xyz.setLeafSize (subsample_param_, subsample_param_, subsample_param_); // 1cm leaf size 00140 grid_xyz.setInputCloud (boost::make_shared <tCloudXYZ> (cloud)); 00141 grid_xyz.filter (cloud_downsampled); 00142 00143 self_filter_xyz->updateWithSensorFrame (cloud_downsampled, cloud_filtered, sensor_frame_); 00144 } 00145 else 00146 { 00147 self_filter_xyz->updateWithSensorFrame (cloud, cloud_filtered, sensor_frame_); 00148 } 00149 00150 pcl::toROSMsg (cloud_filtered, out); 00151 double sec = (ros::WallTime::now() - tm).toSec (); 00152 ROS_DEBUG ("Self filter: reduced %d points to %d points in %f seconds", (int)cloud.points.size(), (int)cloud_filtered.points.size (), sec); 00153 00154 } 00155 else 00156 { 00157 // std::cerr << "IS RGB " << std::endl; 00158 00159 tCloudRGB cloud, cloud_filtered; 00160 pcl::fromROSMsg (*cloud2, cloud); 00161 00162 if (subsample_param_ != 0) 00163 { 00164 tCloudRGB cloud_downsampled; 00165 // Set up the downsampling filter 00166 grid_rgb.setLeafSize (subsample_param_, subsample_param_, subsample_param_); // 1cm leaf size 00167 grid_rgb.setInputCloud (boost::make_shared <tCloudRGB> (cloud)); 00168 grid_rgb.filter (cloud_downsampled); 00169 00170 self_filter_rgb->updateWithSensorFrame (cloud_downsampled, cloud_filtered, sensor_frame_); 00171 } 00172 else 00173 { 00174 self_filter_rgb->updateWithSensorFrame (cloud, cloud_filtered, sensor_frame_); 00175 } 00176 00177 pcl::toROSMsg (cloud_filtered, out); 00178 double sec = (ros::WallTime::now() - tm).toSec (); 00179 ROS_DEBUG ("Self filter: reduced %d points to %d points in %f seconds", (int)cloud.points.size(), (int)cloud_filtered.points.size (), sec); 00180 00181 } 00182 pointCloudPublisher_.publish (out); 00183 } 00184 00185 tf::TransformListener tf_; 00186 //tf::MessageNotifier<sensor_msgs::PointCloud> *mn_; 00187 ros::NodeHandle nh_, root_handle_; 00188 00189 tf::MessageFilter<sensor_msgs::PointCloud2> *mn_; 00190 message_filters::Subscriber<sensor_msgs::PointCloud2> *sub_; 00191 00192 // Two possible self filters 00193 tSelfFilterXYZ *self_filter_xyz; 00194 tSelfFilterRGB *self_filter_rgb; 00195 00196 std::string sensor_frame_; 00197 double subsample_param_; 00198 00199 ros::Publisher pointCloudPublisher_; 00200 ros::Subscriber no_filter_sub_; 00201 00202 pcl::VoxelGrid<pcl::PointXYZ> grid_xyz; 00203 pcl::VoxelGrid<pcl::PointXYZRGB> grid_rgb; 00204 }; 00205 00206 int 00207 main (int argc, char **argv) 00208 { 00209 ros::init (argc, argv, "self_filter"); 00210 00211 SelfFilter s; 00212 ros::spin (); 00213 00214 return (0); 00215 }