坐标转换tf

tf介绍

• 坐标转换(TransForm:位置和姿态)
• 坐标变换是空间实体的位置描述，是从一种坐标系统变换到另一种坐标系统的过程。通过建立两个坐标系统之间一一对应关系来实现
• 下图为机器人几个部件之间的坐标关系

tf概念

• tf是一个用户随时间跟踪多个坐标系的包，机器人不同部位和世界的坐标系以 tree structure 的形式存储起来，tf管理一系列的树状结构坐标系之间的关系
• 允许用户在各个坐标系中进行点、向量的变换
• 通俗的说tf可以帮助我们实时的在各个坐标系中进行坐标转换

TF树样例

tf认知

环境搭建，运行已下命令安装相关插件资源
sudo apt-get install ros- kinetic -ros-tutorials ros- kinetic -geometry-tutorials ros- kinetic -rviz ros- kinetic -rosbash ros- kinetic -rqt-tf-tree

运行以下命令打开demo程序
roslaunch turtle_tf turtle_tf_demo.launch

将当前运行命令的窗口作为激活窗口，用箭头按键控制小乌龟移动，另外一只小乌龟讲会跟着移动

• 这个demo用tf库建立了三个坐标系，世界坐标系，乌龟1坐标系，乌龟2坐标系
• 在乌龟移动的过程中，tf库创建了一个tf发布器，用于发布乌龟1的坐标系在世界坐标系中的实时位置
• 利用一个tf接收器，接收该位置
• 计算两个乌龟坐标系的差异，让乌龟2跟着乌龟1移动

view_frames:该命令会以一个树状图的形式，给我们展现出当前ros中tf发布的所有坐标系

运行命令生成当前坐标系树状图：rosrun tf view_frames

查看pdf文件：evince frames.pdf

从上图中可以看到

• tf广播了三个坐标系：世界坐标，乌龟1坐标系，乌龟2坐标系
• world坐标是 turtle1和 turtle2的父级

rqt_tf_tree是一个可以实时可视化显示当前tf发布的坐标系图

输入命令：rosrun rqt_tf_tree rqt_tf_tree

tf_echo 可以报告任意两个坐标系之间的变换。
用法：
rosrun tf tf_echo [reference_frame] [targe_frame]

输入命令：rosrun tf tf_echo turtle1 turtle2

rviz和tf：rviz作为一个可视化工具，可以用来观测turtle坐标系

输入命令：rosrun rviz rviz -drospack find turtle_tf/rviz/turtle_rviz.rviz
用键盘控制小乌龟移动，观测rviz中坐标系的变化

tf消息

• tf/tfMessage.msg
• tf2_msgs/TFMessage.msg
• 查询tf的类型：rostopic info /tf
  

tf相关数据类型

• 向量：tf::Vector3
• 点：tf::Point
• 四元数：tf::Quaternion
• 3x3矩阵（旋转矩阵）：tf::Matrix3x3
• 位姿：tf::Pose
• 变换：tf::Transform
• 带时间戳的以上类型：tf::Stamped<T>
• 带时间戳的变换：tf::StampedTransform

tf::TransformBroadcaster类

TransformBroadcaster()
void sendTransform(const StampedTransform &transform)
void sendTransform(const std::vector<StampedTransform> &transforms)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms)

tf::TransformListener类

void lookupTransform(const std::string &target_frame, 
	const std::string &source_frame, const ros::Time &time,
	StampedTransform &transform) const 

bool canTransform()
bool waitForTransform() const 

操作小记

写一个tf发布器

创建一个tf发布器，发布tf关系，父坐标系为word，子坐标系为turtle_name

创建一个功能包

catkin_create_pkg learning_tf tf roscpp rospy turtlesim

在src文件下新建一个turtle_tf_broadcaster.cpp写入以下代码

#include <ros/ros.h>
#include <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) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  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 = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

修改CMakeLists.txt文件

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

编译：catkin_make

正常情况下将在下图所示的目录下生成turtle_tf_broadcaster节点

在learning_tf下新建一个launch文件夹，并在其中新建一个start_demo.launch文件

<launch>
    <!-- 打开Turtlesim Node，小乌龟-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

	<!-- 打开键盘控制程序 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

	<!-- 以turtle1为命名打开了一个发布器 -->
    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
    
    <!-- 以turtle2为命名打开了一个发布器 -->
    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
   
</launch>

运行命令查看当前tf树

rosrun rqt_tf_tree rqt_tf_tree

思考：为什么我们运行了两个tf_learning节点分别命名为turtle1和turtle2，而只有turtle1的坐标系发布出来了？

提示：可以运行rqt_graph查看节点，得出答案

写一个tf监听器

