정적 브로드케스터 프로그래밍

저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo / Updated : 2024-10-11

이제 TF2에 대해 이해 했으니 직접 프로그래밍을 해 보도록 하겠습니다.

우선 정적(Static) TF 프로그래밍 부터 시작해 보죠.

이번 세션 에서는 TF2에 정적 좌표 프레임을 브로드케스트 하는 방법을 알아 보겠습니다.

1. 패키지 생성

먼저 이 튜토리얼과 다음 튜토리얼에 사용될 패키지를 만듭니다.

새 터미널을 열고 ros2 작업 공간 src폴더로 이동하여 새 패키지를 만듭니다.

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

터미널에서 패키지 learning_tf2_py와 모든 필수 파일 및 폴더가 생성되었음을 확인하는 메시지가 반환됩니다.

2. 정적(Static) 브로드캐스터 노드 작성

먼저 소스 파일을 만들어 보겠습니다. src/learning_tf2_py/learning_tf2_py디렉토리 내부에서 다음 명령을 입력하여 예시 정적 브로드캐스터 코드를 다운로드합니다.

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

이제 vscode 편집기를 사용하여 파일을 엽니다.

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

    def make_transforms(self, transformation):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = transformation[1]

        t.transform.translation.x = float(transformation[2])
        t.transform.translation.y = float(transformation[3])
        t.transform.translation.z = float(transformation[4])
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.tf_static_broadcaster.sendTransform(t)


def main():
    logger = rclpy.logging.get_logger('logger')

    # obtain parameters from command line arguments
    if len(sys.argv) != 8:
        logger.info('Invalid number of parameters. Usage: \n'
                    '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                    'child_frame_name x y z roll pitch yaw')
        sys.exit(1)

    if sys.argv[1] == 'world':
        logger.info('Your static turtle name cannot be "world"')
        sys.exit(2)

    # pass parameters and initialize node
    rclpy.init()
    node = StaticFramePublisher(sys.argv)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

2.1 코드 검토

이제 tf2에 정적 거북이 포즈를 게시하는 데 관련된 코드를 살펴보겠습니다. 첫 번째 줄은 필요한 패키지를 가져옵니다. 먼저 TransformStamped에서 geometry_msgs를 가져오는데, 이는 변환 트리에 게시할 메시지에 대한 템플릿을 제공합니다.

from geometry_msgs.msg import TransformStamped

그런 다음 Node클래스를 사용할 수 rclpy있도록 가져옵니다.

import rclpy
from rclpy.node import Node

패키지 tf2_ros는 정적 변환의 게시를 쉽게 하기 위해 StaticTransformBroadcaster를 제공합니다.

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

클래스 StaticFramePublisher생성자는 노드를 static_turtle_tf2_broadcaster이름으로 초기화합니다. 그런 다음, 시작 시 정적 변환을 하나 보내는 StaticTransformBroadcaster가 생성됩니다.

self.tf_static_broadcaster = StaticTransformBroadcaster(self)
self.make_transforms(transformation)

여기서 우리는 TransformStamped객체를 생성하는데, 이는 채워지면 보낼 메시지입니다. 실제 변환 값을 전달하기 전에 적절한 메타데이터를 제공해야 합니다.

게시되는 변환에 타임스탬프를 지정해야 하며 현재 시간으로 스탬프를 찍습니다.self.get_clock().now()
그런 다음 만들고 있는 링크의 부모 프레임 이름world을 설정해야 합니다.
마지막으로, 우리가 만들고 있는 링크의 자식 프레임의 이름을 설정해야 합니다.

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = transformation[1]

여기서 우리는 거북이의 6D 포즈(이동 및 회전)를 채웁니다.

t.transform.translation.x = float(transformation[2])
t.transform.translation.y = float(transformation[3])
t.transform.translation.z = float(transformation[4])
quat = quaternion_from_euler(
    float(transformation[5]), float(transformation[6]), float(transformation[7]))
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]

마지막으로 sendTransform()함수를 사용하여 정적 변환을 브로드캐스트합니다.

self.tf_static_broadcaster.sendTransform(t)

2.2 package.xml 업데이트

디렉토리 src/learning_tf2_py로 한 단계 돌아가면 setup.py, setup.cfg, package.xml파일이 만들어진 곳이 있습니다.

package.xml파일을 텍스트 편집기로 엽니다 .

<description>, <maintainer>및 <license>태그를 꼭 채워야 합니다.

<description>Learning tf2 with rclpy</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

위의 줄 뒤에 노드의 import 문에 해당하는 다음 종속성을 추가합니다.

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf2_ros_py</exec_depend>
<exec_depend>turtlesim</exec_depend>

이는 코드가 실행될 때 필요한 geometry_msgs, python3-numpy, rclpy, tf2_ros_py, turtlesim 종속성을 선언합니다.

파일을 저장하세요.

2.3 진입점 추가

ros2 runsetup.py명령이 노드를 실행하도록 하려면 ( src/learning_tf2_py디렉토리에 있는)에 진입점을 추가해야 합니다.

다음 줄을 'console_scripts':괄호 안에 추가하세요.

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

3. 빌드

빌드하기 전에 누락된 종속성을 확인하려면 작업 공간 루트에서 rosdep을 실행하는 것이 좋습니다.

cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro humble -y

이제 패키지를 빌드하세요.

colcon build --packages-select learning_tf2_py

새 터미널을 열고 작업 공간 루트로 이동한 후 설치 파일을 소싱합니다.

. install/setup.bash

4. 실행

이제 static_turtle_tf2_broadcaster노드를 실행하세요.

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

이렇게 하면 거북이 자세가 mystaticturtle땅에서 1미터 높이로 떠 있도록 방송됩니다.

tf_static이제 토픽을 반향하여 정적 변환이 게시되었는지 확인할 수 있습니다.

ros2 topic echo /tf_static

모든 것이 잘 진행되었다면 단일 정적 변환이 표시되어야 합니다.

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

이 튜토리얼은 정적 변환을 게시하는 데 사용할 수 있는 StaticTransformBroadcaster 방법을 보여주는 것을 목표로 합니다. 실제 개발 프로세스에서는 이 코드를 직접 작성할 필요가 없으며 tf2_ros전용 도구를 사용하여 작성해야 합니다. tf2_ros명령줄 도구로 사용할 수 있는 실행 파일 static_transform_publisher이나 launchfiles에 추가할 수 있는 노드를 사용합니다.

다음 명령은 미터 단위의 x/y/z 오프셋과 라디안 단위의 roll/pitch/yaw를 사용하여 tf2에 정적 좌표 변환을 게시합니다. ROS 2에서 roll/pitch/yaw는 각각 x/y/z축에 대한 회전을 나타냅니다.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

다음 명령은 미터 단위의 x/y/z 오프셋과 쿼터니언으로 롤/피치/요를 사용하여 tf2에 정적 좌표 변환을 게시합니다.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

static_transform_publisher수동 사용을 위한 명령줄 도구로 설계되었으며, 정적 변환을 설정하기 위한 launch파일 내에서 사용하도록 설계되었습니다. 예를 들어:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

--frame-id및 --child-frame-id를 제외한 모든 인수는 선택 사항입니다. 특정 옵션을 지정하지 않으면 해당 ID가 가정됩니다.

Previous프로그래밍 환경 설치 Next브로드캐스터 작성하기

Last updated 9 months ago