URDF 로 로봇 모델 만들기

이 장에서 배우는 것

앞 장에서 프로세스 안의 통신 구조를 살펴보았다. 이제 두리의 소프트웨어가 공통으로 사용할 몸체의 구조를 정의한다. 차체 위에 회전하는 센서 헤드가 있고, 헤드 앞쪽에 거리 센서가 달려 있다고 하자. 프로그램마다 센서 위치를 따로 적으면 부품을 옮길 때 여러 파일을 수정해야 한다. 로봇의 구조를 하나의 모델로 표현하면 부품의 관계를 한곳에서 관리할 수 있다.

이 장에서는 통합 로봇 기술 형식(Unified Robot Description Format, URDF)으로 두리의 차체와 센서 헤드를 기술한다. 이어서 robot_state_publisher가 모델과 관절 상태를 받아 좌표 관계를 발행하는 과정을 연결하고, xacro로 반복되는 모델 정의를 정리한다. 예제는 위치 관계를 확인하는 데 필요한 부품만 포함한다. 바퀴 구동이나 경로계획은 다루지 않는다.

• 링크와 조인트로 로봇의 기구 구조를 표현한다.
• 조인트의 원점, 회전축, 이동 범위를 구분한다.
• robot_state_publisher에 모델과 관절 상태를 공급한다.
• xacro의 속성과 매크로로 모델을 재사용한다.
• ROS가 없는 환경에서도 순수 Python으로 센서 위치를 계산한다.

문제 상황

두리의 거리 센서를 차체 중심에서 앞쪽으로 30cm, 위쪽으로 35cm 떨어진 곳에 설치했다. 정지한 상태에서는 이 수치만으로 센서 위치를 설명할 수 있다. 그런데 센서가 달린 헤드를 왼쪽으로 돌리자 센서의 위치까지 달라졌다. 센서는 회전축에서 앞쪽으로 10cm 떨어져 있으므로, 헤드가 돌면 센서 중심도 작은 원을 따라 움직인다.

개발자가 센서 위치를 차체 기준의 고정된 수치로 기록해 두었다면, 헤드를 돌린 뒤에도 프로그램은 이전 위치를 사용한다. 화면에서 센서가 차체와 분리된 것처럼 보이거나, 센서가 관측한 물체의 위치가 실제와 어긋나는 원인이 된다. 필요한 것은 센서의 최종 위치 하나가 아니라 차체에서 회전축까지의 관계와 회전축에서 센서까지의 관계다.

두리의 모델에서는 차체를 base_link, 회전하는 헤드를 head_link, 센서를 sensor_link로 부른다. 차체에서 헤드로 이어지는 조인트는 회전하며, 헤드에서 센서로 이어지는 조인트는 고정된다. 센서의 최종 위치는 이 두 관계를 순서대로 합성해서 구한다.

여기에는 서로 다른 두 종류의 정보가 있다. 회전축의 설치 위치와 센서의 장착 간격은 설계 정보다. 현재 헤드가 얼마나 돌아갔는지는 실행 중의 상태다. URDF에는 설계 정보를 넣고, 현재 각도는 별도 메시지로 전달한다. 이 구분을 유지해야 같은 모델을 실제 로봇과 시험 프로그램에서 함께 사용할 수 있다.

링크와 조인트로 구조 표현하기

링크(link)는 모델에서 하나의 강체로 취급하는 부분이다. 조인트(joint)는 부모 링크와 자식 링크 사이의 관계를 정의한다. 링크가 실제 제품의 부품 한 개와 반드시 일치하지는 않는다. 서로 움직이지 않는 여러 부품을 하나의 링크로 묶을 수도 있고, 센서의 기준점을 명확히 하기 위해 별도의 링크를 둘 수도 있다.

URDF의 연결 구조는 트리다. 루트 링크에는 부모 조인트가 없고, 나머지 링크에는 부모 조인트가 하나씩 있다. 이 예제에서는 base_link가 루트다. sensor_link를 차체와 헤드 양쪽에 동시에 연결하면 하나의 자식에 부모가 둘 생긴다. 기구적으로 연결이 복잡하더라도 URDF에 적을 때는 이 제약을 먼저 고려해야 한다.

