ROS TF2 ：通过tf2_ros::MessageFilter 将世界坐标系下的点转为期望坐标系下的点

本片博客描述：如何使用 tf2_ros::MessageFilter 处理 Stamped（时间戳） 数据类型

在 TF2 的 体系下 如何使用传感器的数据

在 实际 情况下 传感器的数据包括： 相机、激光雷达

1、情景

假设一只新的乌龟 命名为 乌龟3 ， 它没有好的里程计（也就是说不指定它的具体坐标），但是现在有一个上方的相机 跟踪 它的位置并且发布 相对与 世界坐标系 下的 位置信息 （ PointStamped 类型的 topic）

乌龟1 想要知道 乌龟3 相对自己的位置

为此，turtle1必须监听正在发布turtle3位置的topic，直到准备好转换为期望坐标系下，然后进行操作。

做这件事的话，使用 tf2_ros::MessageFilter 类 非常有用

tf2_ros :: MessageFilter将使用标头对任何ros消息进行订阅，并将其缓存，直到可以将其转换为期望坐标系下为止。

2、实现代码

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"

#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

class PoseDrawer
{
public:
  PoseDrawer() :
    tf2_(buffer_),  target_frame_("turtle1"),
    tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0)
  {
    point_sub_.subscribe(n_, "turtle_point_stamped", 10);
    tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  }

  //  Callback to register with tf2_ros::MessageFilter to be called when transforms are available
  void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr) 
  {
    geometry_msgs::PointStamped point_out;
    try 
    {
      buffer_.transform(*point_ptr, point_out, target_frame_);
      
      ROS_INFO("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
             point_out.point.x,
             point_out.point.y,
             point_out.point.z);
    }
    catch (tf2::TransformException &ex) 
    {
      ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
    }
  }

private:
  std::string target_frame_;
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener tf2_;
  ros::NodeHandle n_;
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;

};


int main(int argc, char ** argv)
{
  ros::init(argc, argv, "pose_drawer"); //Init ROS
  PoseDrawer pd; //Construct class
  ros::spin(); // Run until interupted 
  return 0;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

3、代码解释

=============================================================

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"

#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
1
2
3
4
5
6
7

首先时包含 tf2_ros::MessageFilter 的头文件 和其它头文件

===============================================================

private:
  std::string target_frame_;
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener tf2_;
  ros::NodeHandle n_;
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;
1
2
3
4
5
6
7

类的私有变量

tf2_ros::Buffer
tf2_ros::TransformListener
tf2_ros::MessageFilter
这三个要在构造函数时初始化 使数据一直被保持

====================================================================

  PoseDrawer() :
    tf2_(buffer_),  target_frame_("turtle1"),
    tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0)
  {
    point_sub_.subscribe(n_, "turtle_point_stamped", 10);
    tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  }
1
2
3
4
5
6
7

启动ros message_filters :: Subscriber时必须使用该主题进行初始化。 并且tf2_ros :: MessageFilter必须使用该Subscriber对象进行初始化。 MessageFilter构造函数中需要注意的其他参数是target_frame和回调函数。 目标框架是确保canTransform成功执行的框架。 回调函数是在数据准备好后将被调用的函数。

=======================================================================

  //  Callback to register with tf2_ros::MessageFilter to be called when transforms are available
  void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr) 
  {
    geometry_msgs::PointStamped point_out;
    try 
    {
      buffer_.transform(*point_ptr, point_out, target_frame_);
      
      ROS_INFO("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
             point_out.point.x,
             point_out.point.y,
             point_out.point.z);
    }
    catch (tf2::TransformException &ex) 
    {
      ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

tf2_ros::MessageFilter 的 回调函数

buffer_.transform(*point_ptr, point_out, target_frame_);

通过这个函数（就是tf2_ros::TransformListener的一个功能函数） 将 在 世界坐标系的 点 转化为 期望坐标系 下的点

4、 疑问

上面通过 tf2_ros::MessageFilter 的 方式 和 直接订阅 位置信息 然后 通过tf2_ros::TransformListener的transform功能函数去结算 这两者有什么区别呢？