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 #include "ros/ros.h"
00032 #include "sensor_msgs/LaserScan.h"
00033 #include "message_filters/subscriber.h"
00034 #include "tf/message_filter.h"
00035 #include "tf/transform_listener.h"
00036 #include "filters/filter_chain.h"
00037
00038 class ScanToScanFilterChain
00039 {
00040 protected:
00041
00042 ros::NodeHandle nh_;
00043 ros::NodeHandle private_nh_;
00044
00045
00046 tf::TransformListener *tf_;
00047 message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
00048 tf::MessageFilter<sensor_msgs::LaserScan> *tf_filter_;
00049 double tf_filter_tolerance_;
00050
00051
00052 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
00053
00054
00055 sensor_msgs::LaserScan msg_;
00056 ros::Publisher output_pub_;
00057
00058
00059 ros::Timer deprecation_timer_;
00060 bool using_filter_chain_deprecated_;
00061
00062 public:
00063
00064 ScanToScanFilterChain() :
00065 private_nh_("~"),
00066 scan_sub_(nh_, "scan", 50),
00067 tf_(NULL),
00068 tf_filter_(NULL),
00069 filter_chain_("sensor_msgs::LaserScan")
00070 {
00071
00072
00073 using_filter_chain_deprecated_ = private_nh_.hasParam("filter_chain");
00074
00075 if (using_filter_chain_deprecated_)
00076 filter_chain_.configure("filter_chain", private_nh_);
00077 else
00078 filter_chain_.configure("scan_filter_chain", private_nh_);
00079
00080 std::string tf_message_filter_target_frame;
00081
00082 if (private_nh_.hasParam("tf_message_filter_target_frame"))
00083 {
00084 private_nh_.getParam("tf_message_filter_target_frame", tf_message_filter_target_frame);
00085
00086 private_nh_.param("tf_message_filter_tolerance", tf_filter_tolerance_, 0.03);
00087
00088 tf_ = new tf::TransformListener();
00089 tf_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(scan_sub_, *tf_, "", 50);
00090 tf_filter_->setTargetFrame(tf_message_filter_target_frame);
00091 tf_filter_->setTolerance(ros::Duration(tf_filter_tolerance_));
00092
00093
00094 tf_filter_->registerCallback(boost::bind(&ScanToScanFilterChain::callback, this, _1));
00095 }
00096 else
00097 {
00098
00099 scan_sub_.registerCallback(boost::bind(&ScanToScanFilterChain::callback, this, _1));
00100 }
00101
00102
00103 output_pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan_filtered", 1000);
00104
00105
00106 deprecation_timer_ = nh_.createTimer(ros::Duration(5.0), boost::bind(&ScanToScanFilterChain::deprecation_warn, this, _1));
00107 }
00108
00109
00110 ~ScanToScanFilterChain()
00111 {
00112 if (tf_filter_)
00113 delete tf_filter_;
00114 if (tf_)
00115 delete tf_;
00116 }
00117
00118
00119 void deprecation_warn(const ros::TimerEvent& e)
00120 {
00121 if (using_filter_chain_deprecated_)
00122 ROS_WARN("Use of '~filter_chain' parameter in scan_to_scan_filter_chain has been deprecated. Please replace with '~scan_filter_chain'.");
00123 }
00124
00125
00126 void callback(const sensor_msgs::LaserScan::ConstPtr& msg_in)
00127 {
00128
00129 if (filter_chain_.update(*msg_in, msg_))
00130 {
00131
00132 output_pub_.publish(msg_);
00133 }
00134 }
00135 };
00136
00137 int main(int argc, char **argv)
00138 {
00139 ros::init(argc, argv, "scan_to_scan_filter_chain");
00140
00141 ScanToScanFilterChain t;
00142 ros::spin();
00143
00144 return 0;
00145 }