좌표축은 x가 앞쪽, y가 왼쪽, z가 위쪽인 방향으로 정한다. 길이는 미터, 각도는 라디안으로 작성한다. 회전 조인트의 양의 방향은 지정한 축에 대한 오른손 법칙을 따른다. 이 예제의 회전축은 양의 z축이므로 위에서 내려다볼 때 양의 각도는 반시계 방향이다.

모델의 주요 요소가 담당하는 정보

요소	역할	두리의 예
link	강체와 기준 좌표계 정의	base_link, head_link, sensor_link
joint	부모와 자식의 연결 관계	head_yaw, sensor_mount
visual	화면에 표시할 형상과 색상	차체 상자와 헤드 원기둥
collision	충돌 검사에 사용할 형상	필요할 때 별도로 정의
inertial	질량과 관성 정보	물리 시뮬레이션용으로 계산

예제는 모델 표시와 좌표 관계 확인에 집중하므로 visual만 작성한다. 이것만으로 robot_state_publisher를 사용할 수 있다. 그러나 화면에 모양이 보인다고 해서 물리 시뮬레이션용 모델까지 준비된 것은 아니다. 충돌 검사와 동역학 계산이 필요하다면 collision과 inertial을 목적에 맞게 추가해야 한다. 관성값을 임의로 채워 넣는 것은 실제 질량 분포를 모델링하는 일을 대신하지 못한다.

두 종류의 origin을 구별한다

조인트 안의 origin은 부모 링크 좌표계에서 본 조인트 기준의 위치와 자세다. 조인트 변위가 0일 때 자식 링크의 기준 위치와 자세를 결정한다. 반면 visual 안의 origin은 링크 좌표계에서 그림을 어디에 놓을지 지정한다. visual의 위치를 바꾸어도 자식 링크의 연결 위치는 바뀌지 않는다.

두리의 head_yaw는 차체 기준으로 앞쪽 0.20m, 위쪽 0.30m에 설치한다. sensor_mount는 헤드 기준으로 앞쪽 0.10m, 위쪽 0.05m에 설치한다. 헤드 각도가 0이면 센서 위치는 차체 기준으로 앞쪽 0.30m, 위쪽 0.35m다. 헤드가 회전하면 두 번째 간격의 앞쪽 성분도 함께 회전한다.

origin의 rpy는 고정된 x, y, z축에 대한 롤, 피치, 요 각도를 나타낸다. 이 장의 모델에서는 모두 0으로 두어 설치 자세와 관절 회전이 섞이지 않게 한다. axis는 조인트 좌표계에서 표현한 축이다. 설치 자세가 회전되어 있다면 같은 axis 값도 차체에서 보았을 때 다른 방향을 가리킬 수 있다.

고정 연결과 회전 연결을 구별한다

fixed 조인트에는 실행 중 바뀌는 변위가 없다. revolute 조인트에는 회전축과 각도 범위가 있다. continuous 조인트는 각도의 상한과 하한 없이 회전하는 연결에 사용한다. 두리의 헤드는 케이블 때문에 좌우로 제한된 범위만 움직인다고 가정하고 revolute를 선택한다.

head_yaw의 범위는 약 −90도부터 90도까지다. limit의 lower와 upper에는 라디안 값을 넣는다. effort는 회전 조인트에서 허용 토크를, velocity는 허용 각속도를 기술한다. 이 값들은 모델의 제한 정보다. robot_state_publisher는 입력 각도를 제한 범위로 잘라 주거나 모터의 토크를 제어하지 않는다. 상태를 만드는 프로그램과 실제 제어 계층이 제한을 지켜야 한다.

모델과 관절 상태를 연결하기

robot_state_publisher는 robot_description 파라미터로 URDF 문자열을 받는다. 파일 경로를 전달하는 것과 파일 내용을 전달하는 것은 다르다. 예제의 실행 파일은 xacro를 먼저 전개하고, 생성된 XML 문자열을 이 파라미터에 넣는다.

