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 "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:
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::PointXYZ> > (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
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::PointXYZ> cloud, cloud_filtered;
00097 pcl::fromROSMsg (*cloud2, cloud);
00098
00099 if (subsample_param_ != 0)
00100 {
00101 pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
00102
00103 grid_.setLeafSize (subsample_param_, subsample_param_, subsample_param_);
00104 grid_.setInputCloud (boost::make_shared <pcl::PointCloud<pcl::PointXYZ> > (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
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::PointXYZ> > *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::PointXYZ> 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 }