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 class SelfFilter
00044 {
00045 public:
00046
00047 SelfFilter(void): nh_("~"), subscribing_(false)
00048 {
00049 nh_.param<std::string>("sensor_frame", sensor_frame_, std::string());
00050 nh_.param("use_rgb", use_rgb_, false);
00051 if (use_rgb_)
00052 {
00053 self_filter_rgb_ = new filters::SelfFilter<pcl::PointXYZRGB>(nh_);
00054 }
00055 else
00056 {
00057 self_filter_ = new filters::SelfFilter<pcl::PointXYZ>(nh_);
00058 }
00059 ros::SubscriberStatusCallback connect_cb
00060 = boost::bind( &SelfFilter::connectionCallback, this, _1);
00061
00062 if (use_rgb_)
00063 {
00064 self_filter_rgb_->getSelfMask()->getLinkNames(frames_);
00065 }
00066 else
00067 {
00068 self_filter_->getSelfMask()->getLinkNames(frames_);
00069 }
00070 pointCloudPublisher_ = root_handle_.advertise<sensor_msgs::PointCloud2>("cloud_out", 1,
00071 connect_cb, connect_cb);
00072 }
00073
00074 ~SelfFilter(void)
00075 {
00076 if (self_filter_)
00077 {
00078 delete self_filter_;
00079 }
00080 if (self_filter_rgb_)
00081 {
00082 delete self_filter_rgb_;
00083 }
00084 }
00085
00086 private:
00087
00088 void connectionCallback(const ros::SingleSubscriberPublisher& pub)
00089 {
00090 if (pointCloudPublisher_.getNumSubscribers() > 0) {
00091 if (!subscribing_) {
00092 subscribe();
00093 subscribing_ = true;
00094 }
00095 }
00096 else {
00097 if (subscribing_) {
00098 unsubscribe();
00099 subscribing_ = false;
00100 }
00101 }
00102 }
00103
00104 void subscribe() {
00105 if(frames_.empty())
00106 {
00107 ROS_DEBUG("No valid frames have been passed into the self filter. Using a callback that will just forward scans on.");
00108 no_filter_sub_ = root_handle_.subscribe<sensor_msgs::PointCloud2>("cloud_in", 1, boost::bind(&SelfFilter::noFilterCallback, this, _1));
00109 }
00110 else
00111 {
00112 ROS_DEBUG("Valid frames were passed in. We'll filter them.");
00113 sub_.subscribe(root_handle_, "cloud_in", 1);
00114 mn_.reset(new tf::MessageFilter<sensor_msgs::PointCloud2>(sub_, tf_, "", 1));
00115 mn_->setTargetFrames(frames_);
00116 mn_->registerCallback(boost::bind(&SelfFilter::cloudCallback, this, _1));
00117 }
00118 }
00119
00120 void unsubscribe() {
00121 if (frames_.empty()) {
00122 no_filter_sub_.shutdown();
00123 }
00124 else {
00125 sub_.unsubscribe();
00126 }
00127 }
00128
00129 void noFilterCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud){
00130 pointCloudPublisher_.publish(cloud);
00131 ROS_DEBUG("Self filter publishing unfiltered frame");
00132 }
00133
00134 void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud2)
00135 {
00136 ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud2->header.stamp).toSec());
00137 std::vector<int> mask;
00138 ros::WallTime tm = ros::WallTime::now();
00139
00140
00141 sensor_msgs::PointCloud2 out2;
00142 int input_size = 0;
00143 int output_size = 0;
00144 if (use_rgb_)
00145 {
00146 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00147 pcl::fromROSMsg(*cloud2, *cloud);
00148 pcl::PointCloud<pcl::PointXYZRGB> out;
00149 self_filter_rgb_->updateWithSensorFrame(*cloud, out, sensor_frame_);
00150 pcl::toROSMsg(out, out2);
00151 input_size = cloud->points.size();
00152 output_size = out.points.size();
00153 }
00154 else
00155 {
00156 typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00157 pcl::fromROSMsg(*cloud2, *cloud);
00158 pcl::PointCloud<pcl::PointXYZ> out;
00159 self_filter_->updateWithSensorFrame(*cloud, out, sensor_frame_);
00160 pcl::toROSMsg(out, out2);
00161 input_size = cloud->points.size();
00162 output_size = out.points.size();
00163 }
00164
00165 double sec = (ros::WallTime::now() - tm).toSec();
00166 pointCloudPublisher_.publish(out2);
00167 ROS_DEBUG("Self filter: reduced %d points to %d points in %f seconds", input_size, output_size, sec);
00168
00169 }
00170
00171 tf::TransformListener tf_;
00172
00173 ros::NodeHandle nh_, root_handle_;
00174
00175 boost::shared_ptr<tf::MessageFilter<sensor_msgs::PointCloud2> > mn_;
00176 message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
00177
00178 filters::SelfFilter<pcl::PointXYZ> *self_filter_;
00179 filters::SelfFilter<pcl::PointXYZRGB> *self_filter_rgb_;
00180 std::string sensor_frame_;
00181 bool use_rgb_;
00182 bool subscribing_;
00183 std::vector<std::string> frames_;
00184
00185 ros::Publisher pointCloudPublisher_;
00186 ros::Subscriber no_filter_sub_;
00187
00188 };
00189
00190
00191 int main(int argc, char **argv)
00192 {
00193 ros::init(argc, argv, "self_filter");
00194
00195 ros::NodeHandle nh("~");
00196 SelfFilter s;
00197 ros::spin();
00198
00199 return 0;
00200 }