고정 조인트의 관계는 /tf_static으로 발행된다. 움직이는 조인트는 /joint_states에서 현재 위치를 받아 해당 관계를 /tf로 발행한다. 관절 상태(JointState) 메시지의 name에는 링크 이름이 아니라 조인트 이름을 적는다. name과 position은 같은 인덱스로 대응한다. 여기서는 name[0]이 head_yaw이고 position[0]이 그 회전각이다.

URDF에 회전 조인트를 정의했다고 해서 그 관절의 현재 각도를 알 수 있는 것은 아니다. 초기 각도가 0이라는 상태도 메시지로 제공하는 편이 명확하다. 실제 두리에서는 관절을 읽는 하드웨어 프로그램이 상태를 발행하겠지만, 이 장에서는 rclpy 프로그램이 부드럽게 변하는 시험 각도를 만든다.

메시지에는 현재 시각도 넣는다. 관절 상태의 시각은 좌표 관계를 어느 순간의 상태로 해석할지 결정하는 데 사용된다. 예제의 두 노드는 기본 시스템 시계를 사용한다. 실험 중 한 노드만 다른 시간 기준을 사용하면 좌표 관계가 연결되어 있어도 원하는 시점의 상태를 찾기 어려워질 수 있다.

robot_state_publisher는 모델 안의 링크 관계를 계산한다. 루트인 base_link가 지도 위 어디에 있는지는 이 모델만으로 결정되지 않는다. 모델에 월드 좌표를 억지로 넣기 전에, 로봇 자체의 구조와 로봇 전체의 위치를 구분해야 한다. 이 장에서는 base_link를 기준으로 센서 위치를 확인하는 데까지만 진행한다.

xacro로 반복과 수치 정리하기

xacro는 XML 매크로(XML macro)를 전개해 최종 XML을 만드는 도구다. URDF 자체에 변수나 함수를 추가하는 방식이 아니다. xacro가 속성값과 매크로 호출을 처리한 결과가 URDF이며, robot_state_publisher는 그 결과를 사용한다.

자주 바꾸는 설치 수치는 property에 모은다. 차체와 센서처럼 상자 형상을 쓰는 링크는 macro로 묶는다. 매크로를 지나치게 크게 만들면 실제 링크와 조인트의 관계를 찾기 어려워진다. 예제에서는 반복되는 visual 정의만 묶고, 조인트의 부모와 자식은 본문에 그대로 남긴다.

속성 이름에는 단위와 의미가 드러나도록 하는 편이 좋다. 여기서는 모든 길이가 미터라는 규칙 아래 head_x, head_z처럼 위치와 축을 함께 적는다. ${head_x}는 속성값을 참조하고 ${pi / 2.0}는 수식을 계산한다. 이 표현은 xacro 단계에서 사라지므로 전개된 파일을 읽으면 실제 숫자와 링크 구조를 확인할 수 있다.

모델에 문제가 생겼을 때는 원본 xacro와 전개된 URDF를 구분해 살펴본다. 원본은 수정하기 편한 설계 표현이고, 전개 결과는 프로그램이 실제로 읽는 입력이다. 매크로 호출의 오타인지, 잘못된 조인트 연결인지, 실행 중 상태 메시지의 문제인지를 나누는 기준이 된다.

완성 코드

다음 네 파일을 같은 디렉터리에 저장한다. ROS 실행에는 ROS 2 Jazzy의 rclpy, sensor_msgs, launch_ros, robot_state_publisher, xacro가 필요하다. Linux에서는 Jazzy 환경을 구성한 셸에서 실행한다. macOS에서도 해당 패키지와 Python 모듈을 사용할 수 있는 Jazzy 환경이 필요하며, 운영체제의 기본 Python만으로 ROS 예제를 실행할 수는 없다. model_math.py는 외부 패키지 없이 macOS와 Linux의 Python 3에서 실행된다.

