官网:http://wiki.ros.org/tf/Tutorials/Using%20Stamped%20datatypes%20with%20tf%3A%3AMessageFilter
本教程介绍了如何将传感器数据与tf一起使用。一些传感器数据的实际例子是:
单目摄像头和双目摄像头 激光雷达
假设创建了一只名为turtle3的新龟,但它没有里程信息;
但有一个高架摄像机跟踪了其位置,并将其位置作为与世界座标系相关的geometry_msgs / PointStamped消息发布出来。
Turtle 1如果想知道turtle3与自身的比较呢?
要做到这一点,turtle1必须收听正在发布的关于turtle3姿势话题,等待转换到所需的座标已准备好,然后执行其操作。
为了使这更容易,tf :: MessageFilter类非常有用。
tf :: MessageFilter将订阅任何ros消息并对其进行缓存,直到可以将其转换为目标座标(turtle3相对于turtle1的座标)。
重点:前面几节都是发布和接收的各个节点相对于其他节点的tf树的变换transform,这次直接收发的是某个节点相对其他节点的带时间戳的位置信息
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
class PoseDrawer
{
public:
//这里的构造函数很有意思,5个私有成员变量,tf_、target_frame_在初始化列表中构造,point_sub_、tf_filter_ 在构造
//函数的函数体中构造,n_没有提及
//冒号后面是初始化列表;tf_()没有参数,是显式调用默认构造函数(可能加快速度?)
//变量名后加下划线,一种命名风格,表示该变量为类的成员
//target_frame_目标座标系,此处为turtle1。
//重要参数1:目标座标系turtle1
PoseDrawer() : tf_(), target_frame_("turtle1")
{
/*使用 topic “turtle_point_stamped”初始化message_filters::Subscriber(point_sub_是私有成员变量,类型为
message_filters::Subscriber)
重要参数2:turtle3位置消息的topic名 "turtle_point_stamped",
*/
point_sub_.subscribe(n_, "turtle_point_stamped", 10);
/*使用该message_filters::Subscriber初始化tf::MessageFilter,tf::MessageFilter初始化需要的参数还有目标
座标系target_frame,target_frame是tf::MessageFilter会确认transform能够成功执行的座标系
*/
tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
/*And the callback function is the function which will be called when the data is ready. 回调函数是当数据准备
好时会调用的函数。此处boost::bind(&PoseDrawer::msgCallback, this, _1)的作用是将本类中的成员函数
PoseDrawer::msgCallback绑定到tf::MessageFilter上,成为其回调函数
*/
tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
} ;
private:
//这是一个模板类,模板参数类型为geometry_msgs::PointStamped
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 available
*数据准备好后执行如下函数,将turtle3的在全局座标系下的位置消息转为在turtle1下的位置消息
*point_ptr为turtle3的在全局座标系下的位置消息,target_frame_目标座标系,point_out为转换后turtle3在目标坐
*标系下的位置消息。此处再强调一次,前面的教程都是发布的turtle相对于其他节点的变换transform消息,这次发布的
*是geometry_msgs::PointStamped类型的msg消息
*point_out就是转换后turtle3在目标座标系下的位置消息,如果有publisher就可以发布了
*/
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
}
};
};
int main(int argc, char ** argv)
{
ros::init(argc, argv, "pose_drawer"); //Init ROS
PoseDrawer pd; //Construct class
ros::spin(); // Run until interupted
};