Using ROS 2? Check out the ROS 2 tf2 tutorials.
| Note: This tutorial assumes you have completed the learning tf2 tutorials.. |
 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. |
Using Stamped datatypes with tf2_ros::MessageFilter
Description: This tutorial describes how to use tf2_ros::MessageFilter to process Stamped datatypes.Tutorial Level: INTERMEDIATE
This tutorial explains how to use sensor data with tf2. Some real-world examples of sensor data are:
- cameras, both mono and stereo
- laser scans
Suppose that a new turtle named turtle3 is created and it doesn't have good odometry, but there is an overhead camera tracking its position and publishing it as a PointStamped message in relation to the world frame.
Turtle 1 wants to know where turtle3 is compared to itself.
To do this turtle1 must listen to the topic where turtle3's pose is being published wait until transforms into the desired frame are ready, and then do it's operations. To make this easier the tf2_ros::MessageFilter class is very useful. The tf2_ros::MessageFilter will take a subscription to any ros Message with a Header and cache it until it is possible to transform it into the target frame.
Setup
roslaunch turtle_tf2 turtle_tf2_sensor.launch
This will bring up turtle 1 to drive and turtle3 moving on its own. There is a topic turtle_point_stamped with a PointStamped data type stating turtle3's position in the world frame.
To get the streaming data in the frame of turtle1 reliably we will use the following code:
1 #include "ros/ros.h"
2 #include "geometry_msgs/PointStamped.h"
3
4 #include "tf2_ros/transform_listener.h"
5 #include "tf2_ros/message_filter.h"
6 #include "message_filters/subscriber.h"
7 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
8
9 class PoseDrawer
10 {
11 public:
12 PoseDrawer() :
13 tf2_(buffer_), target_frame_("turtle1"),
14 tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0)
15 {
16 point_sub_.subscribe(n_, "turtle_point_stamped", 10);
17 tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
18 }
19
20 // Callback to register with tf2_ros::MessageFilter to be called when transforms are available
21 void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr)
22 {
23 geometry_msgs::PointStamped point_out;
24 try
25 {
26 buffer_.transform(*point_ptr, point_out, target_frame_);
27
28 ROS_INFO("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
29 point_out.point.x,
30 point_out.point.y,
31 point_out.point.z);
32 }
33 catch (tf2::TransformException &ex)
34 {
35 ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
36 }
37 }
38
39 private:
40 std::string target_frame_;
41 tf2_ros::Buffer buffer_;
42 tf2_ros::TransformListener tf2_;
43 ros::NodeHandle n_;
44 message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
45 tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;
46
47 };
48
49
50 int main(int argc, char ** argv)
51 {
52 ros::init(argc, argv, "pose_drawer"); //Init ROS
53 PoseDrawer pd; //Construct class
54 ros::spin(); // Run until interupted
55 return 0;
56 };
Explanation
Includes
You must include the tf2_ros::MessageFilter headers from the tf2_ros package. As well as the previously used tf2 and ros headers.
Persistent Data
There need to be persistent instances of tf2_ros::Buffer tf2_ros::TransformListener tf2_ros::MessageFilter
Constructor
When starting up the ros message_filters::Subscriber must be initialized with the topic. And the tf2_ros::MessageFilter must be initialized with that Subscriber object. The other arguments of note in the MessageFilter constructor are the target_frame and callback function. The target frame is the frame into which it will make sure canTransform will succeed. And the callback function is the function which will be called when the data is ready.
Callback Method
Once the data is ready, just call buffer_.transform and print to screen for the tutorial.
20 // Callback to register with tf2_ros::MessageFilter to be called when transforms are available
21 void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr)
22 {
23 geometry_msgs::PointStamped point_out;
24 try
25 {
26 buffer_.transform(*point_ptr, point_out, target_frame_);
27
28 ROS_INFO("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
29 point_out.point.x,
30 point_out.point.y,
31 point_out.point.z);
32 }
33 catch (tf2::TransformException &ex)
34 {
35 ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
36 }
37 }
Result
If it's running right you should see streaming data like this.
point of turtle 3 in frame of turtle 1 Position(x:-0.603264 y:4.276489 z:0.000000) point of turtle 3 in frame of turtle 1 Position(x:-0.598923 y:4.291888 z:0.000000) point of turtle 3 in frame of turtle 1 Position(x:-0.594828 y:4.307356 z:0.000000) point of turtle 3 in frame of turtle 1 Position(x:-0.590981 y:4.322886 z:0.000000)