이 구성은 별도 패키지를 만드는 과정을 생략하고 파일 경로로 실행한다. ROS용 Python 파일은 ROS 환경에서 사용하는 Python으로 실행해야 한다. ROS가 없는 환경에서는 문법 검사와 보조 예제를 실행할 수 있지만, 그것만으로 ROS 노드의 실제 동작이 검증되는 것은 아니다.

duri.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="duri">
  <xacro:property name="pi" value="3.141592653589793"/>
  <xacro:property name="head_x" value="0.20"/>
  <xacro:property name="head_z" value="0.30"/>
  <xacro:property name="sensor_x" value="0.10"/>
  <xacro:property name="sensor_z" value="0.05"/>

  <xacro:macro name="box_link" params="name sx sy sz color">
    <link name="${name}">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="${sx} ${sy} ${sz}"/>
        </geometry>
        <material name="${name}_paint">
          <color rgba="${color}"/>
        </material>
      </visual>
    </link>
  </xacro:macro>

  <xacro:box_link name="base_link"
                  sx="0.60" sy="0.40" sz="0.20"
                  color="0.25 0.45 0.65 1"/>

  <link name="head_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.05" length="0.08"/>
      </geometry>
      <material name="head_paint">
        <color rgba="0.70 0.75 0.80 1"/>
      </material>
    </visual>
  </link>

  <xacro:box_link name="sensor_link"
                  sx="0.06" sy="0.04" sz="0.04"
                  color="0.75 0.20 0.15 1"/>

  <joint name="head_yaw" type="revolute">
    <parent link="base_link"/>
    <child link="head_link"/>
    <origin xyz="${head_x} 0 ${head_z}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-pi / 2.0}" upper="${pi / 2.0}"
           effort="2.0" velocity="1.0"/>
  </joint>

  <joint name="sensor_mount" type="fixed">
    <parent link="head_link"/>
    <child link="sensor_link"/>
    <origin xyz="${sensor_x} 0 ${sensor_z}" rpy="0 0 0"/>
  </joint>
</robot>

model.launch.py

from pathlib import Path

import xacro
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    model_path = Path(__file__).resolve().parent / "duri.urdf.xacro"
    robot_xml = xacro.process_file(str(model_path)).toxml()

    publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[{
            "robot_description": ParameterValue(
                robot_xml, value_type=str
            ),
            "publish_frequency": 20.0,
        }],
    )
    return LaunchDescription([publisher])

joint_driver.py

import math

import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from sensor_msgs.msg import JointState


class HeadStatePublisher(Node):
    def __init__(self):
        super().__init__("duri_head_state")
        self.publisher = self.create_publisher(
            JointState, "/joint_states", 10
        )
        self.started_at = self.get_clock().now()
        self.timer = self.create_timer(0.1, self.publish_state)

    def publish_state(self):
        now = self.get_clock().now()
        elapsed = (now - self.started_at).nanoseconds / 1_000_000_000
        angle = (math.pi / 4.0) * math.sin(elapsed)

        message = JointState()
        message.header.stamp = now.to_msg()
        message.name = ["head_yaw"]
        message.position = [angle]
        self.publisher.publish(message)


def main():
    rclpy.init()
    node = None
    try:
        node = HeadStatePublisher()
        print("head_yaw 상태 발행 시작", flush=True)
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        if node is not None:
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()

model_math.py

import math


HEAD_X = 0.20
HEAD_Z = 0.30
SENSOR_X = 0.10
SENSOR_Z = 0.05


def pose_z(x, y, z, angle):
    c = math.cos(angle)
    s = math.sin(angle)
    return (
        (c, -s, 0.0, x),
        (s, c, 0.0, y),
        (0.0, 0.0, 1.0, z),
        (0.0, 0.0, 0.0, 1.0),
    )


def multiply(left, right):
    return tuple(
        tuple(
            sum(left[row][k] * right[k][col] for k in range(4))
            for col in range(4)
        )
        for row in range(4)
    )


def sensor_pose(angle):
    base_from_head = pose_z(HEAD_X, 0.0, HEAD_Z, angle)
    head_from_sensor = pose_z(SENSOR_X, 0.0, SENSOR_Z, 0.0)
    return multiply(base_from_head, head_from_sensor)


