| Note: This tutorial assumes that you have completed the previous tutorials: introduction to using laser scanners tutorial. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Laser filtering in C++
Description: Raw laser scans contain all points returned from the scanner without processing. Many applications, however, are better served by filtered scans which remove unnecessary points (such as unreliable laser hits or hits on the robot itself), or pre-process the scans in some way (such as by median filtering). This tutorial will teach you how to apply pre-existing filters to laser scans.Tutorial Level: INTERMEDIATE
Let's take a look at generic_laser_filter_node.cpp to see how to run a set of filters from within a node:
1 #include "ros/ros.h"
2 #include "sensor_msgs/LaserScan.h"
3 #include "message_filters/subscriber.h"
4 #include "tf/message_filter.h"
5 #include "tf/transform_listener.h"
6 #include "filters/filter_chain.h"
7
8 class GenericLaserScanFilterNode
9 {
10 protected:
11 // Our NodeHandle
12 ros::NodeHandle nh_;
13
14 // Components for tf::MessageFilter
15 tf::TransformListener tf_;
16 message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
17 tf::MessageFilter<sensor_msgs::LaserScan> tf_filter_;
18
19 // Filter Chain
20 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
21
22 // Components for publishing
23 sensor_msgs::LaserScan msg_;
24 ros::Publisher output_pub_;
25
26 public:
27 // Constructor
28 GenericLaserScanFilterNode() :
29 scan_sub_(nh_, "scan_in", 50),
30 tf_filter_(scan_sub_, tf_, "base_link", 50),
31 filter_chain_("sensor_msgs::LaserScan")
32 {
33 // Configure filter chain
34 filter_chain_.configure("~");
35
36 // Setup tf::MessageFilter for input
37 tf_filter_.registerCallback(
38 boost::bind(&GenericLaserScanFilterNode::callback, this, _1));
39 tf_filter_.setTolerance(ros::Duration(0.03));
40
41 // Advertise output
42 output_pub_ = nh_.advertise<sensor_msgs::LaserScan>("output", 1000);
43 }
44
45 // Callback
46 void callback(const sensor_msgs::LaserScan::ConstPtr& msg_in)
47 {
48 // Run the filter chain
49 filter_chain_.update (*msg_in, msg_);
50
51 // Publish the output
52 output_pub_.publish(msg_);
53 }
54 };
55
56 int main(int argc, char **argv)
57 {
58 ros::init(argc, argv, "scan_filter_node");
59
60 GenericLaserScanFilterNode t;
61 ros::spin();
62
63 return 0;
64 }
As we can see from this node, a series of filters is called a filter_chain and running a filter chain on a laser scan only involves 4 lines of code:
20 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
Declare a filter chain templated to run on laser scans.
31 filter_chain_("sensor_msgs::LaserScan")
Construct a filter chain, telling it the type of filters that will be loaded ("sensor_msgs::LaserScan" -- this always must be the same as the templated type).
34 filter_chain_.configure("~");
Tell the filter chain to read its configuration (always named "filter_chain") from our private namespace.
49 filter_chain_.update (*msg_in, msg_);
Call the update function to filter the scan.
Now you can use the filtered scan in your own node.
