Note: This tutorial assumes you have completed the learning tf 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 tf::MessageFilter

Description: This tutorial describes how to use tf::MessageFIlter to process Stamped datatypes.

Tutorial Level: INTERMEDIATE

tf is deprecated in favor of tf2. tf2 provides a superset of the functionality of tf and is actually now the implementation under the hood. If you're just learning now it's strongly recommended to use the tf2/Tutorials instead.

This tutorial explains how to use sensor data with tf. 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 geometry_msgs/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 its operations. To make this easier the tf::MessageFilter class is very useful. The tf::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_tf turtle_tf_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 geometry_msgs/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 "tf/transform_listener.h"
   3 #include "tf/message_filter.h"
   4 #include "message_filters/subscriber.h"
   5 
   6 class PoseDrawer
   7 {
   8 public:
   9   PoseDrawer() : tf_(),  target_frame_("turtle1")
  10   {
  11     point_sub_.subscribe(n_, "turtle_point_stamped", 10);
  12     tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
  13     tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  14   } ;
  15 
  16 private:
  17   message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  18   tf::TransformListener tf_;
  19   tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  20   ros::NodeHandle n_;
  21   std::string target_frame_;
  22 
  23   //  Callback to register with tf::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       tf_.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 (tf::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 };

Explanation

Includes

You must include the tf::MessageFilter headers from the tf package. As well as the previously used tf and ROS headers.

   1 #include "ros/ros.h"
   2 #include "tf/transform_listener.h"
   3 #include "tf/message_filter.h"
   4 #include "message_filters/subscriber.h"
   5 

Persistent Data

There need to be persistent instances of tf::TransformListener tf::MessageFilter

  16 private:
  17   message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  18   tf::TransformListener tf_;
  19   tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  20   ros::NodeHandle n_;
  21   std::string target_frame_;

Constructor

When starting up the ros message_filters::Subscriber must be initialized with the topic. And the tf::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() : tf_(),  target_frame_("turtle1")
  10   {
  11     point_sub_.subscribe(n_, "turtle_point_stamped", 10);
  12     tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
  13     tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  14   } ;

Callback Method

Once the data is ready, just call 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       tf_.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 (tf::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)

Wiki: tf/Tutorials/Using Stamped datatypes with tf::MessageFilter (last edited 2021-04-01 07:43:31 by FelixvonDrigalski)