ROS笔记——tf library相关

最新推荐文章于 2024-06-16 09:37:25 发布

RavenRaaven

最新推荐文章于 2024-06-16 09:37:25 发布

阅读量551

点赞数

分类专栏： ubuntu与ros

本文链接：https://blog.csdn.net/qq_41871826/article/details/108021747

版权

ubuntu与ros 专栏收录该内容

6 篇文章 3 订阅

订阅专栏

概要

坐标系变换在机器人相关的应用中非常重要。没有坐标系的变换，机器人无法获取自己和环境的准确信息。本文是我在看完官方tutorial，并大致看了下tf的论文《tf: The Transform Library》之后做的一些笔记。

有不准确不恰当的地方希望大家指正！

1. tf库基本介绍

http://wiki.ros.org/tf

tf is a package that lets the user keep track of multiple coordinate frames over time. tf maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.

tf可以在不同的时刻跟踪不同的坐标系，并且tf会以树形结构保留不同坐标系和他们之间的关系，用户可以在不同坐标系之间变换点、向量等。

以下部分基于tf的论文：《tf: The Transform Library》

需要坐标变换的原因就在于机器人在执行任务的过程中，需要知道自己的准确位置并且知道世界中其余元素的相对位置。tf库是由两大模块组成：Broadcaster和Listener。前者用于把变换的信息（平移量和旋转量）发送给整个系统，后者接收这些坐标变换的信息并存储到指定变量中。

tf库的设计所使用到数据结构是树形结构（有向无环图）。其中坐标系代表图的节点，坐标系之间的变换代表图中的边。而合变换（net transform）就是连接两个节点的这些边的乘积。该树形结构保证了节点连通性的快速搜索。论文中也提到了，tf使用树形结构的优势是：1. efficiency; 2. flexibility (等需要深入学习这部分的时候再重新看一下论文)

2. tf tutorial及tf库中的其他函数

该部分是对官方wiki相关内容的汇总及整理，并且对tf库中其他函数API和内容进行了查阅。

官方wiki上tf turtorial系列的链接：http://wiki.ros.org/tf/Tutorials

I. introduction:

这里的demo实现的主要功能就是：一只乌龟跟踪另一只乌龟。具体来说就是：用tf library建立三个坐标系：/world, /turtle1, /turtle2; 之后使用tf broadcaster 发布turtle1的坐标系，并用tf listener计算两个坐标系之间的不同并让第二只乌龟跟踪第一个。

先安装必要的节点：

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

这一部分重点介绍了几个常用的tf工具：
a. view_frames:
功能是创建坐标系之间的关系图。

在上一步启动好的情况下运行以下指令：

rosrun tf view_frames

结果示意图如下：
在这里插入图片描述
从中可以看出，/world是/turtle1和/turtle2坐标系的父节点。

b. rqt_tf_tree:
rqt_tf_tree出来的结果和view_frames类似，是可视化坐标系之间树形结构的运行时工具。

指令如下：

rosrun rqt_tf_tree rqt_tf_tree

c. tf_echo

tf_echo reports the transform between any two frames broadcast over ROS.

tf_echo是用于打印具体的坐标变换信息，如下图：
在这里插入图片描述

语句命令格式：

rosrun tf tf_echo [reference_frame] [target_frame]

适用于demo中的指令：

rosrun tf tf_echo turtle1 turtle2

d. rviz and tf

rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

II. tf broadcaster (C++)

这里broadcaster的作用：将乌龟的位姿信息发送给tf。具体步骤（比如创建工作区和功能包、设置相关依赖，代码写完之后的编译过程等）请参考官方wiki, 我这里只是记录一下代码中重要的函数。

相关代码解释：

定义TransformBroadcaster类

#include <tf/transform_broadcaster.h>

创建TransfromBroadcaster对象

static tf::TransformBroadcaster br;

创建Transform类的对象（该类的解释见后文）

  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);

（这里相当于把乌龟的2D位姿变换为3D坐标）

用Broadcaster对象中的方法函数发送transform:

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

sendTransform所需要的参数：