def verify():
    cases = (
        (0.0, (0.30, 0.00, 0.35)),
        (math.pi / 2.0, (0.20, 0.10, 0.35)),
    )
    for angle, expected in cases:
        matrix = sensor_pose(angle)
        actual = tuple(matrix[row][3] for row in range(3))
        for value, target in zip(actual, expected):
            if not math.isclose(value, target, abs_tol=1e-12):
                raise AssertionError((actual, expected))

    rotated = sensor_pose(math.pi / 2.0)
    x_axis = tuple(rotated[row][0] for row in range(3))
    for value, target in zip(x_axis, (0.0, 1.0, 0.0)):
        if not math.isclose(value, target, abs_tol=1e-12):
            raise AssertionError(x_axis)


def main():
    verify()
    print("좌표 검증 통과")
    for degrees in (0, 90):
        matrix = sensor_pose(math.radians(degrees))
        x, y, z = (matrix[row][3] for row in range(3))
        print(f"head_yaw={degrees:2d} deg: x={x:.3f}, y={y:.3f}, z={z:.3f}")


if __name__ == "__main__":
    main()

줄별 해설

duri.urdf.xacro의 구조

첫 줄은 XML 선언이다. robot 요소는 로봇 이름과 xacro 이름 공간을 지정한다. 그 아래 다섯 개의 property는 각도 계산용 상수와 두 조인트의 설치 간격이다. 센서를 옮길 때는 sensor_x와 sensor_z를 먼저 수정하면 된다.

box_link 매크로의 name은 링크 이름이고 sx, sy, sz는 상자의 세 변 길이다. visual의 origin을 모두 0으로 두었으므로 상자 중심과 링크 원점이 일치한다. material 이름에는 링크 이름을 넣어 서로 다른 호출의 재질 이름이 충돌하지 않게 한다. color는 공백으로 구분한 네 성분을 하나의 속성값으로 전달한다.

첫 번째 매크로 호출은 차체를 만들고, 두 번째 호출은 센서를 만든다. 그 사이의 head_link는 원기둥을 사용하므로 직접 정의한다. 이 예제의 형상은 좌표 관계를 보기 위한 간략한 표현이다. 부품을 연결하는 지지대나 체결 부품까지 묘사하지 않는다.

head_yaw의 parent와 child는 연결 방향을 결정한다. origin은 회전축을 차체 위에 배치하고 axis는 그 위치에서의 회전 방향을 지정한다. limit의 범위는 ±π/2다. sensor_mount에는 움직이는 축이 없으므로 axis와 limit을 작성하지 않는다. 이 조인트의 origin은 차체가 아니라 head_link를 기준으로 읽어야 한다.

model.launch.py의 처리 순서

Path(__file__)는 현재 작업 디렉터리와 관계없이 실행 파일 옆의 모델을 찾는 기준이다. resolve().parent로 실행 파일이 있는 디렉터리를 구하고 모델 파일명을 붙인다. 상대 경로를 셸의 현재 위치에만 의존하게 만들면 다른 디렉터리에서 실행할 때 파일을 찾지 못할 수 있다.

xacro.process_file은 속성과 매크로를 처리한다. toxml은 결과를 XML 문자열로 만든다. ParameterValue에 value_type=str을 명시하는 이유는 모델 전체를 문자열 파라미터로 전달한다는 의도를 분명히 하기 위해서다. Node는 이 문자열을 robot_description으로 받은 robot_state_publisher를 실행한다.

publish_frequency는 가변 좌표 관계의 발행 빈도 상한을 설정한다. 이 값을 높인다고 관절 상태가 저절로 만들어지지는 않는다. 예제에서는 관절 상태를 초당 약 10회 보내므로 상한을 20.0으로 두어도 새로운 상태의 공급 주기가 먼저 영향을 준다. LaunchDescription에는 모델을 발행하는 노드 하나만 넣었고, 시험 상태 발행기는 별도 터미널에서 실행한다.

joint_driver.py의 상태 생성

