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:

https://raw.githubusercontent.com/ros/geometry_tutorials/indigo-devel/turtle_tf2/src/message_filter.cpp

   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.

   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 

Persistent Data

There need to be persistent instances of tf2_ros::Buffer tf2_ros::TransformListener tf2_ros::MessageFilter

  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_;

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.

  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   }

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)

Wiki: tf2/Tutorials/Using stamped datatypes with tf2::MessageFilter (last edited 2020-01-27 13:26:08 by GiorgosTsamis)