프레임 추가 (Python)

목표: tf2에 추가적인 프레임을 추가하는 방법을 배웁니다.

배경

이전 튜토리얼에서는 tf2 브로드캐스터 및 tf2 리스너 를 작성하여 거북이 데모를 재생성했습니다. 이 튜토리얼에서는 변환 트리에 추가로 고정 및 동적 프레임을 추가하는 방법을 알려줍니다. 실제로 tf2에 프레임을 추가하는 것은 tf2 브로드캐스터를 만드는 것과 매우 유사하지만 이 예제에서는 tf2의 몇 가지 추가 기능을 보여줍니다.

변환 작업과 관련된 많은 작업에서는 지역 프레임 내에서 생각하는 것이 더 쉽습니다. 예를 들어, 레이저 스캔 측정 값을 레이저 스캐너 중심의 프레임에서 생각하는 것이 가장 쉽습니다. tf2를 사용하면 시스템 내의 각 센서, 링크 또는 조인트마다 지역 프레임을 정의할 수 있습니다. 한 프레임에서 다른 프레임으로 변환할 때 tf2는 도입되는 모든 중간 프레임 변환을 처리합니다.

tf2 트리

tf2는 프레임의 트리 구조를 작성하며, 프레임 구조에 닫힌 루프를 허용하지 않습니다. 이것은 프레임이 하나의 단일 부모만 가지지만 여러 개의 자식을 가질 수 있음을 의미합니다. 현재 우리의 tf2 트리에는 world, turtle1, turtle2 라는 세 개의 프레임이 있습니다. 두 개의 turtle 프레임은 world 프레임의 자식들입니다 새로운 프레임을 tf2에 추가하려면 기존의 세 개 프레임 중 하나가 부모 프레임이어야하며, 새로운 프레임이 그의 자식 프레임이 될 것입니다.

작업

1 고정 프레임 브로드캐스터 작성

거북이 예제에서 새 프레임인 carrot1 을 추가할 것입니다. 이 프레임은 turtle1 의 자식이 될 것입니다. 이 프레임은 두 번째 거북이의 목표 역할을 합니다.

먼저 소스 파일을 작성합니다. 이전 튜토리얼에서 생성한 learning_tf2_py 패키지로 이동합니다. 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.tf_broadcaster = 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.tf_broadcaster.sendTransform(t)


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

    rclpy.shutdown()

이 코드는 tf2 브로드캐스터 튜토리얼 예제와 매우 유사하지만 여기서는 변환 값이 시간이 지남에 따라 변경되지 않습니다.

1.1. 코드 검토

이 코드에서 주요 라인을 살펴보겠습니다. 우리는 여기서 새로운 변환을 생성하며 부모 turtle1 에서 자식 carrot1 로 변환합니다. carrot1 프레임은 turtle1 프레임과 비교하여 y 축에서 2 미터 떨어진 위치에 있습니다.

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

1.2. 엔트리 포인트 추가

ros2 run 명령이 노드를 실행하도록하려면 setup.py (src/learning_tf2_py 디렉토리에 위치)에 엔트리 포인트를 추가해야합니다.

다음 줄을 'console_scripts': 구문 내에 추가합니다.

'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

1.3. 런치 파일 작성

이제 이 예제를 위한 런치 파일을 만들어 봅시다. 텍스트 편집기를 사용하여 launch/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',
        ),
    ])

이 런치 파일은 필요한 패키지를 가져오고 이전 튜토리얼의 런치 파일에서 생성한 노드를 저장할 demo_nodes 변수를 만듭니다.

코드의 마지막 부분은 고정 carrot1 프레임을 fixed_frame_tf2_broadcaster 노드를 사용하여 turtlesim 월드에 추가합니다.

