学习ROS笔记之TF——learning tf(一)

1,tf官网学习，第一部分为learning tf       http://wiki.ros.org/tf/Tutorials

2,首先学习introduction to tf      http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf

注意运行

$ roslaunch turtle_tf turtle_tf_demo.launch

若出现错误，即出现process died等内容时，会有一个链接进去帮你找到答案。但是我是直接关掉终端后，再打开终端即可运行（？），可以试试，可能是重新配置了一下

按照一二三步运行，会出现窗口上有两只乌龟，你可以用键盘按键控制其中一只乌龟移动，而另外一只乌龟会跟随着这只乌龟移动，直到它们处于同一个位置

这样是怎样做到的呢？This demo is using the tf library to create three coordinate frames: a world frame, a turtle1 frame, and a turtle2 frame. This tutorial uses atf broadcaster to publish the turtle coordinate frames and a tf listener to compute the difference in the turtle frames and move one turtle to follow the other.这个演示是用tf library来创建三个坐标系：世界的坐标系，乌龟1的坐标系，乌龟2的坐标系。教材中使用了一个tf broadcaster来发布乌龟的坐标，还使用了一个tf listener来计算发布坐标与现在坐标的差异并移动乌龟去跟随另一只。

tf tool 

看tf的工具来了解tf如何运作

2.1view_frames

view_frames creates a diagram of the frames being broadcast by tf over ROS. 工具view_frames从单词字面上理解为查看框架，它创建了一个框架的图形，这个框架是又tf通过ROS发布出来的。Here we can see the three frames that are broadcast by tf: the world, turtle1, and turtle2. We can also see that world is the parent of the turtle1 and turtle2 frames.使用view_frames后发现pdf图形里面tf发布的三个框架：world,turtle1,turtle2,我们发现框架world是框架turtle1和turtle2的父母。

2.2tf_echo

tf_echo reports the transform between any two frames broadcast over ROS.工具tf_echo报告了两个框架之间的转换

rviz and tf

rviz is a visualization tool that is useful for examining tf frames.

rviz是一个可视化的工具，用来检查tf框架十分有用

$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

In the side bar you will see the frames broadcast by tf. As you drive the turtle around you will see the frames move in rviz.

这时你能在rviz中看到由tf发布的框架，当你用键盘控制乌龟移动时能在rviz中看到相应的框架移动，框架的名称分别是turtle1和turtle2。其中框架都对应着父母框架world.

3writing a broadcaster (C++)用C++写一个发布器broadcaster

This tutorial teaches you how to broadcast coordinate frames to tf. In this case, we want to broadcast the changing coordinate frames of the turtles, as they move around本教程教你如何去发布坐标系给tf，在这个例子中，我们想去发布正在改变的乌龟的坐标系，当它们在移动的时候。

turtle_tf_broadcaster.cpp:

#include <ros/ros.h>#include <tf/transform_broadcaster.h>//The tf package provides an implementation of a TransformBroadcaster to help make the task of publishing transforms easier. //To use the TransformBroadcaster, we need to include the tf/transform_broadcaster.h header file。tf包提供了一个实现机制TransformBroadcaster//来帮助使得发布变换更容易，为了使用TransformBroadcaster，我们需要包含这个tf/transform_broadcaster.h头文件。#include <turtlesim/Pose.h>std::string turtle_name;void poseCallback(const turtlesim::PoseConstPtr& msg){  static tf::TransformBroadcaster br;  tf::Transform transform;  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );//Here we create a Transform object, and copy the information from the 2D turtle pose into the 3D transform我们创建了一个Transform对象，并且//复制这个乌龟图形的2D位置并转换为3D位置，添加了一个z系，但是设为0.0,即在rviz中也是在平面
 tf::Quaternion q;  q.setRPY(msg->theta, 0, 0);  transform.setRotation(q);
//Here we set the rotation.通过transform设置方位

 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