第一个参数传入transform本身;
第二个参数是时间戳ros::Time::now()（给将要发布的transform一个时间戳，将其与现在的时间绑定在一起）；
第三个参数是父节点坐标系的名字；
第四个参数需要传入子节点坐标系的名字；

编译过程需要注意的是：
除了写完代码之后修改CMakeLists.txt文件：

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

还要将catkin_package中的CATKIN_DEPENDS的注释打开

catkin_package(
 # INCLUDE_DIRS include
 #  LIBRARIES learning_tf
 CATKIN_DEPENDS roscpp rospy tf turtlesim
 #  DEPENDS system_lib
)

https://blog.csdn.net/jiejinquanil/article/details/102829500

III. tf listener (C++)

代码解释
头文件部分需要注意的事项：
用于速度的头文件是：geometry_msgs/Twist.h.键盘的输入是/turtle/cmd_vel.

包含TransformListener类

#include <tf/transform_listener.h>

创建TransformListener对象

 tf::TransformListener listener;

用户向listener查询具体的变换：(这里使用的是try...catch语句，防止找不到变换之后产生的报错使整个程序崩溃)

try{
  listener.lookupTransform("/turtle2", "/turtle1",
                           ros::Time(0), transform);
}

lookupTransform有4个参数：
前两个参数的目的是将turtle 1变换到turtle 2；
第三个参数：变换的时间，设置ros::Time(0) 是为了获取最近一次的变换；
第四个参数：将获得的变换存储到该变量；

用以上获取的transform对Turtle2的角速度和线速度进行设置，代码片段如下：

    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                pow(transform.getOrigin().y(), 2));

IV. 添加坐标系

Why to add?

在一些任务中，使用对象的局部坐标系可能会更方便。tf可以让用户为传感器定义局部坐标系进行定义并处理这些坐标系和基坐标系之间的变换。

where to add?

tf中的坐标系是满足树形结构的，不允许闭环出现。树形结构要求这些坐标系只有一个父节点，但可以有多个子节点。所以添加的时候需要添加到某个父节点下。

how to add?

关键代码片段：

transform.setOrigin(tf::Vector3(2.0 * sin(ros::Time::now().toSec()),2.0 * cos(ros::Time::now().toSec()),0.0));
//transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));

相关函数的参数说明见前文，其中carrot1是turtle1的子节点，carrot在其左侧2m处，当然也可以将其设置为不断变化的点。

V. tf and time

每个坐标系组成的frame tree都随时间的变化而变化。上文中的lookupTransform()只是记录最近的一次可行变换，并没有给出变换是何时发生的。tf可以获取在buffer允许的前提下任意时刻的坐标变换。

上文中的代码片段：

  try{
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time(0), transform);

time 0 means “the latest available” transform in the buffer

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

waitForTransform()需要四个参数：

transform from this frame
to this frame
at this time
timeout

waitForTransform() will actually block until the transform between the two turtles becomes available (this will usually take a few milliseconds), OR --if the transform does not become available-- until the timeout has been reached.

从设定的当前时刻开始，该函数只有在两个坐标系的变换是可行的情况下才会解阻塞。

VI. Time travel with tf

如果执行以下代码片段会出现第二只乌龟杂乱无章地运动的结果。

  try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

其实是需要提供给该函数更多参数的，需要有一个参考坐标才能实现我们像想要的第二只乌龟跟随第一只乌龟运动，轨迹只有一条。修改后的代码如下：

  try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

使用到的lookupTransform()的高级API有如下6个参数：

a. Give the transform from this frame,
b. at this time …
c. … to this frame,
d. at this time.
e. Specify the frame that does not change over time, in this case the “/world” frame, and
f. the variable to store the result in.

waitForTransform()函数有跟其一样的参数列表。

该函数的过程
tf先在past时间点上计算/turtle1到/world的变换；紧接着在world坐标系下，tf的时间从past到了now；最后tf再在now的时间点上计算/world到/turtle2的变换。

其结果就是五秒之后，第二个乌龟跟上了第一个乌龟，轨迹只有一条而不是杂乱无章。
在这里插入图片描述

