ROS_TF

TF

失去TF的机器人 类比到人类 嗯… 或许没法想象 因为我们没有失去过对四肢的感觉

TF的作用：
1） TF库的目的是实现系统中任一个点在所有坐标系之间的坐标变换，也就是说，只要给定一个坐标系下的一个点的坐标，就能获得这个点在其他坐标系的坐标.
2 ）它是一个可以让用户实时追踪多个坐标系的功能包。tf通过一个实时缓存的树结构来维护坐标系之间的关系，可以让用户任何时间点完成两个坐标系之间点、向量之间的转换。

使用tf功能包：
a. 监听tf变换： 接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
b.广播 tf变换: 向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播，每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

机器人中的应用
机器人本体和机器人的工作环境中，往往存在大量的组件元素，在机器人设计和机器人应用中都会涉及不同组件的位置和姿态，这就需要引入坐标系以及坐标变换的概念。
在很多应用中，添加一个参考系是很有必要的，比如在一个world参考系下，有很一个激光扫描节点，tf可以帮助我们将激光扫描的信息坐标装换成全局坐标。

如图所示的A、B两个坐标系，A坐标系下的位姿，可以通过平移和旋转变换成B坐标系下的位姿，这里的平移和旋转可以通过4*4的变换矩阵来描述，其理论原理可以参考机器人学的理论教程。

TF消息结构：
tf中的信息是一个树状的结构，world参考系是最顶端的父参考系，其他的参考系都需要向下延伸。如果我们在上文的基础上添加一个参考系，就需要让这个新的参考系成为已有三个参考系中的一个的子参考系。

TF的数据类型：

相关函数
1 创建单位四元数
2 从roll pitch yaw三角创建四元素
3 几何消息中 从roll pitch yaw三角创建四元素

下面会用海龟仿真器来简单学习TF广播与监听

我们要实现：先生产一个海龟（2号）并让他自动跟踪仿真器原先的海龟（1号）
这里主要要弄清楚，

TF数据怎么形成 需要哪些数据
谁在订阅TF数据 又如何监听到这种数据
谁在发布TF数据 又是如何发布的

C++ 描述

广播器 broadcaster：

/*TF广播器 订阅海龟1号的位置信息 在订阅的回调函数中对tf转换数据结构进行填写 创建一个广播器 用广播器发送 海龟1号 与 世界 两个坐标系之间的关系*/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <std/h>

std::string turtle_name;

//重点
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	static tf::TransformBroadcaster br;                       //创建tf的广播
	tf::Transform transform;								  //初始化tf数据
	transform.setOrigin tf::Vector3(msg->x,msg->y,0.0);
	tf::Quaternion q;
	q.setRPY(0,0,msg->theta);
	transform.setRotation(q);
     /*通过TransformBroadcaster来发送转换关系，需要附带5个参数。
 	1.旋转变换
 	２．平移变换
 	３、时间戳
 	４、parent节点名字
 	５、child节点名字
 */ 

	
	//广播world与海龟坐标系之间的tf数据
	br.sendTransform( tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name) )
}

int main(int argc,char** argv)
{
	ros::init(argc,argv,"my_tf_broadcaster");
	
	if(argc !=2)
	{
		ros_ERROR("need turtle name as argument");
		return -1;	
	}	
	turtle_name = argv[1];

	ros::NodeHandle node;
	ros::Subscriber sub = nod.subscribe(turtle_name+"/pose",10,&poseCallback);
	
	ros::spin();
	
	return 0;
}

监听者 listener：

//首先会向spwan服务请求再生产一个海龟2号  并且创建能控制一个海龟2号的发布者
//创建一个监听器 监听器会等待TF_tree返回海龟1号与海龟2号的关系 并且将结果放到transform结构体中
//我们可以从transform结构体中取出表达两者相对关系的数据 计算出海龟2号所需要的线速度和角速度 最后通过海龟2号的发布者发布出去

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

int main(int argc,char** argv )
{	
	ros::init(argc,argv,"my_tf_listener");
	ros::NodeHandle node;
	
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::spawn>("/spawn");
	turtle::Spawn srv;
	add_turtle.call(srv);
	
	ros::Publisher turtle_vel=node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);
	
	ros::TransformListener listener;
	ros::Rate rate(10.0);
	while(node.ok())
	{
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle2","/turtle1",ros:;Time(0),ros::Duration(3.0));
			listener.lookupTransform ("/turtle2","/turtle1",ros:;Time(0),transform);
			
		}
		catch (tf::TransformException &ex)
		{
			ROS_EORROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
	}
	
	geometry_msgs:Twist vel_msg;
	vel_msg.angular.z = 4.0 * atan2(transform.geOrigin().y()),transform.getOrigin().x();
	vel_msg.linear.x  = 0.5 * sqrt( pow(transform.getOrigin().x(),2) + pow( transform.getOrigin().y(),2));
	
	turtle_vel.publish(vel_msg);
	
	rate.sleep();

}

Python描述

Broadcaster
 1  #!/usr/bin/env python     
 2   import roslib  
 3   roslib.load_manifest('learning_tf') 
 4   import rospy 
 5    
 6   import tf
 7   import turtlesim.msg
 8   #使用 tf 完成坐标变换，订阅这个topic "turtleX/pose" 并且对每一条msg进行以下计算
 9   def handle_turtle_pose(msg, turtlename):
10       br = tf.TransformBroadcaster()   #将turtlebot的坐标系发到 tf tree中
11       br.sendTransform((msg.x, msg.y, 0),
12                        tf.transformations.quaternion_from_euler(0, 0, msg.theta),  #使用四元数完成坐标变换
13                        rospy.Time.now(),               #
14                       turtlename,
15                        "world")
16  
17   if __name__ == '__main__':
18       rospy.init_node('turtle_tf_broadcaster')  #添加新的节点
19       turtlename = rospy.get_param('~turtle')#
20       rospy.Subscriber('/%s/pose' % turtlename,   #发布的信息内容
21                        turtlesim.msg.Pose,
22                        handle_turtle_pose,
23                        turtlename)
24       rospy.spin()

Listener
 1 #!/usr/bin/env python  
 2 import roslib
 3 roslib.load_manifest('learning_tf')
 4 import rospy
 5 import math
 6 import tf     #tf 里面有定义订阅者的函数
 7 import geometry_msgs.msg
 8 import turtlesim.srv
 9 
10 if __name__ == '__main__':
11     rospy.init_node('tf_turtle')
12 
13     listener = tf.TransformListener()  #定义listener  一旦定义listener  ，他就开始接受信息，并且可以缓冲10S.
14 
15     rospy.wait_for_service('spawn')   #等待服务。 spwn ，这个服务可以创建另一只乌龟
16     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
17     spawner(4, 2, 0, 'turtle2')   #位置 跟名字
18 
19     turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
20    #这一段是我们真正做的工作，我们使 turtle1是父坐标系，turtle2是子坐标系，然后就调用了listener.lookupTransform
21  rate = rospy.Rate(10.0)
22   while not rospy.is_shutdown(): 
23       try: 
24        (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))    #可以缓冲10S内最近的信息。
25       except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 
26       continue 
27    angular = 4 * math.atan2(trans[1], trans[0]) 
28    linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) 
29    cmd = geometry_msgs.msg.Twist() 
30    cmd.linear.x = linear 
31    cmd.angular.z = angular 
32    turtle_vel.publish(cmd) 
33    rate.sleep()