[ROS] ROS TF 좌표계 및 변환

💡 본 문서는 'ROS TF 좌표계 및 변환'에 대해 정리해놓은 글입니다.
이 튜토리얼에서는 ROS를 사용하는 모바일 로봇의 표준 좌표계(예: x,y,z 축 )의 기본 사항을 다룹니다. 또한 기본적인 2륜 차동 구동 모바일 로봇 의 좌표계를 설정하는 방법도 다룰 것이니 참고하시기 바랍니다.

1. Why Coordinate Frames Are Important?

바퀴 달린 로봇 위에 LIDAR가 장착되어 있다고 가정해 보겠습니다. LIDAR의 임무는 환경 내 물체까지의 거리에 대한 데이터를 제공하는 것입니다.

하지만 여기서 LIDAR는 보통 로봇의 중심점에 위치하지 않습니다. 따라서 실제 시나리오에서는 로봇이 환경에서 이동할 때 장애물을 피하려면 LIDAR 좌표계에서 감지된 객체의 좌표를 로봇 베이스 프레임의 해당 좌표로 변환해야 합니다. 

예를 들어 LIDAR는 100cm 떨어진 물체를 감지하더라도 LIDAR가 로봇의 전면부(로봇 베이스 프레임의 중심점에서 10cm 앞쪽)에 장착된 경우, 물체는 실제로 로봇으로부터 110cm 떨어져 있을 수 있습니다. 

따라서 정확한 자율주행을 위해서는 좌표 프레임과 데이터를 한 좌표 프레임에서 다른 좌표 프레임으로 변환하는 것이 중요합니다.

2. 모바일 로봇을 위한 ROS의 표준 좌표계

아래는 기본 2륜 차동 구동 로봇의 표준 좌표계입니다. 빨간색 화살표는 x축, 파란색 화살표는 z축, 녹색 점은 y축을 나타냅니다.

 

공식 ROS 문서에는 이러한 좌표 프레임에 대한 설명이 있지만 주요 좌표 프레임을 간략하게 정의하겠습니다.

• map 프레임은 world에서 임의로 선택한 지점에서 시작됩니다. 이 좌표계는 world 좌표계에 고정되어 있습니다.
• odom 프레임은 로봇이 초기화되는 지점에서 시작됩니다. 이 좌표계는 world 좌표계에 고정되어 있습니다.
• base_footprint 의 원점은 로봇 중심 바로 아래에 있습니다. 로봇의 2D 포즈입니다. 이 좌표계는 로봇이 움직일 때 움직입니다.
• base_link 의 원점은 로봇의 피벗점이나 중심에 직접 있습니다. 이 좌표계는 로봇이 움직일 때 움직입니다.
• laser_link는 레이저 센서(예: LIDAR)의 중심에서 시작됩니다. 이 좌표 프레임은 base_link를 기준으로 고정된(즉, "정적") 상태로 유지됩니다.

만약 로봇에 IMU 와 같은 다른 센서가 있는 경우, 이에 대한 좌표계도 가질 수 있습니다.

3. TF(Transform) Publisher: Static, Broadcaster

한 좌표계에 게시된 데이터를 다른 좌표계의 해당 데이터로 자동 변환하려면 어떻게 해야 합니까? 예를 들어 map 좌표계의 좌표 (x=3.7, y=1.23, z = 0.0)에 객체가 있다고 가정합니다. 우리는 로봇을 이 객체로 이동시키고 싶습니다. base_link 좌표계를 기준으로 개체의 위치를 어떻게 알 수 있을까요? 다행히 ROS 에는 이러한 모든 좌표 변환을 처리하는 tf라는 패키지가 있습니다.

• 정적 좌표계: 시간이 지나도 서로 상대적으로 변하지 않는 좌표 프레임의 경우(예: 레이저가 로봇에 부착되어 있기 때문에 base_link에 대한 laser_link가 정적으로 유지됨) Static Transform Publisher를 사용합니다. 
• 동적 좌표계: 시간이 지남에 따라 서로 상대적으로 변경되는 좌표 프레임의 경우(예: base_link에 매핑) tf broadcasters and listeners를 사용합니다 . 많은 ROS 패키지가 이러한 이동 좌표계 변환을 처리하므로 직접 작성할 필요가 없는 경우가 많습니다.

예시)

Static transform

ROS에서 정적 변환 게시자를 설정하는 방법에 대한 몇 가지 예를 살펴보겠습니다. 이러한 정적 변환 게시자는 일반적으로 작업 중인 로봇에 대한 실행 파일 내부에 나타납니다.

구문은 다음과 같습니다.

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms

ROS 웹사이트에서: “미터 단위의 x/y/z 오프셋과 라디안 단위의 요/피치/롤을 사용하여 tf에 정적 좌표 변환을 게시합니다. (요는 Z에 대한 회전, 피치는 Y에 대한 회전, 롤은 X에 대한 회전) 기간(밀리초)은 변환을 보내는 빈도를 지정합니다. 100ms(10hz)가 좋은 값입니다.”

Map to Odom

예를 들어, 지도 와 주행 거리 측정 프레임을 동등하게 만들고 싶다면 ROS 실행 파일 내에 작성하는 코드는 다음과 같습니다.

<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 30" />

 

map -> odom 변환은 지도의 좌표계(즉, 상위) 내부에서 로봇의 시작점(즉, 하위인 odom 좌표계)의 위치와 방향을 알려줍니다.

위의 예에서는 Odom 프레임이 시간이 지나도 맵 프레임을 기준으로 움직이지 않는다고 가정합니다.

Odom to Base Footprint

odom -> base_footprint 변환은 로봇이 전 세계를 이동하기 때문에 정적이 아닙니다. base_footprint 좌표계는 바퀴가 회전함에 따라 지속적으로 변경됩니다. 이 비정적 변환은 종종 robots_pose_ekf 패키지 와 같은 패키지에서 제공됩니다 .

Base Footprint to Base Link

base_footprint -> base_link는 두 좌표 프레임이 서로 고정되어 있으므로 정적 변환입니다. 로봇이 움직이고 로봇의 "발자국"의 좌표계가 움직입니다.

<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0.09 0 0 0 base_footprint base_link 30" />

 

위의 경우, base_link 좌표계의 원점(즉, 로봇의 중심)이 발자국 위 0.09미터에 있다고 가정합니다.

Base Link to Laser

base_link -> Laser 변환은 base_link의 좌표계 내부에서 레이저의 위치와 방향을 제공합니다. 이 변환은 정적입니다.

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.06 0 0.08 0 0 0 base_link laser 30" />

 

레이저는 로봇 중심에서 앞쪽으로 0.06m, 로봇 중심에서 위쪽으로 0.08m에 위치한다고 가정합니다.

Base Link to IMU

base_link -> imu 변환은 base_link의 좌표 프레임 내 IMU(관성 측정 장치…일반적으로 BNO055 센서)의 위치와 방향을 제공합니다. 이 변환은 정적입니다.

<node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0.06 0.02 0 0 0 base_link imu 30" />

 

여기까지가 ROS의 좌표 프레임 및 정적 변환 게시자의 기본 사항입니다. 

Dynamic Publisher

TF Broadcaster

우선, 로봇의 귀환지를 원점으로 하는 좌표계의 이름을 home이라고 해보자. 그렇다면, home 프레임 상에서 (-5, 10, 0) 만큼 떨어진 위치는 아래와 같이 표현될 것이다.

#!/usr/bin/python

import rospy
import tf
from geometry_msgs.msg import PoseStamped


class Robot(object):
   def __init__(self):
       position = PoseStamped()

       position.header.frame_id = "home"

       position.pose.position.x = -5
       position.pose.position.y = 10

       position.pose.orientation.w = 1

       self.position = position
       self.pub = rospy.Publisher("robot_pose", PoseStamped, queue_size=1)

   def publishPose(self):
       self.position.header.stamp = rospy.Time.now()

       self.pub.publish(self.position)


if __name__ == '__main__':
   rospy.init_node("tf_pub")

   robot = Robot()

   r = rospy.Rate(1)
   while not rospy.is_shutdown():

       robot.publishPose()

       r.sleep()

이번에는 home 프레임과 world 프레임 간의 관계를 정의해야 한다. 여기에는 tf.TransformBroadcaster() 클래스와, 해당 클래스에 선언된 sendTransform 메소드를 이용한다.

sendTransform 메소드는 아래와 같은 매개변수를 받아, TF 관계를 발행한다.

• translation: (x, y, z), 좌표계 간의 병진운동을 정의하는 값
• rotation: (x, y, z, w), 좌표계 간의 회전운동을 정의하는 값
• time: time, transform을 가지는 시점
• child: string, 자식 프레임의 이름
• parent: string, 부모 프레임의 이름

여기서 부모와 자식이란, 어떤 프레임을 더 상위의 프레임으로 취급할지를 의미한다.

따라서 위의 내용을 코딩하면 아래와 같다.

    tf_broadcaster = tf.TransformBroadcaster()

    r = rospy.Rate(1)
    while not rospy.is_shutdown():

        tf_broadcaster.sendTransform(
            translation=[10, 10, 0],
            rotation=[0., 0., 0., 1],
            time=rospy.Time.now(),
            child="home",
            parent="world"
        )

        robot.publishPose()

        r.sleep()

