ROS tf2 库的基本使用以及变换 (Transform) 的理解方式

ROS (ROS1 C++) 中，使用 geometry_msgs::TransformStamped::transform 和 tf2::Transform 处理坐标架之间的变换（transform），前者是 ROS 的消息（message）类型，后者是 ROS 的 tf2 库提供的带有计算功能的类。想要处理 ROS 中全局的变换关系，需要用到 tf2_ros 库，它可以订阅相关消息，维护和计算指定坐标架之间的变换。

Python 与 C++ 基本类似，不同之处在于 Python 中变换的计算通常使用 tf.transformations，本文不详细介绍 Python 中的情况。

使用 `tf2_ros` 库维护和计算坐标架之间的变换

常常定义以下对象。

cpp 复制代码

tf2_ros::TransformBroadcaster tf_broadcaster;
tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener{this->tf_buffer};

其大致工作原理是，tf2_ros::TransformListener 订阅 /tf 和 /tf_static 这两个话题，这两个话题的消息类型给出了某个时间戳下从一个坐标架到另一个坐标架的变换关系。然后，将变换关系直接保存到 tf2_ros::Buffer 中，作为所谓的 tf 树的边（当然内部会作一些维护，比如舍弃太久远的或者重复的变换关系）。需要知道某个时间戳从一个坐标架到另一个坐标架下的变换关系时，只需要问 Buffer，Buffer 会根据保存的一系列变换关系计算出想要的变换关系，并尽可能优化重复查询、查询静态变换的效率。至于 tf2_ros::TransformBroadcaster，它负责将变换关系发布到话题 /tf 中。

我们只需要关注 TransformBroadcaster 和 Buffer 的使用。TransformBroadcaster 通常只使用其名为 sendTransform 的方法，它接受一个类型为 geometry_msgs::TransformStamped 的参数，样例代码如下。

cpp 复制代码

TransformStamped trans;
trans.header.frame_id = this->local_path.header.frame_id;
trans.header.stamp = pose.header.stamp;
trans.child_frame_id = "base_link_alt";
trans.transform.translation.x = local_pose.pose.position.x;
trans.transform.translation.y = local_pose.pose.position.y;
trans.transform.translation.z = local_pose.pose.position.z;
trans.transform.rotation = local_pose.pose.orientation;
this->tf_broadcaster.sendTransform(trans);

TransformStamped 表示在 header.stamp 这个时间点，frame_id 到 child_frame_id 的变换关系为 transform，在 tf 树中，有一条时间戳为 header.stamp 的，以 frame_id 为父结点，child_frame_id 为子结点的边。

Buffer 可以使用 lookupTransform 查询变换关系，它包含以下参数：

target_frame、source_frame：假设有一个 header.frame_id 为 source_frame 的 PoseStamped，则使用查询出的结果对其进行变换后，得到的结果是一个 target_frame 下的 PoseStamped。
time：要查询的变换的时间戳。如果 Buffer 里面最新的结果都不到这个时间，则要么等待，要么失败，要么外推。特别地，如果 time 是 ros::Time(0)，则查询最新的结果。
timeout：如果暂时查不到指定时间戳的变换，便阻塞等待，最多等待该参数指定的事件。特别地，如果 timeout 是 ros::Duration(0)，则不等待，找不到就立即抛出异常。

还要其他重载和其他方法，这里不再赘述。样例代码如下。

cpp 复制代码

PoseStamped pose_in_base_link;
try {
    auto const trans =                     //
        this->tf_buffer.lookupTransform(   //
            "base_link",                   //
            obstacle.pose.header.frame_id, //
            ros::Time()                    //
        );
    tf2::doTransform(obstacle.pose, pose_in_base_link, trans);
} catch (tf2::TransformException const& e) {
    continue;
}

变换的理解方式

前面提到，TransformStamped 表示在 header.stamp 这个时间点，frame_id 到 child_frame_id 的变换关系为 transform。所谓"变换关系"，可以有以下两种理解方式：

主动观点：child_frame_id 下的位姿经过该变换后变到 frame_id 坐标架下。

<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> p base_link in A = t B → A ⋅ p base_link in B p_{\text{base\link}~\text{in}~A} = t{B \to A} \cdot p_{\text{base\_link}~\text{in}~B} </math>pbase_link in A=tB→A⋅pbase_link in B

