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 message_filters::Subscriber<sensor_msgs::LaserScan> no_sub_;
00049 tf::MessageFilter<sensor_msgs::LaserScan> tf_filter_;
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_filter_(scan_sub_, tf_, "", 50),
00068 filter_chain_("sensor_msgs::LaserScan")
00069 {
00070
00071
00072 using_filter_chain_deprecated_ = private_nh_.hasParam("filter_chain");
00073
00074 if (using_filter_chain_deprecated_)
00075 filter_chain_.configure("filter_chain", private_nh_);
00076 else
00077 filter_chain_.configure("scan_filter_chain", private_nh_);
00078
00079 std::string tf_message_filter_target_frame;
00080
00081 if (private_nh_.hasParam("tf_message_filter_target_frame"))
00082 {
00083 private_nh_.getParam("tf_message_filter_target_frame", tf_message_filter_target_frame);
00084
00085 tf_filter_.setTargetFrame(tf_message_filter_target_frame);
00086 tf_filter_.setTolerance(ros::Duration(0.03));
00087
00088
00089 tf_filter_.registerCallback(boost::bind(&ScanToScanFilterChain::callback, this, _1));
00090 }
00091 else
00092 {
00093 tf_filter_.connectInput(no_sub_);
00094
00095 scan_sub_.registerCallback(boost::bind(&ScanToScanFilterChain::callback, this, _1));
00096 }
00097
00098
00099 output_pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan_filtered", 1000);
00100
00101
00102 deprecation_timer_ = nh_.createTimer(ros::Duration(5.0), boost::bind(&ScanToScanFilterChain::deprecation_warn, this, _1));
00103 }
00104
00105
00106 void deprecation_warn(const ros::TimerEvent& e)
00107 {
00108 if (using_filter_chain_deprecated_)
00109 ROS_WARN("Use of '~filter_chain' parameter in scan_to_scan_filter_chain has been deprecated. Please replace with '~scan_filter_chain'.");
00110 }
00111
00112
00113 void callback(const sensor_msgs::LaserScan::ConstPtr& msg_in)
00114 {
00115
00116 filter_chain_.update (*msg_in, msg_);
00117
00118
00119 output_pub_.publish(msg_);
00120 }
00121 };
00122
00123 int main(int argc, char **argv)
00124 {
00125 ros::init(argc, argv, "scan_to_scan_filter_chain");
00126
00127 ScanToScanFilterChain t;
00128 ros::spin();
00129
00130 return 0;
00131 }