添加一个frame tf2-ros2

在之前的教程中，我们完成了如何写一个broadcaster和一个监听者，该教程会介绍如何向tf树中添加一个固定的或者是动态变化的frame。事实上，添加一个frame的过程非常像创建一个tf2 broadcaser的过程，下面我们会说明一些额外的关于添加frame的特点。

tf2 树

tf2构建的是框架的树形结构，因此在框架结构中不允许出现闭合循环。这意味着一棵树可以有多个子节点，但是只能有一个父节点。目前我们的tf2树有三个节点，两个turtle frame 和一个 world frame.如果我们要添加一个frame ，那么我们就需要让三者之一成为父节点

写一个fixed frame的boardcaster

下面我们会添加一个新的frame，名字为carrot1，它的父节点为turtle1，他会作为第二个乌龟的目标去追踪

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py

打开文件

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class FixedFrameBroadcaster(Node):

   def __init__(self):
      super().__init__('fixed_frame_tf2_broadcaster')
      self.br = TransformBroadcaster(self)
      self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

   def broadcast_timer_callback(self):
      t = TransformStamped()
      t.header.stamp = self.get_clock().now().to_msg()
      t.header.frame_id = 'turtle1'
      t.child_frame_id = 'carrot1'
      t.transform.translation.x = 0.0
      t.transform.translation.y = 2.0
      t.transform.translation.z = 0.0
      t.transform.rotation.x = 0.0
      t.transform.rotation.y = 0.0
      t.transform.rotation.z = 0.0
      t.transform.rotation.w = 1.0

      self.br.sendTransform(t)


def main():
   rclpy.init()
   node = FixedFrameBroadcaster()
   try:
      rclpy.spin(node)
   except KeyboardInterrupt:
      pass

   rclpy.shutdown()

这一段代码和写一个broadcaser非常类似，唯一不同的是，这里的transform是固定的，不要忘记去完善setup.py

注意，from的frame都是在父节点，就是tf树上面的点，to的结点都是子节点。定义的变换都是相对于父节点，比如这里，子节点相对于父节点在y轴上偏差2m。

代码审查

下面看一下关键行的代码

t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0

这里我们创建了一个transform，他的父节点为turtle1，并且距离父节点y轴2米的距离

launch文件

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
   demo_nodes = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
      )

   return LaunchDescription([
      demo_nodes,
      Node(
            package='learning_tf2_py',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
      ),
   ])

build and run

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

你应该看见carrot1

结果检查

如果你使用键盘移动了一个乌龟，你会发现这和原来的结果并没有任何的不同。虽然我们添加了一个frame，但是这个新增的frame并没有对其他的frame产生任何的影响。

因此如果我们想要让第二个乌龟发生改变，我们可以通过修改原来文件中的一个参数，我们可以通过如下设置

def generate_launch_description():
   demo_nodes = IncludeLaunchDescription(
      ...,
      launch_arguments={'target_frame': 'carrot1'}.items(),
      )

这样，可以让turtle2跟着carrot1跑

等价的，我们也可以去修改target_frame

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

这样，他会去查询carrto1在turtle2坐标系上的相对位置，turtle2会去追踪carrot1

写一个动态的frame broadcaster

我们在之前创建了一个固定的frame，他和父节点的关系不会改变，已知在父节点y轴方向2米处。但是，如果您想发布一个非固定的 frame，您可以编boardcaseter程序来随时间改变frame。让我们改变我们的carrto1 frame，使它随着时间相对于turtle1 frame发生变化。现在输入以下命令下载以下代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.py

打开文件

import math

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class DynamicFrameBroadcaster(Node):

   def __init__(self):
      super().__init__('dynamic_frame_tf2_broadcaster')
      self.br = TransformBroadcaster(self)
      self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

   def broadcast_timer_callback(self):
      seconds, _ = self.get_clock().now().seconds_nanoseconds()
      x = seconds * math.pi

      t = TransformStamped()
      t.header.stamp = self.get_clock().now().to_msg()
      t.header.frame_id = 'turtle1'
      t.child_frame_id = 'carrot1'
      t.transform.translation.x = 10 * math.sin(x)
      t.transform.translation.y = 10 * math.cos(x)
      t.transform.translation.z = 0.0
      t.transform.rotation.x = 0.0
      t.transform.rotation.y = 0.0
      t.transform.rotation.z = 0.0
      t.transform.rotation.w = 1.0

      self.br.sendTransform(t)


def main():
   rclpy.init()
   node = DynamicFrameBroadcaster()
   try:
      rclpy.spin(node)
   except KeyboardInterrupt:
      pass

   rclpy.shutdown()

代码审查

现在我们不再固定y轴为2.0，利用sin和cos函数实时改变转换

seconds, _ = self.get_clock().now().seconds_nanoseconds()
x = seconds * math.pi
...
t.transform.translation.x = 10 * math.sin(x)
t.transform.translation.y = 10 * math.cos(x)

launch 文件

新建文件turtle_tf2_dynamic_frame_demo.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
   demo_nodes = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
      launch_arguments={'target_frame': 'carrot1'}.items(),
      )

   return LaunchDescription([
      demo_nodes,
      Node(
            package='learning_tf2_py',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
      ),
   ])

build and run