HeadStatePublisher는 /joint_states 발행자와 0.1초 주기의 타이머를 만든다. started_at은 각도 함수의 시작 시각이다. 콜백은 ROS 시계로 경과 시간을 구한 뒤 사인 함수에 넣는다. 경과 시간에 1rad/s의 위상 변화율을 적용한 식이므로 각도의 진폭은 π/4, 최대 각속도도 π/4rad/s다. 모델에 기록한 각도 범위와 1rad/s의 속도 제한 안에서 움직인다.

메시지 생성 뒤에는 시각, 조인트 이름, 각도를 차례로 채운다. velocity와 effort는 이 예제에서 제공하지 않으므로 빈 배열로 남긴다. JointState는 제공하지 않는 상태 배열을 비워 둘 수 있다. 대신 제공하는 name과 position의 길이와 순서는 맞아야 한다.

main은 ROS를 초기화하고 노드를 만든 뒤 spin으로 콜백을 실행한다. 종료 요청을 받으면 노드를 정리하고, 아직 유효한 ROS 문맥을 종료한다. 첫 print는 발행기가 시작되었음을 사람이 확인하기 위한 한 줄이다. 실제 하드웨어를 움직이는 명령은 전송하지 않는다.

model_math.py의 좌표 계산

pose_z는 z축 회전과 평행 이동을 담은 4×4 행렬을 만든다. 왼쪽 위 3×3은 방향을 나타내고 마지막 열의 앞 세 값은 위치를 나타낸다. 이 함수는 임의의 rpy를 처리하는 URDF 해석기가 아니다. 설치 자세가 모두 0이고 움직이는 축이 z축인 이번 모델의 계산을 재현한다.

multiply는 행과 열의 곱을 더하는 행렬 곱셈이다. sensor_pose의 base_from_head는 헤드 좌표로 적은 점을 차체 좌표로 바꾸는 관계다. head_from_sensor는 센서 좌표를 헤드 좌표로 바꾸는 관계다. 따라서 두 행렬을 그 순서로 곱하면 센서 좌표를 차체 좌표로 바꿀 수 있다.

센서 원점의 위치는 합성 행렬의 마지막 열에서 읽는다. 수식으로 쓰면 x는 0.20 + 0.10cosθ, y는 0.10sinθ, z는 0.30 + 0.05다. 90도에서 앞쪽 간격이 왼쪽 간격으로 바뀌는 이유가 이 식에 드러난다.

verify는 0도와 90도의 위치를 확인하고, 90도에서 센서의 x축이 차체의 y축을 향하는지도 확인한다. 위치만 맞고 방향이 틀린 구현을 놓치지 않기 위한 검사다. 부동소수점 연산에서는 0에 가까운 작은 오차가 생길 수 있으므로 isclose로 비교한다. 상수는 xacro와 같은 값을 직접 적었다. 모델 수치를 바꾸었다면 보조 예제와 기대값도 함께 검토해야 한다.

실행 결과

먼저 네 파일을 저장한 디렉터리에서 Python 문법을 검사한다. 다음 명령은 경고를 오류로 취급한다. 정상적으로 끝나면 표준 출력은 없다. py_compile은 import 대상 모듈을 실행하지 않으므로 ROS가 없어도 이 문법 검사를 수행할 수 있다.

python3 -W error -m py_compile model.launch.py joint_driver.py model_math.py

보조 예제는 다음 명령으로 실행한다.

python3 -W error model_math.py

예상 표준 출력은 다음과 같다. 이 출력은 코드의 계산식과 서식에 따른 것이며, 여기서 ROS 실행 검증을 수행했다는 뜻은 아니다.

좌표 검증 통과
head_yaw= 0 deg: x=0.300, y=0.000, z=0.350
head_yaw=90 deg: x=0.200, y=0.100, z=0.350

ROS 환경에서는 먼저 xacro 전개 결과를 별도 파일로 저장한다. 명령이 성공하면 표준 출력 대신 duri.urdf에 XML이 기록된다. 파일을 열어 xacro 호출이 사라지고 세 링크와 두 조인트가 남았는지 확인한다.