TF Listener

보다싶이 TF 관계의 발행은 ROS Publisher와 매우 유사한 형태를 가지고 있는데, 이와 비슷하게 TF관계 또한, 구독하여 실제 프레임 간의 좌표 변환을 시킬 수 있다.

구체적으로는 tf.TransformListener 클래스를 사용할 수 있다.

사용법은 아래와 같은 코드를 참고하면 된다.

#!/usr/bin/python

import rospy
import tf
from geometry_msgs.msg import PoseStamped


if __name__ == '__main__':
    rospy.init_node("tf_listener")

    tf_listener = tf.TransformListener()    # TF Listener 객체 선언

    r = rospy.Rate(1)
    while not rospy.is_shutdown():

        if tf_listener.canTransform(
                target_frame="world", source_frame="home", time=rospy.Time(0)):
            # TF 변환이 가능한지 검사. 불가능 할 때, lookupTransform을 호출할 경우 예외 발생
            # TF 변환시 시간은 rospy.Time.now()가 아니라 Time(0)을 써야한다!!

            tf_matrix = tf_listener.lookupTransform(
                target_frame="world", source_frame="home", time=rospy.Time(0))
            # 변환 행렬 리턴. 리턴 값은 ([x, y, z], [x, y, z, w])

            rospy.loginfo(tf_matrix)

        else:
            rospy.logwarn("Cannot lookup transform between world and home")

        r.sleep()

위의 코드를 실행한 결과는 아래와 같다.

또한, ROS Docs를 참고하면, TransformListener가 사용 가능한 여러 메소드를 확인할 수 있다.

이 중에서 transformPose를 사용한 결과는 아래와 같다.

#!/usr/bin/python

import rospy
import tf
from geometry_msgs.msg import PoseStamped

msg = None


def poseCallback(m):
    global msg
    m.header.stamp = rospy.Time(0)
    msg = m


if __name__ == '__main__':
    rospy.init_node("tf_listener")

    tf_listener = tf.TransformListener()
    pose_subscriber = rospy.Subscriber(
        "robot_pose", PoseStamped, callback=poseCallback)

    r = rospy.Rate(1)
    while not rospy.is_shutdown():

        if tf_listener.canTransform(
                target_frame="world", source_frame="home", time=rospy.Time(0)) and msg is not None:

            res = tf_listener.transformPose(ps=msg, target_frame="world")

            rospy.loginfo(res)

        else:
            rospy.logwarn("Cannot lookup transform between world and home")

        r.sleep()

혹시 이런 의문이 들지는 않을까. TF는 전혀 새로운 내용이 아니며, 변환 행렬의 계산은 numpy 등 단순 행렬 계산을 통하여 얼마든지 대신할 수 있고, 오히려 그쪽이 빠르지 않을까?

물론 완전히 틀린 말은 아니지만, ROS의 tf 패키지를 사용하면, 시간에 따라 변화하는 변환 행렬을 추적하기 편리하며, 다른 ROS 라이브러리와 연동되어 귀찮은 작업을 대폭 감소시킬 수 있다.

또한, 프레임 간 부모-자식 관계는 단순히 하나만 존재하는 것이 아니라, 트리 구조로 무한하게 확장할 수 있으며, TransformListener를 사용한다면, 여러 좌표계 간의 변환 행렬을 한번에 계산 가능하다는 장점이 존재한다.

tf_broadcaster.sendTransform(
	translation=[10, 10, 0],
	rotation=[0., 0., 0., 1],
	time=rospy.Time.now(),
	child="home",
	parent="world"
)

tf_broadcaster.sendTransform(
	translation=[10, 10, 0],
	rotation=[0., 0., 0., 1],
	time=rospy.Time.now(),
	child="world",
	parent="utm"
)

if tf_listener.canTransform(
	target_frame="utm", source_frame="home", time=rospy.Time(0)) and msg is not None:

	res = tf_listener.transformPose(ps=msg, target_frame="utm")

	rospy.loginfo(res)

또한, 이 역시도 ROS의 일부이기 때문에, 간단하게 시각화 할 수 있다는 점 또한, 장점으로 볼 수 있을 것이다.

$ rosrun tf view_frames

참고

• [Blog] Coordinate Frames and Transforms for ROS-based Mobile Robots: https://automaticaddison.com/coordinate-frames-and-transforms-for-ros-based-mobile-robots/
• [Blog] 초보자를 위한 ROS : TF란 무엇인가: https://velog.io/@7cmdehdrb/whatIsTF