URDF : Using URDF with robot_state_publisher

https://docs.ros.org/en/jazzy/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher.html

Using URDF with robot_state_publisher — ROS 2 Documentation: Jazzy documentation

 

background

• 이번 튜토리얼은 걷는 로봇을 모델링하고, 상태를 tf2 메시지로 publish합니다. 그리고 Rviz에서 시뮬레이션을 보는 방법을 보여줍니다.
  • 1. 우선 로봇 조립을 위해서 URDF 모델을 생성한다.
  • 2. 동작을 시뮬레이션하고 jointState와 변환을 publish하는 노드를 작성한다.
  • 3. robot_state_publisher를 사용해서 전체 로봇 상태를 /tf2에 publish한다.

 

tasks

• 1. 우선 작업할 디렉토리를 만들고 패키지도 만들자.

mkdir -p second_ros2_ws/src

cd second_ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 urdf_tutorial_r2d2 --dependencies rclpy
cd urdf_tutorial_r2d2

 

• 2. assets을 저장할 dir을 만들자. 그리고 해당 디렉토리에는 나중에 아래 2개의 파일이 저장될거임.
  • second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml
  • second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz

mkdir -p urdf

 

• 3. state를 publish하는 파일을 만들자
  • second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py
  • 우리는 로봇의 state를 구체적으로 알수있는 메서드가 필요하다. 그래서 우리는 3개의 모든 joint와 overall odometry를 명세화 시켜야 한다.

from math import sin, cos, pi
import rclpy  # ROS 2 Python 클라이언트 라이브러리
from rclpy.node import Node  # ROS 2 노드 클래스를 임포트
from rclpy.qos import QoSProfile  # Quality of Service 설정을 위한 클래스
from geometry_msgs.msg import Quaternion  # 기하학적 회전 정보를 담는 메시지 타입
from sensor_msgs.msg import JointState  # 로봇의 조인트 상태를 표현하는 메시지 타입
from tf2_ros import TransformBroadcaster, TransformStamped  # 좌표 변환 정보를 퍼블리시하기 위한 클래스

# 로봇의 상태를 퍼블리시하는 노드 클래스 정의
class StatePublisher(Node):

    def __init__(self):
        rclpy.init()  # ROS 2 초기화
        super().__init__('state_publisher')  # 'state_publisher'라는 이름의 노드 초기화

        # QoS (Quality of Service) 설정: 퍼블리시 큐의 깊이를 10으로 설정
        qos_profile = QoSProfile(depth=10)

        # 조인트 상태를 퍼블리시할 퍼블리셔 생성
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)

        # 좌표 변환 정보를 퍼블리시할 TransformBroadcaster 생성
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)

        # 노드 이름 가져오기 및 시작 메시지 출력
        self.nodeName = self.get_name()
        self.get_logger().info(f"{self.nodeName} started")

        # 각도를 라디안으로 변환하는 상수 (1도 = pi/180 라디안)
        degree = pi / 180.0

        # 루프 속도 설정 (초당 30회 루프 실행)
        loop_rate = self.create_rate(30)

        # 초기 로봇 상태 설정
        tilt = 0.0      # 기울기 초기값
        tinc = degree   # 기울기 변화량
        swivel = 0.0    # 회전 각도 초기값
        angle = 0.0     # 로봇이 움직일 원의 각도
        height = 0.0    # 로봇의 높이 초기값
        hinc = 0.005    # 높이 변화량

        # 메시지 객체 선언 및 초기 설정
        odom_trans = TransformStamped()  # 좌표 변환 메시지 객체
        odom_trans.header.frame_id = 'odom'  # 부모 프레임 설정
        odom_trans.child_frame_id = 'axis'   # 자식 프레임 설정

        joint_state = JointState()  # 조인트 상태 메시지 객체

        try:
            while rclpy.ok():  # ROS 2 노드가 실행 중일 때 루프 반복
                rclpy.spin_once(self)  # 콜백 함수가 있다면 한 번 실행 (비동기 처리를 위해)

                # 현재 시간 정보 가져오기
                now = self.get_clock().now()

                # 조인트 상태 메시지 업데이트
                joint_state.header.stamp = now.to_msg()  # 현재 시간으로 타임스탬프 설정
                joint_state.name = ['swivel', 'tilt', 'periscope']  # 조인트 이름 리스트
                joint_state.position = [swivel, tilt, height]       # 각 조인트의 현재 위치 설정

                # 좌표 변환 정보 업데이트
                odom_trans.header.stamp = now.to_msg()  # 현재 시간으로 타임스탬프 설정
                odom_trans.transform.translation.x = cos(angle) * 2  # 원 궤도상의 x 좌표 (반지름 = 2)
                odom_trans.transform.translation.y = sin(angle) * 2  # 원 궤도상의 y 좌표
                odom_trans.transform.translation.z = 0.7            # 고정된 z 좌표 (높이)

                # 로봇의 회전 정보 (롤, 피치, 요)를 쿼터니언으로 변환하여 설정
                odom_trans.transform.rotation = euler_to_quaternion(0, 0, angle + pi/2)

                # 퍼블리셔를 통해 메시지 전송
                self.joint_pub.publish(joint_state)         # 조인트 상태 퍼블리시
                self.broadcaster.sendTransform(odom_trans)  # 좌표 변환 정보 퍼블리시

                # 로봇 상태 업데이트 (애니메이션 효과를 위해 주기적으로 값 변경)
                tilt += tinc  # 기울기 증가
                if tilt < -0.5 or tilt > 0.0:  # 기울기 범위를 벗어나면 방향 전환
                    tinc *= -1

                height += hinc  # 높이 증가
                if height > 0.2 or height < 0.0:  # 높이 범위를 벗어나면 방향 전환
                    hinc *= -1

                swivel += degree  # 회전 각도 증가
                angle += degree / 4  # 원 궤도상의 각도 증가

                loop_rate.sleep()  # 루프 주기 맞추기 위해 대기

        except KeyboardInterrupt:  # Ctrl+C로 프로그램 종료 시 처리
            pass

