< >
Home » ROS2与tf2入门教程 » ROS2与tf2入门教程-添加框架 (Python)

ROS2与tf2入门教程-添加框架 (Python)

说明:

  • 介绍如何用Python添加框架

概述：

  • 在之前的教程中，我们通过编写一个 tf2 广播器和一个 tf2 监听器重新创建了海龟演示。

  • 本教程将教您如何向转换树添加额外的固定和动态帧。

  • 实际上，在 tf2 中添加框架与创建 tf2 广播器非常相似，但是这个示例将向您展示 tf2 的一些附加功能。

  • 对于许多与转换相关的任务，在本地框架内思考会更容易。

  • 例如，最容易推断激光扫描仪中心框架中的激光扫描测量值。

  • tf2 允许您为系统中的每个传感器、链接或关节定义一个本地框架。

  • 当从一帧转换到另一帧时，tf2 将处理所有引入的隐藏中间帧转换。

tf2树：

  • tf2 建立了框架的树状结构，因此不允许框架结构中出现闭环。
  • 这意味着一个框架只有一个父级，但它可以有多个子级。
  • 目前，我们的 tf2 树包含三个框架：world、turtle1和turtle2。
  • 两个海龟框架turtle1和turtle2是框架world的子框架。
  • 如果我们想在tf2中添加一个新的frame，三个已经存在的frame中的一个需要是父frame，新的frame会成为它的子frame

请输入图片描述

固定帧广播器：

  • carrot1坐标系与turtle1坐标系在y轴间隔2米
  • 下载固定帧广播器代码
cd ~/tf2_ws/src/learning_tf2_py/learning_tf2_py
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.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()

　- 赋执行权限

sudo chmod +x fixed_frame_tf2_broadcaster.py

　- 添加入口，修改setup.py在'console_scripts': [里添加

'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

　- 新建launch文件

cd ~/tf2_ws/src/learning_tf2_py/launch
vim turtle_tf2_fixed_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']),
      )

   return LaunchDescription([
      demo_nodes,
      Node(
            package='learning_tf2_py',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
      ),
   ])
  • 构建
colcon build --symlink-install --packages-select learning_tf2_py
  • 加载工作空间
. ~/tf2_ws/install/local_setup.bash

测试：　

　- 启动turtle_tf2_fixed_frame_demo.launch.py文件

. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

  • 生成TF树

    ros2 run tf2_tools view_frames

  • 查看TF树

evince frames.pdf
  • 新carrot1框架出现在转换树中,如图

请输入图片描述

通过参数指定海龟2跟随不同的坐标系

  • 可以这样运行
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1
  • 修改文件turtle_tf2_fixed_frame_demo.launch.py文件，并通过参数添加参数
  • 修改后内容如下：
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    target_frame = LaunchConfiguration('target_frame')
    
    declare_target_frame_cmd = DeclareLaunchArgument(
        'target_frame',
        default_value='turtle1',
        description='target_frame')

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

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

  • 通过DeclareLaunchArgument定义参数target_frame

  • 通过LaunchConfiguration获取target_frame的值,定义为变量target_frame

  • 最后launch_arguments里面再赋值给turtle_tf2_demo.launch.py的target_frame参数

  • 重新构建

colcon build --symlink-install --packages-select learning_tf2_py
  • 重新启动turtle_tf2_fixed_frame_demo.launch.py
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1
  • 键盘控制
ros2 run turtlesim turtle_teleop_key
  • 指定跟随carrot1坐标系,打开后, 第二只海龟, 会停留在第一只海龟的一侧2米附近.
  • 指定跟随turtle1坐标系,打开后, 第二只海龟, 会叠在第一只海龟上

请输入图片描述

动态帧广播器代码

  • carrot1坐标系与turtle1坐标系在在X和Y轴上随机变化
  • 下载动态帧广播器代码
cd ~/tf2_ws/src/learning_tf2_py/learning_tf2_py
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.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()
  • 赋执行权限
sudo chmod +x dynamic_frame_tf2_broadcaster.py
  • 添加入口，修改setup.py在'console_scripts': [里添加
'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',
  • 创建一个新的启动文件
cd ~/tf2_ws/src/learning_tf2_py/launch
vim 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 DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    target_frame = LaunchConfiguration('target_frame')
    
    declare_target_frame_cmd = DeclareLaunchArgument(
        'target_frame',
        default_value='carrot1',
        description='target_frame')

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

    return LaunchDescription([
      declare_target_frame_cmd,
      demo_nodes,
      Node(
            package='learning_tf2_py',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
      ),
    ])
  • 重新构建包
cd ~/tf2_ws/
colcon build --symlink-install --packages-select learning_tf2_py
  • 并启动turtle_tf2_dynamic_frame_demo.launch.py启动文件
ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo.launch.py
  • 默认第二只海龟跟随carrot1坐标系随机位置,您将看到第二只海龟跟随不断变化的位置。

请输入图片描述

参考:

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与tf2入门教程