上式中， <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 表示 frame_id 坐标架， <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B 表示 child_frame_id 坐标架， <math xmlns="http://www.w3.org/1998/Math/MathML"> t B → A t_{B \to A} </math>tB→A 表示从 child_frame_id 到 frame_id 的变换，即 transform。这就是为什么 lookupTransform 的第一个参数为 target_frame，第二个参数 source_frame，查询到的变换的 frame_id 是 target_frame，child_frame_id 是 source_frame。

为了方便之后链式消除，将 <math xmlns="http://www.w3.org/1998/Math/MathML"> B → A B \to A </math>B→A 的箭头反着写，上式可以记作：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> p base_link in A = t A ← B ⋅ p base_link in B p_{\text{base\link}~\text{in}~A} = t{A \leftarrow B} \cdot p_{\text{base\_link}~\text{in}~B} </math>pbase_link in A=tA←B⋅pbase_link in B
被动观点：frame_id 坐标架经过该变换就是 child_frame_id 坐标架。

<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> p B in A = t A ← B ⋅ I \boxed{ p_{B~\text{in}~A} = t_{A \leftarrow B} \cdot I } </math>pB in A=tA←B⋅I

<math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架自己（恒等变换 <math xmlns="http://www.w3.org/1998/Math/MathML"> I I </math>I）经过"该变换"就是 <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B，平移和旋转在数值上等于 <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B 坐标架相对 <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架的位姿。可以看出，位姿和变换可以一一对应， <math xmlns="http://www.w3.org/1998/Math/MathML"> p B in A p_{B~\text{in}~A} </math>pB in A 等价于 <math xmlns="http://www.w3.org/1998/Math/MathML"> t A ← B t_{A \leftarrow B} </math>tA←B，这也是为什么使用 tf2::toMsg 可以在 Pose 和 Transform 之间互相转换。

下面从思想上验证以上说法。

想象二维平面下， <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架（frame_id）在左下角， <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B 坐标架（child_frame_id）在右上角，两个坐标架是姿态相同的平面直角坐标架。
主动观点下， <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B 坐标架下的一个坐标要变到 <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架下， <math xmlns="http://www.w3.org/1998/Math/MathML"> x , y x, y </math>x,y 都需要加上一个正数。
被动观点下， <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架要变成 <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B 坐标架， <math xmlns="http://www.w3.org/1998/Math/MathML"> x , y x, y </math>x,y 也都需要加上一个正数。
主动观点的公式中，把姿态换成变换，有：

<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> t A ← base_link = t A ← B ⋅ t B ← base_link t_{A \leftarrow \text{base\link}} = t{A \leftarrow B} \cdot t_{B \leftarrow \text{base\_link}} </math>tA←base_link=tA←B⋅tB←base_link

符合链式消除的特征，因此在一些计算场合下可以利用矩阵乘法的结合律提前算好。

使用以上理解方式，可以公式化地解决以下样例问题。

将在坐标架 A 的位姿转换到坐标架 B 中

例如，相机（ <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B）看到一个物体（ <math xmlns="http://www.w3.org/1998/Math/MathML"> C C </math>C），该物体相对相机的位姿为 <math xmlns="http://www.w3.org/1998/Math/MathML"> p C in B p_{C~\text{in}~B} </math>pC in B，我们想知道该物体相对 base_link ( <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A) 的位姿，已知 base_link 相对相机的位姿，tf 树中 base_link ( <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A) 为父结点，相机（ <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B）为子结点。将被动观点转换为主动观点，我们根据 tf 树知道了"base_link 相对相机的位姿"，实际上是知道了 <math xmlns="http://www.w3.org/1998/Math/MathML"> t A ← B t_{A \leftarrow B} </math>tA←B。

现在我们已知 <math xmlns="http://www.w3.org/1998/Math/MathML"> p C in B p_{C~\text{in}~B} </math>pC in B，希望知道 <math xmlns="http://www.w3.org/1998/Math/MathML"> p C in A p_{C~\text{in}~A} </math>pC in A，把姿态转换成变换，可以写出公式：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> t A ← C = t A ← B ⋅ t B ← C t_{A \leftarrow C} = t_{A \leftarrow B} \cdot t_{B \leftarrow C} </math>tA←C=tA←B⋅tB←C