xacro duri.urdf.xacro -o duri.urdf

첫 번째 터미널에서 모델 발행기를 실행한다. 생성된 duri.urdf는 점검용이며, 이 실행 파일은 원본 xacro를 직접 전개한다.

ros2 launch ./model.launch.py

같은 ROS 환경을 설정한 두 번째 터미널에서는 관절 상태를 발행한다.

python3 joint_driver.py

프로그램이 직접 출력하는 시작 문구는 다음과 같다.

head_yaw 상태 발행 시작

세 번째 터미널에서 상태 메시지 하나를 확인할 수 있다.

ros2 topic echo /joint_states --once

메시지의 name에는 head_yaw가 들어가고 position에는 −π/4부터 π/4 사이의 값 하나가 들어간다. 실행 시점에 따라 시각과 각도가 달라지므로 고정된 출력으로 제시하지 않는다. launch의 시각, 프로세스 번호, 로그도 실행 환경에 따라 달라진다.

RViz2로 확인한다면 기준 좌표계를 base_link로 설정하고 RobotModel 표시를 추가한다. 설명 소스를 토픽으로 선택해 /robot_description을 사용하면 robot_state_publisher가 제공하는 모델을 읽을 수 있다. 헤드와 센서가 함께 회전하고 차체는 기준 위치에 남아 있어야 한다. 시험 발행기는 ±45도만 움직이며, 보조 예제의 90도는 모델의 경계 자세를 따로 계산한 결과다.

실무에서 자주 틀리는 것

센서의 설치 간격을 차체 기준으로 적는다

sensor_mount의 부모가 head_link인데 차체에서 측정한 최종 좌표를 넣으면 간격이 중복된다. 다음은 sensor_mount 안의 잘못된 origin이다.

<origin xyz="0.30 0 0.35" rpy="0 0 0"/>

헤드에서 센서까지의 간격으로 고친다.

<origin xyz="0.10 0 0.05" rpy="0 0 0"/>

부품의 위치를 수정하기 전에 그 수치를 어느 링크에서 측정했는지 기록한다. visual의 origin으로 그림만 옮기면 센서 기준점은 이전 위치에 남는다는 점도 함께 확인한다.

관절 상태에 링크 이름을 넣는다

다음 코드는 회전하는 부품의 이름을 보내지만, 모델에서 상태를 찾아야 하는 대상은 조인트다.

message.name = ["head_link"]
message.position = [angle]

URDF에 정의한 조인트 이름으로 고친다.

message.name = ["head_yaw"]
message.position = [angle]

여러 조인트를 보낼 때는 이름 배열과 위치 배열을 따로 정렬하지 않는다. 이름과 값의 대응이 바뀌어도 각 배열 자체는 문법적으로 정상이라 원인을 찾기 어려워진다.

회전각을 도 단위로 보낸다

다음 코드는 45도를 의도했더라도 45라디안으로 해석된다.

message.position = [45.0]

도 단위의 입력을 받았다면 메시지를 만들기 전에 변환한다.

message.position = [math.radians(45.0)]

limit을 적어 두었다고 잘못된 입력이 자동 보정되지는 않는다. 범위 밖의 값이 들어왔을 때 거부할지 보고할지는 상태를 생산하는 계층의 정책으로 정해야 한다.

xacro 원문을 robot_description에 넣는다

다음 코드는 파일을 읽기는 하지만 매크로를 처리하지 않는다.

robot_xml = model_path.read_text(encoding="utf-8")

xacro 원본이라면 전개한 결과를 전달한다.

robot_xml = xacro.process_file(str(model_path)).toxml()

이미 전개된 URDF 파일은 문자열로 읽어도 된다. 확장자만 바꾸는 것으로는 전개가 이루어지지 않는다. XML로 읽을 수 있다는 사실과 올바른 URDF 구조라는 사실도 구별해야 한다.

한눈에 보기

두리 모델에서 설계 정보와 실행 상태를 나누는 기준

