7、tf坐标系的广播与监听

一、对tf的了解

一个机器人中通常有多个坐标系，如世界坐标系、基座坐标系、夹具坐标系等等，这些坐标系在tf中构成坐标树，tf通过坐标树维护多个坐标系之间的坐标变换，ros坐标系可以在分布式系统中进行变换，这意味着坐标对所有的节点都是可用的，同时所有订阅tf消息的节点都会缓存一份所有坐标系的变换关系数据，而不需要中心节点来存储。

1、TF功能包能干什么？

五秒钟之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？
机器人夹取的物体相对于机器人中心坐标系的位置在哪里？
机器人中心坐标系相对于全局坐标系的位置在哪里？

2、TF坐标变换如何实现？
（1）广播TF变换
向系统中广播坐标系之间的坐标变换关系。系统中可能存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入TF树中，不需要再进行同步。
（2）监听TF变换
接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。

二、编程实现

（1）创建功能包

 cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim

（2）如何实现一个TF广播器和监听器

广播器：
定义TF广播器（TransformBroadcaster）
创建坐标变换值；
发布坐标变换（sendTransform）

监听器：
定义TF监听器；（TransformListener）
查找坐标变换；（waitForTransform、 lookupTransform）

（3）创建tf广播器代码（C++）

/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 */

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	// 创建tf的广播器，相当于发布话题时定义一个发布器
	static tf::TransformBroadcaster br;

	//根据乌龟当前的位姿。设置相对于世界坐标系的坐标变换
	tf::Transform 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);

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

int main(int argc, char** argv)
{
    // 初始化ROS节点
	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 = node.subscribe(turtle_name+"/pose", 10, &poseCallback);//订阅海龟的位置，发生变化则进入回调函数，进行广播

    // 循环等待回调函数
	ros::spin();

	return 0;
};

需要注意的问题：

对于setOrigin()函数和setRPY()函数
（1）setOrigin()函数的参数类型需要为tf::Vector3类型
（2）假设是要发布一个子坐标系为”turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标。
（3）setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)
（4）为了确保转换正确强烈建议，在转换完后，运行下程序，打开rviz下使用确认下转换是否正确，我在实验中有一次，明明依次填入了三个角度，但是在rviz下发现并不正确，pitch,yaw还存在一个负号的关系。

对于transform:存储变换关系的变量；
（1）ros::Time::now():广播tf使的时间戳；
（2）“world”:父坐标系的名字；
（3）turtle_name:子坐标系的名字，这里因为子坐标系有两个，所以它定义了字符串用于存放名字，方便切换，通常我们把名字填到引号里就可以了。
总结一下，假设你在机器人上应用，如果你知道机器人的位置x,y,z,与三个旋转角roll,pitch,yaw就可以广播一个tf了，如果在是平面移动机器人，则只需要知道x,y与yaw即可。

逻辑梳理：
（1）先从主函数分析，首先常规操作，初始化节点，判断命令行中是否有出现坐标系的输入，若没有，报错，然后生成节点句柄，定义一个subscriber，订阅的是小乌龟的pose消息，循环等待启动回调函数。
（2）回调函数：先定义一个tf广播器，定义一个TF中坐标变换的类型，将订阅到的pose中的相对于世界坐标系下的位置和旋转角度对transform进行设置，最后将坐标变换，当前时间，源坐标系和目标坐标系等信息进行广播、插入TF树中。
（3）我们可以看出该广播函数只需知道小乌龟的pose，就能够将世界坐标系相对于小乌龟坐标系的坐标变换进行广播。

（3）创建tf监听器代码（C++）

/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 */

#include 
#include 
#include 
#include 

int main(int argc, char** argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle2
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);

	// 创建发布turtle2速度控制指令的发布者
	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		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_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
		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));
		turtle_vel.publish(vel_msg);

		rate.sleep();
	}
	return 0;
};

逻辑梳理：
从代码中，我们可以看出，发布的消息类型是geometry_msgs::Twist，这个消息类型就是我们在ROS中常用的速度消息，这个消息主要包括两部分，线速度和角速度，线速度是分别在XYZ轴上移动的速度，而角速度分别是绕XYZ轴旋转的速度。
进入while循环，获得的变换关系的消息的类型和前面的广播的消息类型一致，不过获得的是turtle1和turtle2之间的变换关系，我们广播的是turtle和世界坐标系的变换关系。我们可能会疑惑，为什么发布的消息是和世界坐标系的变换关系而能直接获得两只小海龟的变换关系呢？这是因为我们的广播和监听并不是直接进行的，我们广播和监听的对象都是TF树，TF树中能够直接将两个坐标系的转换关系完成，所以涉及多个坐标系之间的转换我们并不需要自己去做，而是直接去监听TF树中的变换信息，这也是ROS提供坐标转换工具包最大的用处。最后我们获得了两个小海龟之间的坐标系的转换关系，接下来我们便可以设置小海龟。

（4）配置tf广播器与监听器代码编译规则

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

（5）编译并运行

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
$ rosrun learning_tf turtle_tf_listener
$ rosrun turtlesim turtle_teleop_key

注意：
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
这里两句都是运行的同一个功能包，但是在代码中初始化节点的时候却采用了“my_tf_broadcaster”的节点名称，但是我们海龟一和海龟二的代码是一样的都是为了广播它们的坐标系，因此此处为了实现代码复用，采用了命名节点名称的方式，命名规则如下：

①__先是两个下划线
②name:=重命名的名称

这样我们就可以实现具有相同功能的程序节点而避免发生冲突。

（6）效果