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 GenericLaserScanFilterNode
00039 {
00040 protected:
00041
00042 ros::NodeHandle nh_;
00043
00044
00045 tf::TransformListener tf_;
00046 message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
00047 tf::MessageFilter<sensor_msgs::LaserScan> tf_filter_;
00048
00049
00050 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
00051
00052
00053 sensor_msgs::LaserScan msg_;
00054 ros::Publisher output_pub_;
00055
00056 ros::Timer deprecation_timer_;
00057
00058 public:
00059
00060 GenericLaserScanFilterNode() :
00061 scan_sub_(nh_, "scan_in", 50),
00062 tf_filter_(scan_sub_, tf_, "base_link", 50),
00063 filter_chain_("sensor_msgs::LaserScan")
00064 {
00065
00066 filter_chain_.configure("");
00067
00068
00069 tf_filter_.registerCallback(boost::bind(&GenericLaserScanFilterNode::callback, this, _1));
00070 tf_filter_.setTolerance(ros::Duration(0.03));
00071
00072
00073 output_pub_ = nh_.advertise<sensor_msgs::LaserScan>("output", 1000);
00074
00075 deprecation_timer_ = nh_.createTimer(ros::Duration(5.0), boost::bind(&GenericLaserScanFilterNode::deprecation_warn, this, _1));
00076 }
00077
00078 void deprecation_warn(const ros::TimerEvent& e)
00079 {
00080 ROS_WARN("'generic_laser_filter_node' has been deprecated. Please switch to 'scan_to_scan_filter_chain'.");
00081 }
00082
00083
00084 void callback(const sensor_msgs::LaserScan::ConstPtr& msg_in)
00085 {
00086
00087 filter_chain_.update (*msg_in, msg_);
00088
00089
00090 output_pub_.publish(msg_);
00091 }
00092 };
00093
00094 int main(int argc, char **argv)
00095 {
00096 ros::init(argc, argv, "scan_filter_node");
00097
00098 GenericLaserScanFilterNode t;
00099 ros::spin();
00100
00101 return 0;
00102 }