| Note: This tutorial assumes you have completed the learning tf2 tutorials.. |
Using Stamped datatypes with tf2::MessageFilter
Description: This tutorial describes how to use tf2::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::MessageFilter class is very useful. The tf2::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_pose_stamped with a PoseStamped 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 "tf2/transform_listener.h"
3 #include "tf2/message_filter.h"
4 #include "message_filters/subscriber.h"
5
6 class PoseDrawer
7 {
8 public:
9 PoseDrawer() : tf2_(), target_frame_("turtle1")
10 {
11 point_sub_.subscribe(n_, "turtle_point_stamped", 10);
12 tf2_filter_ = new tf2::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf2_, target_frame_, 10);
13 tf2_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
14 } ;
15
16 private:
17 message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
18 tf2::TransformListener tf2_;
19 tf2::MessageFilter<geometry_msgs::PointStamped> * tf2_filter_;
20 ros::NodeHandle n_;
21 std::string target_frame_;
22
23 // Callback to register with tf2::MessageFilter to be called when transforms are available
24 void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
25 {
26 geometry_msgs::PointStamped point_out;
27 try
28 {
29 tf2_.transformPoint(target_frame_, *point_ptr, point_out);
30
31 printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
32 point_out.point.x,
33 point_out.point.y,
34 point_out.point.z);
35 }
36 catch (tf2::TransformException &ex)
37 {
38 printf ("Failure %s\n", ex.what()); //Print exception which was caught
39 }
40 };
41
42 };
43
44
45 int main(int argc, char ** argv)
46 {
47 ros::init(argc, argv, "pose_drawer"); //Init ROS
48 PoseDrawer pd; //Construct class
49 ros::spin(); // Run until interupted
50 };
Explaination
Includes
You must include the tf2::MessageFilter headers from the tf2 package. As well as the previously used tf2 and ros headers.
Persistent Data
There need to be persistent instances of tf2::TransformListener tf2::MessageNotifier
Constructor
When starting up the ros message_filters::Subscriber must be initialized with the topic. And the tf2::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.
9 PoseDrawer() : tf2_(), target_frame_("turtle1")
10 {
11 point_sub_.subscribe(n_, "turtle_point_stamped", 10);
12 tf2_filter_ = new tf2::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf2_, target_frame_, 10);
13 tf2_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
14 } ;
Callback Method
Once the data is ready, just call tf2.transformPose and print to screen for the tutorial.
24 void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
25 {
26 geometry_msgs::PointStamped point_out;
27 try
28 {
29 tf2_.transformPoint(target_frame_, *point_ptr, point_out);
30
31 printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
32 point_out.point.x,
33 point_out.point.y,
34 point_out.point.z);
35 }
36 catch (tf2::TransformException &ex)
37 {
38 printf ("Failure %s\n", ex.what()); //Print exception which was caught
39 }
40 };
41
42 };
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)