因此，将位姿变成变换后，左乘查询到的变换关系，就可以得到姿态在另一个坐标架下的结果。

tf2 库提供了 doTransform 函数便捷地实现这个功能，下面是其源码。

cpp 复制代码

/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type.
* This function is a specialization of the doTransform template defined in tf2/convert.h.
* \param t_in The pose to transform, as a Pose3 message.
* \param t_out The transformed pose, as a Pose3 message.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
template <>
inline
void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform)
{
  tf2::Vector3 v;
  fromMsg(t_in.position, v);
  tf2::Quaternion r;
  fromMsg(t_in.orientation, r);

  tf2::Transform t;
  fromMsg(transform.transform, t);
  tf2::Transform v_out = t * tf2::Transform(r, v);
  toMsg(v_out, t_out);
}

已知坐标架 A 到 C 的变换，和 B 到 C 的变换，求坐标架 A 到 B 的变换

例如，传感器（ <math xmlns="http://www.w3.org/1998/Math/MathML"> C C </math>C）得到 <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架下的位姿，又知道 base_link ( <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B) 相对传感器（ <math xmlns="http://www.w3.org/1998/Math/MathML"> C C </math>C）的位姿，求 base_link 在 <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 坐标架下的位姿。由于收到的是传感器的原始输出数据，tf 树实际上没有维护 <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 到 <math xmlns="http://www.w3.org/1998/Math/MathML"> B B </math>B 或者 <math xmlns="http://www.w3.org/1998/Math/MathML"> A A </math>A 到 <math xmlns="http://www.w3.org/1998/Math/MathML"> C C </math>C 的边，因此需要我们自己计算。

我们已知 <math xmlns="http://www.w3.org/1998/Math/MathML"> p C in A p_{C~\text{in}~A} </math>pC in A 和 <math xmlns="http://www.w3.org/1998/Math/MathML"> t B ← C t_{B \leftarrow C} </math>tB←C，要求 <math xmlns="http://www.w3.org/1998/Math/MathML"> p B in A p_{B~\text{in}~A} </math>pB in A。将位姿变成变换后，我们已知 <math xmlns="http://www.w3.org/1998/Math/MathML"> t A ← C t_{A \leftarrow C} </math>tA←C， <math xmlns="http://www.w3.org/1998/Math/MathML"> t B ← C t_{B \leftarrow C} </math>tB←C，要求 <math xmlns="http://www.w3.org/1998/Math/MathML"> t A ← B t_{A \leftarrow B} </math>tA←B，显然有：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> t A ← B = t A ← C ⋅ ( t B ← C ) − 1 t_{A \leftarrow B} = t_{A \leftarrow C} \cdot (t_{B \leftarrow C})^{-1} </math>tA←B=tA←C⋅(tB←C)−1

其中 <math xmlns="http://www.w3.org/1998/Math/MathML"> ( t B ← C ) − 1 (t_{B \leftarrow C})^{-1} </math>(tB←C)−1 就是 <math xmlns="http://www.w3.org/1998/Math/MathML"> t C ← B t_{C \leftarrow B} </math>tC←B。

下面是具体案例源码。由于 tf_buffer 可以帮我们直接查询 <math xmlns="http://www.w3.org/1998/Math/MathML"> t C ← B t_{C \leftarrow B} </math>tC←B，所以我们不用自己取逆了。

cpp 复制代码

auto const ins_to_world = tf2::Transform(r, t);
tf2::Transform base_link_to_world;
try {
  auto const transform_stamped = this->tf_buffer.lookupTransform("ins", "base_link", ros::Time(0));
  tf2::Transform base_link_to_ins;
  tf2::fromMsg(transform_stamped.transform, base_link_to_ins);
  base_link_to_world = ins_to_world * base_link_to_ins;
} catch (tf2::TransformException const& e) {
  return std::nullopt;
}

ROS tf2 库的基本使用以及变换 (Transform) 的理解方式

使用 tf2_ros 库维护和计算坐标架之间的变换

变换的理解方式

将在坐标架 A 的位姿转换到坐标架 B 中

已知坐标架 A 到 C 的变换，和 B 到 C 的变换，求坐标架 A 到 B 的变换

使用 `tf2_ros` 库维护和计算坐标架之间的变换