Node(
    package='learning_tf2_py',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

1.4. 빌드

작업 공간의 루트에서 빠진 종속성을 확인하려면 rosdep 를 실행하십시오.

rosdep install -i --from-path src --rosdistro humble -y

작업 공간의 루트에서 패키지를 빌드하십시오.

colcon build --packages-select learning_tf2_py

새 터미널을 열고 작업 공간의 루트로 이동하고 설정 파일을 소스로합니다.

. install/setup.bash

1.5. 실행

이제 거북이 브로드캐스터 데모를 시작할 수 있습니다.

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

새로운 carrot1 프레임이 변환 트리에 나타난 것을 볼 수 있어야합니다.

../../../_images/turtlesim_frames_carrot.png

첫 번째 거북이를 주위로 움직이면 새 프레임을 추가했지만 동작이 이전 튜토리얼과 변경되지 않았음을 알 수 있습니다. 이것은 추가 프레임이 다른 프레임에 영향을 미치지 않으며 리스너가 이전에 정의된 프레임을 사용하기 때문입니다.

따라서 두 번째 거북이가 첫 번째 turtle가 아닌 carrot을 따라가기를 원한다면 target_frame 의 값을 변경해야합니다. 이것은 두 가지 방법으로 수행 할 수 있습니다. 하나는 콘솔에서 런치 파일로 target_frame 인수를 직접 전달하는 것입니다.

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

두 번째 방법은 런치 파일을 업데이트하는 것입니다. 이를 위해 turtle_tf2_fixed_frame_demo.launch.py 파일을 열고 launch_arguments 인수를 통해 'target_frame': 'carrot1' 매개 변수를 추가하십시오.

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

이제 패키지를 다시 빌드하고 turtle_tf2_fixed_frame_demo.launch.py 를 다시 시작하면 첫 번째 turtle 대신 두 번째 turtle가 carrot을 따라가는 것을 볼 수 있습니다!

2. 동적 프레임 브로드캐스터 작성

이 튜토리얼에서 추가 한 추가 프레임은 부모 프레임에 대해 시간에 따라 변경되지 않는 고정 프레임입니다. 그러나 움직이는 프레임을 게시하려면 브로드캐스터를 코드화하여 프레임을 시간에 따라 변경하도록 할 수 있습니다. 이제 carrot1 프레임을 turtle1 프레임에 대해 시간에 따라 변경되도록 변경해 보겠습니다. 이전 튜토리얼에서 생성한 learning_tf2_py 패키지로 이동합니다. 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.tf_broadcaster = 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.tf_broadcaster.sendTransform(t)


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

    rclpy.shutdown()

2.1. 코드 검토

현재 시간에 sin() 및 cos() 함수를 사용하여 carrot1 의 오프셋이 지속적으로 변경되도록하므로 x 및 y 오프셋의 고정 정의 대신에 현재 시간을 사용합니다.

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)

2.2. 엔트리 포인트 추가

ros2 run 명령이 노드를 실행하도록하려면 setup.py (src/learning_tf2_py 디렉토리에 위치)에 엔트리 포인트를 추가해야합니다.

다음 줄을 'console_scripts': 구문 내에 추가합니다.

'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',

2.3. 런치 파일 작성

이 코드를 테스트하기 위해 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']),
        )

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

2.4. 빌드

작업 공간의 루트에서 빠진 종속성을 확인하려면 rosdep 를 실행하십시오.

rosdep install -i --from-path src --rosdistro humble -y

작업 공간의 루트에서 패키지를 빌드하십시오.

colcon build --packages-select learning_tf2_py

새 터미널을 열고 작업 공간의 루트로 이동하고 설정 파일을 소스로합니다.

. install/setup.bash

2.5 실행

이제 동적 프레임 데모를 시작할 수 있습니다:

ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo.launch.py

두 번째 거북이가 지속적으로 변하는 carrot의 위치를 따르는 것을 볼 수 있어야합니다.

요약

이 튜토리얼에서는 tf2 변환 트리, 그 구조 및 기능에 대해 배웠습니다. 또한 지역 프레임 내에서 생각하는 것이 가장 쉽다는 것을 배우고 지역 프레임에 대한 추가 고정 및 동적 프레임을 추가하는 방법을 배웠습니다.