tf2 정적 브로드캐스터 작성 (Python)

목표: tf2에 정적 좌표 프레임을 브로드캐스트하는 방법 배우기

배경

정적 변환을 게시하면 로봇 베이스와 해당 센서 또는 정지 부품 간의 관계를 정의하는 데 유용합니다. 예를 들어, 레이저 스캔 측정 값을 레이저 스캐너 중심에있는 프레임에서 생각하는 것이 가장 쉽습니다.

이것은 정적 변환의 기본을 다루는 독립형 튜토리얼로, 두 부분으로 구성되어 있습니다. 첫 번째 부분에서는 정적 변환을 tf2에 게시하기 위한 코드를 작성합니다. 두 번째 부분에서는 명령 줄 tf2_ros 의 static_transform_publisher 실행 도구를 사용하는 방법을 설명합니다.

다음 두 튜토리얼에서는 tf2 소개 튜토리얼 에서 데모를 재현하는 코드를 작성합니다. 그 후 다음 튜토리얼에서는 더 고급 tf2 기능으로 데모를 확장하는 데 중점을 둡니다.

필수 준비물

이전 튜토리얼에서는 작업 공간을 생성하는 방법 과 패키지를 생성하는 방법 을 배웠습니다.

작업

1 패키지 생성

먼저 이 튜토리얼과 이후 튜토리얼에서 사용할 패키지를 만들겠습니다. learning_tf2_py 라는 패키지는 geometry_msgs, python3-numpy, rclpy, tf2_ros_py 및 turtlesim 에 종속됩니다. 이 튜토리얼의 코드는 여기 에 저장되어 있습니다.

새 터미널을 열고 ROS 2 설치 환경을 소스로 지정 하여 ros2 명령을 사용할 수 있도록합니다. 작업 공간의 src 폴더로 이동하고 새 패키지를 만듭니다.

ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

터미널에서 패키지 learning_tf2_py 및 모든 필요한 파일 및 폴더를 생성하는 확인 메시지가 반환됩니다.

2 정적 브로드캐스터 노드 작성

먼저 소스 파일을 만들어 봅시다. 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

선호하는 텍스트 편집기로 파일을 엽니다.

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에 정적 거북이 포즈를 게시하는 데 관련된 코드를 살펴 보겠습니다. 첫 번째 라인에서는 필요한 패키지를 가져옵니다. 먼저 geometry_msgs 에서 TransformStamped 를 가져오는데, 이는 변환 트리에 게시 할 메시지의 템플릿을 제공합니다.

from geometry_msgs.msg import TransformStamped

그 다음으로 rclpy 를 가져오기 때문에 Node 클래스를 사용할 수 있습니다.

import rclpy
from rclpy.node import Node

tf2_ros 패키지는 정적 변환을 쉽게 게시하도록하는 StaticTransformBroadcaster 를 제공합니다. StaticTransformBroadcaster 를 사용하려면 tf2_ros 모듈에서 가져와야합니다.

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 객체를 생성합니다. 실제 변환 값을 전달하기 전에 적절한 메타데이터를 설정해야 합니다.

1. 게시되는 변환에 타임스탬프를 지정해야 하며 현재 시간으로 타임스탬프를 지정하겠습니다. self.get_clock().now() 입니다.

2. 그런 다음 우리는 생성 중인 링크의 부모 프레임 이름을 설정해야 합니다. 이 경우 world 입니다.

3. 마지막으로 생성 중인 링크의 자식 프레임 이름을 설정해야 합니다.

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 entry point 추가

ros2 run 명령을 사용하여 노드를 실행하려면 setup.py (src/learning_tf2_py 디렉토리에 위치)에 entry point를 추가해야 합니다.

다음 라인을 'console_scripts': 괄호 사이에 추가하십시오.

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

3 빌드

빌드하기 전에 빌드 전에 빠진 종속성을 확인하기 위해 워크스페이스 루트에서 rosdep 를 실행하는 것이 좋습니다.

Linux

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

아직 워크스페이스 루트에 있는 상태에서 새 패키지를 빌드하십시오.

Linux

colcon build --packages-select learning_tf2_py

새로운 터미널을 열고 워크스페이스 루트로 이동하여 설정 파일을 소스하십시오.

Linux

. 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 토픽을 echo 명령으로 확인할 수 있습니다.

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 라는 실행 파일을 제공하며 이를 명령행 도구 또는 런치 파일에 추가할 수 있습니다.

미터 단위의 x/y/z 오프셋과 라디안 단위의 roll/pitch/yaw를 사용하여 tf2에 정적 좌표 변환을 게시합니다. 이 경우 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 를 제외한 모든 인수는 선택 사항입니다. 특정 옵션을 지정하지 않으면 항등 변환으로 가정됩니다.

요약

이 튜토리얼에서는 StaticTransformBroadcaster 를 사용하여 정적 프레임 간의 정적 관계를 정의하는 데 유용한 방법을 배웠습니다. 예를 들어 world 프레임에 대한 mystaticturtle 와 같은 프레임 간의 관계를 정의하는 데 사용할 수 있습니다. 또한 정적 변환은 레이저 스캐너와 같은 센서 데이터를 이해하는 데 유용하며 데이터를 공통 좌표 프레임과 관련시키는 데 사용할 수 있습니다. 마지막으로 static_transform_publisher 실행 파일 및 런치 파일을 사용하여 필요한 정적 변환을 게시하는 방법을 배웠습니다.