VII. Setting up your robot using tf

http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

A transform tree defines offsets in terms of both translation and rotation between different coordinate frames. Tf uses a tree structure to guarantee that there is only a single traversal that links any two coordinate frames together, and assumes that all edges in the tree are directed from parent to child nodes.

可以看出tf中的坐标变换形成的树形结构的重要性，保证了两个坐标系之间只有唯一一个变换。

坐标系命名的约定俗成可参考：https://www.ros.org/reps/rep-0105.html

代码片段:
这一部分类似前文中Broadcaster发送transform的写法。

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

sendTransform需要五个参数：（上文中的该函数需要四个参数，其实本质上是一样的，上文中的第一个参数是Transform类的对象，其包含了以下的1，2两点）

旋转变换（quaternion）
平移变换（vector3）
时间戳timestamp
父节点坐标系
子节点坐标系

发送了transform之后，可以用tf中的Listener接收这些变换，并且可以使用transformPoint函数实现不同坐标系下相同点的描述。比如这里就是将base_laser中的点转化到base_link中。

在这里插入图片描述

listener.transformPoint("base_link", laser_point, base_point);

这里的transformPoint()需要三个参数:
a. the name of the frame we want to transform the point to (“base_link” in our case); (在base_laser中的点已经加戳了，其所在坐标系已经给定了，所以该函数参数列表中就不需要再添加base_laser了)
b. the point we’re transforming,
c. and storage for the transformed point.

tf命名空间的其他函数

除了以上tutorial中的相关内容，以下是我在autoware中遇到与tf有关的一些类或者函数。

tf相关API 的查询可以参考以下链接：http://docs.ros.org/kinetic/api/tf/html/c++/namespacetf.html#aeb5330596aec7928df8aa1ea3a4628a2

1. tf::poseMsgToTF

函数原型：

static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt)

函数内容：

static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt)
{
bt = Transform(Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w), Vector3(msg.position.x, msg.position.y, msg.position.z));
};

这里的Pose就是tf::Transform类。该函数的目的就是将geometry_msgs::Pose的类型转化为tf::Transform类

tf::Pose
typedef tf::Transform tf::Pose

2. tf::Transform类

The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes.

Transform类的构造函数有以下四种（原理就是构造函数的重载）：

no initialization constructor
通过quaternion和vector3进行构造
通过3x3矩阵和vector3进行构造
拷贝其他的transform类型
（详细的语法可以参考以上查询链接）

该类的定义下由两个成员数据

Matrix3x3 m_basis;
Vector3   m_origin;

与之类似的是tf::Point, 定义为：

typedef tf::Vector3 tf::Point

tf::Pose::setOrigin()的函数的准确定义：

TFSIMD_FORCE_INLINE void setOrigin(const Vector3& origin) 
{
	m_origin = origin;
}

该函数的功能是设置位移元素。这里的原点origin: the vector to set the translation to

tf::Pose::setRotation()的函数准确定义：

TFSIMD_FORCE_INLINE void setRotation(const Quaternion& q)
{
	m_basis.setRotation(q);
}

该函数的功能是：
设置旋转量（set the rotational element by quaternion），里面用到的子函数：setRotation来自Matrix3x3类

void tf::Matrix3x3::setRotation(const Quaternion& q)

该函数的功能是将四元数转化为3x3的旋转矩阵

3. tf::Quaternion类

tf::Quaternion类的构造函数也有以下四种：

no initialization constructor
construct from scalars, x,y,z,w
Axis angle
(axis: the rotation is around; angle: the magnitude of the rotation around the angle, radians)
pitch yaw and roll (没有定义TF_EULER_DEFAULT_ZYX 的时候，分别代表Y,X,Z，否则代表Z,Y,X)

tf::createQuaternionFromYaw：
函数原型：

static Quaternion tf::createQuaternionFromYaw(double yaw)

函数功能：
从yaw中构建一个Quaternion数据
函数输入：
yaw: The yaw about the Z axis

函数返回：
The quaternion constructed