ROS入门-16.tf坐标系广播与监听的编程实现

介绍这两只海龟跟随背后的原理，怎样通过tf坐标系来完成广播与监听的编程实现
第一步，创建功能包learning_tf

cd ~/catkin_ws/src 在工作空间src下进入终端
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
这里我们需要去依赖roscpp，rospy，tf工具，还有和小海龟相关的turtlesim功能包

我们看到两个海海龟与world之间都有位置关系，这个位置关系要通过tf去广播出来的，看一下在程序里如何来实现
首先看一下如何通过tf来去广播任意两个坐标系之间的位置关系，称之为tf的广播器
第二步，创建tf广播器代码（C++）

将课件代码/learning_tf/src里的turtle_tf_broadcaster.cpp代码文件放置于catkin_ws/src/learning_tf/src里

turtle_tf_broadcaster.cpp代码文件内容

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

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>//包括tf变换的一个broadcaster
#include <turtlesim/Pose.h>    //包括海龟的位置，它是通过海龟仿真器turtlesim不断地往外去发布的，和前面去创建监听者去监听海龟的位置subscribe关系是一样，这里也是有一个subscribe来监听turtlesim里面海龟的实时位置的

std::string turtle_name;

//在函数里会传入海龟的位置，这个位置是海龟在整个坐标系下的X,Y坐标还有#theta(希腊字母)
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	// 创建tf的广播器，实例TransformBroadcaster
	//通过这个实例把海龟1或者海龟2相对于world坐标系的位置关系发出去
	static tf::TransformBroadcaster br;

	// 初始化tf数据
	//填充坐标系之间的映射关系；以turtle1为例，turtle1与world两个坐标系之间的位置关系包括平移和旋转，所以这两个数据组成了坐标变换transform；transform即是在机器人导论里学到的4×4矩阵数据，在transform里填数据：首先是平移即通过transform里的setOrigin来设置平移参数，包括围绕X,Y,Z三轴的平移；通过海龟的位置关系可以得到turtle1相对于world的X轴平移，y轴平移，由于是平面的所以z轴平移永远是0，他们组成向量Vector3，所以我们通过setOrigin把平移关系给设进来；；接下来设置旋转，通过Quaternion即四元数，通过setRPY即围绕X,Y,Z轴三轴的旋转可以设置姿态，这里X,Y方向都是0，只有Z轴有旋转关系，所以通过setRPY完成了两个坐标系之间姿态变化的数据的设置，然后通过setRotation把它设置到transform变量里面来；transform到目前为止，就保存了平移保存了旋转的关系，整个两个坐标系之间的位置关系就可以完全的描述出来，接下来通过Broadcaster br，通过sendTransform这样一个方法就可以把位置关系去发布广播出去，之后ros后台TF树就会把这两个坐标系的关系插入到对应的树中，上节看到的树就是这里的sendTransform广播之后变到树里来的；
	tf::Transform transform;
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);
	transform.setRotation(q);

	// 广播world与海龟坐标系之间的tf数据
	//添加一个时间戳，知道TF是有一个时间的概念的，默认可以保存10秒钟之内，ros::Time::now()表示当前时间；描述的是"world"和turtle_name(就是turtle1和turtle2)两个坐标系之间的位置关系，transform就是前面描述的这两个坐标系之间的位置关系。所以这里通过sendTransform广播出去了两个坐标系是谁，关系怎么样，以及两个坐标系关系是哪个时刻的，因为两个坐标系可能会发生变化；通过这句话后，TF树终究会出现world和turtle1或turtle2的位置关系了
	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");

	// 输入参数作为海龟的名字
	//判断输入参数，因为不管是海龟1还是海龟2，他和world之间的位置关系都是通用的一个程序不需要写两遍，所以通过main函数的输入参数来确定当前的这个程序执行的是turtle1还是turtle2，前者则建立前者和world的关系，后者则建立后者与world的关系。但注意，任意的ROS节点它的节点名在整个ROS环境当中只能有一个，想象一下如果这个程序执行两遍的话肯定会有节点名冲突的问题，后面会发现想办法让节点名换一个名字，这样程序就可以执行两遍了，在ROS中成为重映射
	if (argc != 2)
	{
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}

    //在这获取海龟的名字，argv[1]是字符串
	turtle_name = argv[1];

	// 订阅海龟的位姿话题
	ros::NodeHandle node;  //创建一个句柄，通过句柄来创建一个话题subscribe的订阅者，来订阅海龟仿真器里不断在发布的海龟位置信息turtle_name+"/pose"，记得在turtlesim里默认发布的是turtle1/pose这样一个话题，如果是海龟2的话就是turtle2/pose，所以这里的turtle_name不是turtle1就是turtle2，和后面的pose接到一起就直接订阅仿真器里海龟的位置了，如果一旦有海龟位置在发布的话就会跳到回调函数poseCallback里来
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    // 循环等待回调函数
    //spin会不断在等待队列的数据，一旦有数据进来就会进到poseCallback
	ros::spin();

	return 0;
};

第二步，创建tf监听器代码（C++） 8.59