ROS学习-tf

tf在ROS里面是很重要的知识点，使用tf的目的是为了让不同的坐标系得到换算，从而得到统一的标准。

学习tf之前，需要先看一些其他的知识：
什么是坐标系？
坐标的表示方法？
怎样换算坐标系①？
怎样换算坐标系②？

还需要了解，如何学习tf？
博客[转][ROS] :TF坐标变换
Wiki[英文]ROS - Wiki tf 教程

可能你看到tf的教程是英文版之后，不想打开进行学习，想在国内搜索引擎搜教程来学习，这可以理解，但大多数的博客都是基于wiki的教程进行翻译，或者理解写下来的。如果真心想学好，还是需要先读一遍wiki教程，对不懂的内容再进行搜索了解。

1️⃣ 如何发布tf坐标变换？
发布tf坐标变换有两种方法，第一种方法是静态发布：当你知道两个坐标之间的换算关系，即A与B距离多远，在A的坐标系下怎样平移旋转得到B的坐标系，那么就可以采用静态发布的方法，通过“static_transform_publisher”指令进行发布。第二种方法是API发布：当你发现坐标系的变换关系有点复杂，而且对象特别多的时候，就需要通过API的方式，编写代码来发布。

①静态发布
静态发布的使用方法可以通过终端指令输入，也可以在编写launch文件的时候将指令规则加入，规则如下：

	<node 
		pkg="tf" 
		type="static_transform_publisher" 
		name="node_name" 
		args=" x x x x x x (x)
		parent_frame_id
		child_frame_id
		time" /> 

pkg指当前调用功能包为tf；type指运行tf包中的类型，可以把它看似函数，也就是调用tf包中的static_transform_publisher函数；name是节点名，因为这是通过节点方式启动，所以需要命名该节点；args里面包含6（或者7）个数字外加三个参数，它是非常重要的内容，下面细讲，先解释三个参数。
parent_frame_id是父坐标系的名字，也就是被作为参照的坐标系；child_frame_id是子坐标系的名字，也就是放弃自己的坐标系；time是该节点的发布频率，也就是时间间隔，单位是ms（毫秒）。

这里需要详细讲解args的数字（上面规则中的x）内容：
args的数字其实有两种写法，一种是欧拉角写法，一种是四元素写法。它们都是为了表示子坐标系在父坐标系中的位置，换句话说，它们是子坐标系到父坐标系的换算关系。

如果是欧拉角写法，x有6个，“x y z yaw pitch roll”这六个数值，其中xyz是子坐标系在父坐标系中的距离关系，难点在于后三个数值，它们是：
yaw（Y）代表绕z轴旋转的角度，偏航
pitch（P） 代表绕y轴旋转的角度，俯仰
roll（R） 代表绕x轴旋转的角度，横滚
而且它们的单位不是°（度），而是rad（弧度角）。1rad=180/pi。1rad≈57.3°。180°=πrad。
如果是四元素写法，x有7个，“x y z qx qy qz qw”这七个数值，但是官网上并没有详细解释，这里暂不深耕，后续了解清楚再补。

②API发布
API的有两种语言的编写方法，这里仅解释C++的写法，Python写法后续深耕。

Wiki教程中，以两只仿真乌龟的例子解释了怎样发布tf坐标变换，这里提取了其中的关键代码如下：

    //声明tf广播对象
    static tf::TransformBroadcaster br;
    
    //声明一个转换对象
    tf::Transform transform;
    
    //将三元素xyz坐标传给转换对象
    transform.setOrigin( tf::Vector3(0.04, 0.0, 0.1) );
    
    //声明一个四元素对象
    tf::Quaternion q;
    
    //将欧拉RPY传给四元素对象
    //R(roll),P(pitch),Y(yaw)
    q.setRPY(0, 0, 0);
    
    //将四元数传给转换对象
    transform.setRotation(q);
    
    //进行广播,StampedTransform有四个值
    //第一个值是 转换对象
    //第二个值是 发布的时间戳,当前时间坐标的位置
    //第三个值是 父坐标系
    //第四个值是 子坐标系
 	br.sendTransform(tf::StampedTransform
 	(transform,ros::Time::now(),frame_id,child_frame_id));

教程的解释内容并不详细，这里附带上注释内容以帮助理解代码，也就不再赘述。

当我读这代码的时候，其实是有很多不理解的，尤其是q.setRPY()，我想不明白这个函数到底是为了做什么，直到翻到RPY的英语，瞬间就明白是欧拉角的三个旋转角度！其次，在理解父坐标系与子坐标系的关系的时候，也搞不清楚到底这父跟子指代的是什么意思，直到转换才明白，父是作为参考系的坐标系。

深入学习tf，还需要了解什么？
Wiki[英文]ROS -Wiki 让机器人使用tf

2️⃣如何接收tf坐标信息？

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "CoP.h"
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

/*这里我们创建一个tf转换接收节点，
将"/laser"坐标系中的一个任意点转换成"/base_link"坐标系中的点，
同时附上该转换的时间戳，以验证tf变换是否成功*/

void transformPoint(const tf::TransformListener& listener){
   //在base_laser坐标系中创建一个点，将转换为base_link坐标系
   geometry_msgs::PointStamped laser_point;
   laser_point.header.frame_id = "/laser";

   //以ros::Time的时间作为时间戳
   laser_point.header.stamp = ros::Time();

   //该点在坐标的位置
   laser_point.point.x = 1.0;
   laser_point.point.y = 0.2;
   laser_point.point.z = 0.0;
 
   try{
     geometry_msgs::PointStamped base_point;
	 /*这里transformPoint有三个参数
	 第一个参数是 目标坐标系，也就是tf变换的父坐标系
	 第二个参数是 等待转换的坐标点，
	 第三个参数是 用于存储转换成功后该点在父坐标系中的坐标值
	 */
     listener.transformPoint("/base_link", laser_point, base_point);

     ROS_INFO("/laser: (%.2f, %.2f. %.2f) -----> /base_link: (%.2f, %.2f, %.2f) at time %.2f",
         laser_point.point.x, laser_point.point.y, laser_point.point.z,
         base_point.point.x, base_point.point.y, base_point.point.z, 
		 base_point.header.stamp.toSec());
   }
   catch(tf::TransformException& ex){
     ROS_ERROR("Received an exception trying to transform a point from \"/laser\" to \"/base_link\": %s", ex.what());
   }
}

int main(int argc, char *argv[])
{

	ros::init(argc, argv, "receivetf");
	ros::NodeHandle n;

	//声明一个tf转换订阅者，参数是 存储转换参数的时间
	static tf::TransformListener listener(ros::Duration(10));
	/*声明该节点的一个定时器 
	第一个参数是 定时时长
	第二个参数是 定时时间到的执行事件
		boost::bind的第一个参数是响应函数，第二个参数是tf转换的订阅者
	*/
	ros::Timer timer = n.createTimer(
		ros::Duration(1.0),
		boost::bind(&transformPoint, boost::ref(listener)));
	
	ros::spin();

	return 0;
}