# 오일러 각(roll, pitch, yaw)을 쿼터니언으로 변환하는 함수
def euler_to_quaternion(roll, pitch, yaw):
    qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
    qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
    qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
    qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
    return Quaternion(x=qx, y=qy, z=qz, w=qw)  # 변환된 쿼터니언 반환

# 메인 함수: 노드 실행 시작
def main():
    node = StatePublisher()  # StatePublisher 노드 객체 생성

if __name__ == '__main__':
    main()  # 메인 함수 실행

 

 

• 4. launch file도 생성하자.
  • second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo_launch.py

import os
from ament_index_python.packages import get_package_share_directory  # 패키지의 공유 디렉토리 경로를 얻기 위한 모듈
from launch import LaunchDescription  # Launch 파일을 정의하는 클래스
from launch.actions import DeclareLaunchArgument  # 런치 아규먼트(인자)를 선언하는 클래스
from launch.substitutions import LaunchConfiguration  # 런치 아규먼트를 런타임에 사용할 수 있도록 설정
from launch_ros.actions import Node  # ROS 2 노드를 실행하기 위한 클래스

# Launch 파일을 생성하는 함수
def generate_launch_description():

    # 'use_sim_time'이라는 런치 아규먼트를 선언하고 기본값을 'false'로 설정
    # 이는 시뮬레이션(Gazebo) 시간 사용 여부를 결정함
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    # URDF 파일 이름 설정
    urdf_file_name = 'r2d2.urdf.xml'

    # URDF 파일의 경로를 얻기 위해 패키지 디렉토리를 찾음
    urdf = os.path.join(
        get_package_share_directory('urdf_tutorial_r2d2'),  # 패키지 이름
        urdf_file_name  # 파일 이름
    )

    # URDF 파일을 읽어 로봇 설명을 변수에 저장
    with open(urdf, 'r') as infp:
        robot_desc = infp.read()

    # LaunchDescription 객체를 반환하여 런치 파일 정의
    return LaunchDescription([

        # 'use_sim_time' 런치 아규먼트 선언
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'  # 시뮬레이션 시간 사용 여부 설명
        ),

        # robot_state_publisher 노드 실행
        Node(
            package='robot_state_publisher',  # 노드가 포함된 패키지 이름
            executable='robot_state_publisher',  # 실행 파일 이름
            name='robot_state_publisher',  # 노드 이름 지정
            output='screen',  # 노드의 출력 결과를 화면에 표시
            parameters=[  # 노드에 전달할 파라미터 설정
                {'use_sim_time': use_sim_time, 'robot_description': robot_desc}
            ],
            arguments=[urdf]  # URDF 파일 경로를 인자로 전달
        ),

        # state_publisher 노드 실행
        Node(
            package='urdf_tutorial_r2d2',  # 노드가 포함된 패키지 이름
            executable='state_publisher',  # 실행 파일 이름
            name='state_publisher',  # 노드 이름 지정
            output='screen'  # 노드의 출력 결과를 화면에 표시
        ),
    ])

 

• 5. setup.py를 아래와 같이 수정

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'urdf_tutorial_r2d2'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
        (os.path.join('share', package_name), glob('urdf/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='jihong',
    maintainer_email='jihong@todo.todo',
    description='TODO: Package description',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
        ],
    },
)
~

 

 

• 6. 패키지 설치 후 적용 및 실행

cd second_ros2_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2

source install/setup.bash

ros2 launch urdf_tutorial_r2d2 demo_launch.py

rviz2 -d second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz