本片博客描述：如何使用 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
point_sub_;
tf2_ros::MessageFilter
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;
};
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"
首先时包含 tf2_ros::MessageFilter 的头文件 和其它头文件
===============================================================
private:
std::string target_frame_;
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener tf2_;
ros::NodeHandle n_;
message_filters::Subscriber
point_sub_;
tf2_ros::MessageFilter
tf2_filter_;
类的私有变量
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) );
}
启动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
}
}
tf2_ros::MessageFilter 的 回调函数
buffer_.transform(*point_ptr, point_out, target_frame_);
通过这个函数（就是tf2_ros::TransformListener的一个功能函数） 将 在 世界坐标系的 点 转化为 期望坐标系 下的点
4、 疑问
上面通过 tf2_ros::MessageFilter 的 方式 和 直接订阅 位置信息 然后 通过tf2_ros::TransformListener的transform功能函数去结算 这两者有什么区别呢？
