$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 "pr2_navigation_self_filter/self_see_filter.h" 00040 #include <tf/message_filter.h> 00041 #include <message_filters/subscriber.h> 00042 00043 00044 class SelfFilter 00045 { 00046 public: 00047 00048 SelfFilter(void): nh_("~") 00049 { 00050 nh_.param<std::string>("sensor_frame", sensor_frame_, std::string()); 00051 self_filter_ = new filters::SelfFilter<sensor_msgs::PointCloud>(nh_); 00052 00053 sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud>(root_handle_, "cloud_in", 1); 00054 mn_ = new tf::MessageFilter<sensor_msgs::PointCloud>(*sub_, tf_, "", 1); 00055 00056 //mn_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1); 00057 pointCloudPublisher_ = root_handle_.advertise<sensor_msgs::PointCloud>("cloud_out", 1); 00058 std::vector<std::string> frames; 00059 self_filter_->getSelfMask()->getLinkNames(frames); 00060 if(frames.empty()) 00061 { 00062 ROS_DEBUG("No valid frames have been passed into the self filter. Using a callback that will just forward scans on."); 00063 no_filter_sub_ = root_handle_.subscribe<sensor_msgs::PointCloud>("cloud_in", 1, boost::bind(&SelfFilter::noFilterCallback, this, _1)); 00064 } 00065 else 00066 { 00067 ROS_DEBUG("Valid frames were passed in. We'll filter them."); 00068 mn_->setTargetFrames(frames); 00069 mn_->registerCallback(boost::bind(&SelfFilter::cloudCallback, this, _1)); 00070 } 00071 } 00072 00073 ~SelfFilter(void) 00074 { 00075 delete self_filter_; 00076 delete mn_; 00077 delete sub_; 00078 } 00079 00080 private: 00081 00082 void noFilterCallback(const sensor_msgs::PointCloudConstPtr &cloud){ 00083 pointCloudPublisher_.publish(cloud); 00084 ROS_DEBUG("Self filter publishing unfiltered frame"); 00085 } 00086 00087 void cloudCallback(const sensor_msgs::PointCloudConstPtr &cloud) 00088 { 00089 sensor_msgs::PointCloud out; 00090 00091 ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec()); 00092 std::vector<int> mask; 00093 ros::WallTime tm = ros::WallTime::now(); 00094 00095 self_filter_->updateWithSensorFrame(*cloud, out, sensor_frame_); 00096 00097 double sec = (ros::WallTime::now() - tm).toSec(); 00098 00099 pointCloudPublisher_.publish(out); 00100 ROS_DEBUG("Self filter: reduced %d points to %d points in %f seconds", (int)cloud->points.size(), (int)out.points.size(), sec); 00101 } 00102 00103 tf::TransformListener tf_; 00104 //tf::MessageNotifier<sensor_msgs::PointCloud> *mn_; 00105 ros::NodeHandle nh_, root_handle_; 00106 00107 tf::MessageFilter<sensor_msgs::PointCloud> *mn_; 00108 message_filters::Subscriber<sensor_msgs::PointCloud> *sub_; 00109 00110 filters::SelfFilter<sensor_msgs::PointCloud> *self_filter_; 00111 std::string sensor_frame_; 00112 00113 ros::Publisher pointCloudPublisher_; 00114 ros::Subscriber no_filter_sub_; 00115 00116 }; 00117 00118 00119 int main(int argc, char **argv) 00120 { 00121 ros::init(argc, argv, "self_filter"); 00122 00123 SelfFilter s; 00124 ros::spin(); 00125 00126 return 0; 00127 }