브로드캐스터 작성하기

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

1. 브로드캐스터 노드

먼저 소스 파일 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/turtle_tf2_broadcaster.py

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

import math

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

from turtlesim.msg import Pose


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 FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.turtlename = self.declare_parameter(
          'turtlename', 'turtle').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription  # prevent unused variable warning

    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # Send the transformation
        self.tf_broadcaster.sendTransform(t)


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

    rclpy.shutdown()

1.1 코드 검토

이제 tf2에 거북이 포즈를 게시하는 데 관련된 코드를 살펴보겠습니다. 먼저 거북이 이름(예: turtle1또는 turtle2)을 지정하는 단일 매개변수 turtlename를 정의하고 획득합니다.

self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

그런 다음 노드는 {self.turtlename}/pose토픽을 구독 하고 모든 수신 메시지에 대해 handle_turtle_pose함수를 실행합니다.

self .subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)

이제 TransformStamped객체를 생성하고 적절한 메타데이터를 제공합니다.

  1. 게시되는 변환에 타임스탬프를 제공해야 하며, 를 호출하여 현재 시간으로 스탬프를 찍습니다 self.get_clock().now(). 이렇게 하면 Node에서 사용하는 현재 시간이 반환됩니다.

  2. 그런 다음, 만들고 있는 world링크의 부모 프레임 이름을 설정해야 합니다. 이 경우에는 .

  3. 마지막으로, 우리가 만들고 있는 링크의 자식 노드의 이름을 설정해야 하는데, 이 경우에는 거북이 그 자체의 이름이 됩니다.

거북이 포즈 메시지에 대한 핸들러 함수는 거북이의 이동 및 회전을 브로드캐스트하고, world, turtlename프레임 간 변환으로 게시합니다.

t = TransformStamped()

# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

여기서는 3D 거북이 자세의 정보를 3D 변환으로 복사합니다.

# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0

# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]

마지막으로 우리가 구성한 변환을 가져와 브로드캐스팅을 처리할 TransformBroadcaster의 sendTransform메서드 에 전달합니다.

# Send the transformation
self.tf_broadcaster.sendTransform(t)

1.2 진입점 추가

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

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

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

2. Launch 파일

이제 이 데모를 위한 실행 파일을 만듭니다. 디렉토리 launch에 폴더를 만듭니다. 텍스트 편집기를 사용하여 src/learning_tf2_py폴더 에서 turtle_tf2_demo.launch.py라는 새 launch파일을 만들고 다음 줄을 추가합니다.

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

2.1 코드 검토

먼저 launch및 launch_ros패키지 에서 필요한 모듈을 가져옵니다. 

from launch import LaunchDescription
from launch_ros.actions import Node

이제 turtlesim 시뮬레이션을 시작하고 turtle_tf2_broadcaster를 사용하여 turtle1 tf2에 상태를 브로드캐스트하는 노드를 실행합니다 .

Node(
    package='turtlesim',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='learning_tf2_py',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

2.2 종속성 추가

디렉토리 로 한 단계 뒤로 이동합니다 learning_tf2_py. 디렉토리에는 setup.py, setup.cfg, 및 package.xml파일이 있습니다.

텍스트 편집기로 엽니다 package.xml. 실행 파일의 가져오기 문에 해당하는 다음 종속성을 추가합니다.

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

이는 코드가 실행될 때 필요한 추가적 종속성 launch,launch_ros을 선언합니다.

파일을 저장하세요.

2.3 setup.py 업데이트

다시 setup.py열고 해당 줄을 추가하여 launch/폴더의 실행 파일이 설치되도록 합니다. data_files필드는 이제 다음과 같아야 합니다.

data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
],

또한 파일 맨 위에 적절한 가져오기를 추가합니다.

import os
from glob import glob

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. 실행

이제 turtlesim 시뮬레이션 노드와 turtle_tf2_broadcaster노드를 시작할 launch 파일을 실행하세요.

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

두 번째 터미널 창에 다음 명령을 입력하세요.

ros2 run turtlesim turtle_teleop_key

이제 조종할 수 있는 거북이 한 마리로 거북이 시뮬레이션이 시작된 것을 볼 수 있습니다.

이제 tf2_echo도구를 사용하여 거북이 포즈가 실제로 tf2에 브로드캐스트되는지 확인하세요.

ros2 run tf2_ros tf2_echo world turtle1

이것은 첫 번째 거북이의 포즈를 보여줄 것입니다. turtle_teleop_key화살표 키를 사용하여 거북이 주변을 운전하세요. 콘솔 출력에서 ​​다음과 비슷한 것을 볼 수 있습니다.

At time 1714913843.708748879
- Translation: [4.541, 3.889, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.999, -0.035]
- Rotation: in RPY (radian) [0.000, -0.000, -3.072]
- Rotation: in RPY (degree) [0.000, -0.000, -176.013]
- Matrix:
 -0.998  0.070  0.000  4.541
 -0.070 -0.998  0.000  3.889
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

tf2_echo 명령으로 실행하면 두 번째 거북이가 아직 거기에 없기 때문에 변환이 보이지 않아야 합니다. 그러나 다음 튜토리얼에서 두 번째 거북이를 추가하자마자 turtle2의 포즈가 tf2로 브로드캐스트됩니다.

Previous정적 브로드케스터 프로그래밍Next리스너 작성하기

Last updated