Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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<robot_self_filter::PointCloud>(nh_);
00052
00053 sub_ = new message_filters::Subscriber<robot_self_filter::PointCloud>(root_handle_, "cloud_in", 1);
00054 mn_ = new tf::MessageFilter<robot_self_filter::PointCloud>(*sub_, tf_, "", 1);
00055
00056
00057 pointCloudPublisher_ = root_handle_.advertise<robot_self_filter::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<robot_self_filter::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 robot_self_filter::PointCloud::ConstPtr &cloud){
00083 pointCloudPublisher_.publish(cloud);
00084 ROS_DEBUG("Self filter publishing unfiltered frame");
00085 }
00086
00087 void cloudCallback(const robot_self_filter::PointCloud::ConstPtr &cloud)
00088 {
00089 robot_self_filter::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
00105 ros::NodeHandle nh_, root_handle_;
00106
00107 tf::MessageFilter<robot_self_filter::PointCloud> *mn_;
00108 message_filters::Subscriber<robot_self_filter::PointCloud> *sub_;
00109
00110 filters::SelfFilter<robot_self_filter::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 }