//This is where the real work is done.  Sending a transform with a TransformBroadcaster requires four arguments. 
First, we pass in the transform itself. 
Now we need to give the transform being published a timestamp, we'll just stamp it with the current time, ros::Time::now(). 
Then, we need to pass the name of the parent frame of the link we're creating, in this case "world" 
Finally, we need to pass the name of the child frame of the link we're creating, in this case this is the name of the turtle itself.
//定义了一个名称为br的TransformBroadcaster，即上面所提到的，是tf包中的，通过TransformBroadcaster来发布信息需要四点:
//1.首先需要transform
//2.需要给transform一个时间戳，这个时间戳是现在的时间
//3.然后，我们需要将我们创建的link的父框架的名字传输过去，在这个例子中是world
//4.最后，我们需要将我们创建的link的子框架的名字传输过去，在这里就是turtle本身
 }
//Here, we create a TransformBroadcaster object that we'll use later to send the  transformations over the wire我们创建了一个TransformBroadcaster
//对象，我们之后会使用它来发布转换消息
 int main(int argc, char** argv){  ros::init(argc, argv, "my_tf_broadcaster");
//初始化ros,命名节点为my_tf_broadcaster
 if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};  turtle_name = argv[1];
//乌龟的名字可以输入 
 ros::NodeHandle node;
//NodeHandle是与ROS系统交流的最主要的接入点，是一个句柄
 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
//从master订阅某乌龟的“/pose”话题，当消息到来时，即当乌龟位置改变时产生新的消息时，ROS将会调用poseCallback 
 ros::spin(); 
 return 0;};

总结：从.launch文件中启动了四个节点，分别是turtlesim包中的类型为turtlesim_node的节点sim，turtlesim中类型为turtle_teleop_key的节点teleop,还有刚才建的包learning_tf中类型为turtle_tf_broadcaster的两个节点turtle1_tf_broadcaster和turtle2_tf_broadcaster.当运行.launch文件后生成了一个乌龟turtle1,当利用键盘控制使乌龟移动时，乌龟的位置发生变化，并通过主题turtle1/pose来发布位置改变的消息，由于节点turtle1_tf_broadcaster订阅了这个主题，当这个主题有消息发布时（比如乌龟位置变化），则会调用poseCallback，正如上面分析所示，这个方法将会将消息反馈回来，反馈的消息将2D改为3D，反馈为Translation和Rotation,其中Rotation包括Quaternion和RPY. 这样，当乌龟移动时能够反馈位置信息并能够在终端看到，主要利用了订阅pose主题，并利用TransformBroadcaster来发布转换消息。

4.writing a listener(C++)用C++编写一个listener节点

turtle_tf_listener.cpp:

#include <ros/ros.h>#include <tf/transform_listener.h>//The tf package provides an implementation of a TransformListener to help make the task of receiving transforms easier.//To use the TransformListener, we need to include the tf/transform_listener.h header file.//tf包提供了一个实现机制TransformListener来帮助更容易实现接收转换，为了要使用这个TransformListener，就要包含tf包中的tansform_listener.h头文件#include <geometry_msgs/Twist.h>//消息类型头文件#include <turtlesim/Spawn.h>//生成乌龟的头文件spawn turtleint main(int argc, char** argv){  ros::init(argc, argv, "my_tf_listener");//初始化ROS  ros::NodeHandle node;//NodeHandle是与ROS交流的最主要的接入点  ros::service::waitForService("spawn");  ros::ServiceClient add_turtle =     node.serviceClient<turtlesim::Spawn>("spawn");  turtlesim::Spawn srv;  add_turtle.call(srv);  ros::Publisher turtle_vel =node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);//通过主题turtle2/cmd_vel来发布geometry_msgs::Twist类型的消息  tf::TransformListener listener;//Here, we create a TransformListener object. Once the listener is created, it starts receiving tf transformations over the wire, //and buffers them for up to 10 seconds.  The TransformListener object should be scoped to persist otherwise it's cache will be //unable to fill and almost every query will fail.  A common method is to make the TransformListener object a member variable of a class.//在这里我们创建了一个TransformListener名称为listener,一旦listener创建后，它开始就接收tf的转换消息并让它们缓冲10s.TransformListener应该要一直存在，否则//它的缓存将不能填满并且几乎所有的查询将不能进行 ros::Rate rate(10.0);  while (node.ok()){    tf::StampedTransform transform;    try{      listener.lookupTransform("/turtle2", "/turtle1",  ros::Time(0), transform);//Here, the real work is done, we query the listener for a specific transformation. Let's take a look at the four arguments: 
We want the transform from this frame ... 
... to this frame. 
The time at which we want to transform. Providing ros::Time(0) will just get us the latest available transform. 
The object in which we store the resulting transform. 
//All this is wrapped in a try-catch block to catch possible exceptions.//我们查询listener为一个指定的转换，有四个主要参数：//1，2我们想要转换从turtle2到turtle1,也即最初运行结果所看到的当turtle2移动时，turtle1跟随着turtle2的轨迹移动//3转换的时间//4用transform来存储转换结果//使用模块try-catch }    catch (tf::TransformException &ex) {      ROS_ERROR("%s",ex.what());      ros::Duration(1.0).sleep();    }    geometry_msgs::Twist vel_msg;    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),                                    transform.getOrigin().x());    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +                                  pow(transform.getOrigin().y(), 2));//Here, the transform is used to calculate new linear and angular velocities for turtle2, based on its distance and angle from //turtle1.  The new velocity is published in the topic "turtle2/command_velocity" and the sim will use that to update turtle2's //movement.//transform用来计算新的线性和角度的向量值为turtle2,这是基于turtle2到turtle1的距离和角度，新的向量值将会发布在主题turtle2/cmd_vel上，sim乌龟将会使用它//并更改turtle2的移动  turtle_vel.publish(vel_msg);    rate.sleep();  }  return 0;};

总结：运行.launch文件时出现了错误，在链接中找到了答案http://answers.ros.org/question/148121/something-wrong-with-tf-tutorial/，首先是出现一个错误[ERROR] [1397783547.530858724]: "turtle2" passed to lookupTransform argument target_frame does not exist. 之后在listener_lookuptransfom()函数上添加语句listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));则没有出现错误了，但是按键只能控制一只乌龟，另外一只还在不停的从一边移动到另外一边，按照链接中所说，又将turtle_tf_broadcaster.cpp中的 "q.setRPY(msg->theta, 0, 0)" 改为 "q.setRPY(0, 0, msg->theta)"此时则没有错误了，按键能控制一只乌龟移动，另外一只会跟着这只乌龟的轨迹移动。

这是用rqt看到的节点之间的通信图，当.launch运行时，生成了一个sim节点即生成一只乌龟，这个乌龟可以用teleop节点即按键控制它移动，注意上图，是通过主题/turtle1/cmd_vel来发布消息，/sim来接收消息来使用的，launch还产生了节点/turtle1_tf_broadcaster和/turtle2_tf_broadcaster,当乌龟1的位置改变时，节点/turtle1_tf_broadcaster和/turtle2_tf_broadcaster通过订阅/turtle1/pose和/turtle2/pose来得到位置转换消息，并将消息发布出来通过主题/tf，节点listener通过订阅主题/tf来的到位置转换消息，并将消息改变一定的linear和anguar发布到主题/turtle2/cmd_vel,而乌龟2订阅了这个主题，并随着这个命令来改变位置变换。即从上可知当乌龟1接收按键消息改变位置时，乌龟2能够接收到乌龟1改变的消息，并能够按照乌龟1位置的改变来改变自己的位置，这样就出现了这样的结果，乌龟2跟随这乌龟1移动。主要通过turtle_tf_broadcaster订阅位置改变消息并通过tf主题发布然后listener来订阅tf主题计算出位置改变消息并将消息通过主题/turtle2/cmd_vel发送给turtuer2。其中重点是位置转换使用tf。