创建一个订阅者：订阅乌龟的位置

在src文件下新建一个turtle_tf_listener.cpp写入以下代码

// 头文件包含
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.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");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  // 创建一个话题发布器，发布第二只乌龟运行的速度
  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  // 创建tf侦听器listener
  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      // 最多等待10秒，如果提前得到了坐标的转换信息，直接结束等待
      listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0));
      // 从tf上侦听turtle2在turtle1坐标系中的位置,将该位置存储在transform中
      listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0),  transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    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));
    // 计算turtle2跑的速度和方向
    turtle_vel.publish(vel_msg);

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

修改CMakeLists.txt文件

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

编译：catkin_make

在start_demo.launch中添加以下代码：

<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

运行

roslaunch learning_tf start_demo.launch

效果

一般一步实现了前面例子的乌龟跟随

运行 rqt_graph

分析是turtle1跟着turtle2跑还是turtle2跟着turtle1跑？

添加一个坐标系

为什么要添加坐标系？

• 对于许多问题，在它自身的坐标系中思考是非常容易的，比如在二维激光雷达自身的坐标系中，推算障碍物的方位是非常容易的
• tf允许你为系统中所有的传感器，部件定义自身坐标系，前面讲述的urdf建模时，就自动生成了tf树
• 但如果要处理系统外部的问题，如果能够随时添加一个坐标系的话，将会非常方便
• tf所建立的坐标系关系为树状结构，他不允许坐标系结构中存在闭环，也就是说一个坐标系只能有一个父级，但是可以有多个子级
• 目前，我们的tf树包含三个坐标系，world，turtle1，turtle2。turtle1和turtle2为world的子级
  
• 因此如果我们想添加新的坐标系到tf中的话，我们必须选择一个现存的坐标系作为父级

---

操作小记

在src/learning_tf/src下创建文件frame_tf_broadcaster.cpp，写入以下代码：

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

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    // 子坐标系在父坐标系中的初始姿态
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    // 发布一个以turtle1为父级命名为carrot1的坐标系
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

在CMakeList.txt中添加如下代码：

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

编译：catkin_make

在start_demo.launch中添加如下代码：
<node pkg="learning_tf" type="frame_tf_broadcaster" name="broadcaster_frame" />

运行 roslaunch learning_tf start_demo.launch
运行命令查看tf树：rosrun rqt_tf_tree rqt_tf_tree

此时，操作键盘控制乌龟移动，发现跟之前的行为并没有什么区别
在turtle_tf_listener.cpp文件中做以下修改:

try{
      listener.waitForTransform("/turtle2", "/carrot1", ros::Time(0), ros::Duration(10.0));
      listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0),  transform);
}

重新编译运行，查看效果跟之前的区别

---

之前，我们创建了一个固定关系的坐标系，它相对于父级是不会变化的。现在创建一个动态的坐标系
修改frame_tf_broadcaster.cpp中对应的代码:

  while (node.ok()){
    transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }

编译运行，观察现象

tf和时间

• 在前面的内容中我们学习了如何跟踪tf树上的坐标系
• tf树是随时间变化的，tf为tf树上的每个坐标系之间的转换都会缓存一定的时间
• 我们用lookupTransform()函数可获取到最近的转换关系，但是我们并不知道该转换关系对应的时间
• 接下来学习如何得到一个转换关系在一个特定的时间

操作小记

打开turtle_tf_listener.cpp，原来代码为:

ros::Time(0)参数指的就是获取当前最新的转化关系，将代码做如下修改:

该段代码是指获取当前时间点的转换关系
编译：catkin_make
运行：roslaunch learning_tf start_demo.launch
运行时会出现一个错误：该错误是由于turtle2的生成需要一定的时间，当我们第一次要询问转换关系时，turtle2还没有生成

---

如何获取5S前的坐标系转换关系？
修改代码如下:

编译：catkin_make
运行：roslaunch learning_tf start_demo.launch

感觉第二只小乌龟完全不受控！
原因在于，我们获取的相对关系是5s前的，5s前的turtle2和turtle1之间的关系

---

如何让turtle2跟着turtle1 5s前的位置运行呢？

一共包含了6个参数：

1. 要转换的目标坐标系；
2. 在什么时间；
3. 源坐标系；
4. 在什么时间；
5. 不随时间变化的坐标系；
6. 存储结果的变量

上图显示了坐标系转换的过程：

1. tf计算在past的时间点turtle1和世界坐标系之间的关系
2. 随着时间推移到达现在的时间点，计算turtle2和世界坐标系之间的关系
3. tf计算当前turtle2和past时间点turtle1之间的关系

编译：catkin_make
运行：roslaunch learning_tf start_demo.launch