대상	담는 정보	확인할 점	예제 값
링크	부품 기준과 형상	이름의 고유성	sensor_link
조인트 origin	부모 기준 설치 관계	기준 링크와 단위	0.10, 0, 0.05m
조인트 axis	움직이는 축	조인트 좌표계 기준	0, 0, 1
xacro	속성과 반복 정의	전개 결과가 URDF인지	box_link 매크로
robot_description	전개된 모델 문자열	경로 대신 내용 전달	robot_xml
/joint_states	현재 조인트 상태	이름, 위치, 시각	head_yaw의 각도
robot_state_publisher	링크 사이 좌표 관계	모델과 상태의 대응	고정 및 가변 관계

두리의 구조를 바꾸는 작업은 모델 수정이고, 헤드를 움직인 뒤 현재 각도를 알리는 작업은 상태 갱신이다. 이 두 경로가 맞물리면 센서의 차체 기준 위치를 일관되게 계산할 수 있다. 다음 장에서 주행 기능의 흐름을 살펴볼 때도 이 모델은 차체와 센서가 어디에 있는지를 설명하는 기준으로 사용된다.

세부 동작의 사실 확인에는 robot_state_publisher 문서, xacro 문서, JointState 메시지 정의를 참고할 수 있다.

연습 문제

1. 센서를 헤드 앞쪽 0.15m로 옮긴다. 높이는 그대로다. 수정할 xacro 속성을 적고, 헤드 각도가 0도와 90도일 때 센서의 차체 기준 위치를 구한다.
2. 헤드를 회전하지 않는 부품으로 바꾼다. head_yaw의 조인트 종류와 불필요해지는 요소를 적고, 시험 관절 상태 발행기가 필요한지 설명한다.
3. model_math.py에 −90도 위치 검사를 추가한다. 기대 위치와 센서 x축의 차체 기준 방향을 구한다.
4. sensor_mount를 그대로 둔 채 sensor_link의 visual 원점만 x축으로 0.05m 옮겼다. sensor_link의 좌표 관계와 표시 형상이 각각 어떻게 바뀌는지 설명한다.

정답과 해설

1. sensor_x를 0.15로 수정한다. 0도에서는 두 앞쪽 간격을 더하므로 위치는 (0.35, 0.00, 0.35)m다. 90도에서는 헤드 기준 앞쪽 간격이 차체 기준 왼쪽 간격으로 바뀌므로 (0.20, 0.15, 0.35)m다. model_math.py의 SENSOR_X와 verify의 기대값도 수정해야 한다.

2. head_yaw의 type을 fixed로 바꾸고 axis와 limit을 제거한다. parent, child, origin은 유지한다. 이 경우 두 조인트가 모두 고정되므로 모델의 링크 관계를 얻는 데 joint_driver.py가 필요하지 않다. head_link는 기존 origin에 의해 정해진 자세에 고정된다.

   <joint name="head_yaw" type="fixed">
     <parent link="base_link"/>
     <child link="head_link"/>
     <origin xyz="${head_x} 0 ${head_z}" rpy="0 0 0"/>
   </joint>
   

3. 위치는 (0.20, −0.10, 0.35)m다. 센서의 x축은 차체 기준 음의 y축을 향하므로 방향은 (0, −1, 0)이다. verify의 cases에 다음 항목을 넣으면 위치를 검사할 수 있다.

   (-math.pi / 2.0, (0.20, -0.10, 0.35)),
   

   방향 검사에는 sensor_pose(-math.pi / 2.0)의 첫 번째 열을 사용하고 (0.0, -1.0, 0.0)과 비교한다. 양쪽 회전을 검사하면 회전 부호를 잘못 적용한 구현을 발견하는 데 도움이 된다.

4. sensor_link의 좌표 관계는 바뀌지 않는다. 조인트 origin을 수정하지 않았기 때문이다. 화면에 표시하는 상자만 센서 좌표계의 x축 방향으로 0.05m 이동한다. 헤드가 회전하면 이 표시 간격도 함께 회전한다. 실제 센서의 기준점이 옮겨진 상황이라면 visual만 고치는 대신 sensor_mount의 설치 관계를 수정해야 한다.