ROS学习笔记74（TF Using Stamped datatypes with tf::MessageFilter）

本教程介绍了如何将传感器数据与tf一起使用。一些传感器数据的实际例子是：

单目摄像头和双目摄像头
激光雷达

假设创建了一只名为turtle3的新龟，但它没有里程信息；

但有一个高架摄像机跟踪了其位置，并将其位置作为与世界坐标系相关的geometry_msgs / PointStamped消息发布出来。

定义的geometry_msgs / PointStamped消息类型。

Turtle 1如果想知道turtle3与自身的比较呢？

要做到这一点，turtle1必须收听正在发布的关于turtle3姿势话题，等待转换到所需的坐标已准备好，然后执行其操作。

为了使这更容易，tf :: MessageFilter类非常有用。

tf :: MessageFilter将订阅任何ros消息并对其进行缓存，直到可以将其转换为目标坐标。

1 Setup

roslaunch turtle_tf turtle_tf_sensor.launch

这将会使得龟1和龟3自己移动。

话题为turtle_point_stamped，其中包含一个geometry_msgs / PoseStamped数据类型，说明了turtle3在世界框架中的位置。

为了可靠地获取turtle1坐标中的流数据，我们使用以下代码：

#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"class PoseDrawer
{
public:PoseDrawer() : tf_(),  target_frame_("turtle1") {point_sub_.subscribe(n_, "turtle_point_stamped", 10); //节点，话题，缓冲大小tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );//this->msgCallback(x)} ;private:message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;tf::TransformListener tf_;tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;ros::NodeHandle n_;std::string target_frame_;//  Callback to register with tf::MessageFilter to be called when transforms are availablevoid msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) {geometry_msgs::PointStamped point_out;try {tf_.transformPoint(target_frame_, *point_ptr, point_out);printf("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 (tf::TransformException &ex) {printf ("Failure %s\n", ex.what()); //Print exception which was caught}};};int main(int argc, char ** argv)
{ros::init(argc, argv, "pose_drawer"); //Init ROSPoseDrawer pd; //Construct classros::spin(); // Run until interupted
};

2 说明

2.1 包括

您必须包含tf包中的tf :: MessageFilter标头。就行之前使用的tf和ROS头文件。

#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"

2.2 永久数据

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

private:message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;tf::TransformListener tf_;tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;ros::NodeHandle n_;std::string target_frame_;

2.3 构造函数

启动时，必须使用话题初始化ros message_filters :: Subscriber。并且必须使用该Subscriber object初始化tf :: MessageFilter。

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。

回调函数是在数据准备好时调用的函数。

  PoseDrawer() : tf_(),  target_frame_("turtle1"){point_sub_.subscribe(n_, "turtle_point_stamped", 10);tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );} ;

注意：注册了一个回调函数。而bind把成员函数msgCallback绑定到对象上。

2.4 回调方法

Once the data is ready, just call transformPose and print to screen for the tutorial.

void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) {geometry_msgs::PointStamped point_out;try {tf_.transformPoint(target_frame_, *point_ptr, point_out); //这个是重点printf("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 (tf::TransformException &ex) {printf ("Failure %s\n", ex.what()); //Print exception which was caught}};};

3 结果

如果它正在运行，你应该看到像这样的流数据。

海龟3在龟的框架中的位置1位置（x：-0.603264 y：4.276489 z：0.000000）
海龟3在龟的框架中的位置1位置（x：-0.598923 y：4.291888 z：0.000000）
海龟3在龟的框架中的位置1位置（x：-0.594828 y：4.307356 z：0.000000）
海龟3在龟的框架中的位置1位置（x：-0.590981 y：4.322886 z：0.000000）