URDF를 이용해 만든 로봇에 가상 자동차 연결하기 (1)

이번 포스팅은 URDF를 이용한 간단한 로봇 만들기 (3) 과정을 통해서 만들어진 로봇에 가상의 자동차를 만들어서 연결해서 테스트하고 이후에 실제 자동차를 연결하기 위한 사전 준비를 해 보는 과정입니다.

 

이 포스트는 다음 과정을 완료한 후에 참고하시길 바랍니다.

• URDF를 이용한 간단한 로봇 만들기 (1)
• URDF를 이용한 간단한 로봇 만들기 (2)
• URDF를 이용한 간단한 로봇 만들기 (3)

1. car_tutorial package 생성 및 설정하기

• 이전 과정에서 테스트를 위해서 사용했던 'urdf_tutorial' package에서 최소한의 필요한 기능만을 포함하는 'car_tutorial' package를 만들고 동작하도록 하는 과정입니다.
• 터미널에서 아래 명령을 실행해서 'car_tutorial' package를 생성합니다.

$ cd ~/Workspace/ros_ws/src/
$ ros2 pkg create --build-type ament_python car_tutorial

• car_tutorial' package 아래 다음 3개의 폴더를 생성합니다.
  ◦ src/car_tutorial/urdf
  ◦ src/car_tutorial/launch
  ◦ src/car_tutorial/config
• 'src/car_tutorial/package.xml' 파일의 내용을 아래와 같이 편집합니다.

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>car_tutorial</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="cchyun@todo.todo">cchyun</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>geometry_msgs</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

• 'src/car_tutorial/setup.py' 파일의 내용을 아래와 같이 편집합니다.

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

package_name = 'car_tutorial'

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('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*')),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/*.xacro')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='cchyun',
    maintainer_email='cchyun@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

• 'src/car_tutorial/urdf/car.xacro' 파일을 생성하고 아래와 같이 편집합니다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="urdf_tutorial">

    <!-- COLOR -->
    <material name="white">
        <color rgba="1 1 1 1" />
    </material>

    <material name="blue">
        <color rgba="0 0 1 1"/>
    </material>

    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>

    <material name="red">
        <color rgba="1 0 0 1"/>
    </material>

    <!-- BASE LINK -->
    <link name="base_link">
    </link>

    <!-- BODY LINK -->
    <joint name="body_joint" type="fixed">
        <parent link="base_link"/>
        <child link="body"/>
        <origin xyz="-0.12 0 0"/>
    </joint>

    <link name="body">
        <visual>
            <origin xyz="0.1 0 0.03"/>
            <geometry>
                <box size="0.2 0.1 0.06"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin xyz="0.1 0 0.03"/>
            <geometry>
                <box size="0.2 0.1 0.06"/>
            </geometry>
        </collision>
    </link>

    <!-- LEFT WHEEL LINK -->
    <joint name="left_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="left_wheel"/>
        <origin xyz="0 0.065 0" rpy="-${pi/2} 0 0" />
        <axis xyz="0 0 1"/>
    </joint>

    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.03" length="0.03"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.03" length="0.03"/>
            </geometry>
        </collision>
    </link>

    <!-- RIGHT WHEEL LINK -->
    <joint name="right_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="right_wheel"/>
        <origin xyz="0 -0.065 0" rpy="${pi/2} 0 0" />
        <axis xyz="0 0 -1"/>
    </joint>

    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.03" length="0.03"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.03" length="0.03"/>
            </geometry>
        </collision>
    </link>

    <!-- CASTER WHEEL LINK -->
    <joint name="caster_wheel_joint" type="fixed">
        <parent link="body"/>
        <child link="caster_wheel"/>
        <origin xyz="0.03 0 0"/>
    </joint>

    <link name="caster_wheel">
        <visual>
            <geometry>
                <sphere radius="0.03"/>
            </geometry>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="0.03"/>
            </geometry>
        </collision>
    </link>

    <!-- CAMERA -->
    <joint name="camera_joint" type="fixed">
        <parent link="body"/>
        <child link="camera_link"/>
        <origin xyz="0.20 0 0.03" rpy="0 0 0"/>
    </joint>

    <link name="camera_link">
        <visual>
            <geometry>
                <box size="0.01 0.03 0.03"/>
            </geometry>
            <material name="red"/>
        </visual>
        <collision>
            <geometry>
                <box size="0.01 0.03 0.03"/>
            </geometry>
        </collision>
    </link>

    <joint name="camera_optical_joint" type="fixed">
        <parent link="camera_link"/>
        <child link="camera_link_optical"/>
        <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
    </joint>

    <link name="camera_link_optical">
    </link>

</robot>

• 'src/car_tutorial/launch/fake.launch.py' 파일을 생성하고 아래와 같이 편집합니다.

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import xacro

def generate_launch_description():
    package_name = 'car_tutorial'

    # robot_state_publisher
    pkg_path = os.path.join(get_package_share_directory(package_name))
    xacro_file = os.path.join(pkg_path, 'urdf', 'car.xacro')
    robot_description = xacro.process_file(xacro_file)
    params = {'robot_description': robot_description.toxml(), 'use_sim_time': False}

    rsp = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params],
    )

    # rviz2
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', 'src/car_tutorial/config/car.rviz'],
    )

    return LaunchDescription(
        [
            rsp,
            rviz,
        ]
    )

• 터미널에서 아래 명령을 실행해서 'src/car_tutorial/launch/fake.launch.py'를 실행합니다.

$ cd ~/Workspace/ros_ws/
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch car_tutorial fake.launch.py

• 실행되는 rviz 화면에서 'Fixed Frame'을 'base_link'로 설정합니다.

• Rviz에서 'Add' 버튼을 누른 후 뜨는 팝업에서 TF를 선택 후 'Ok' 버튼을 눌러 TF를 추가합니다.

• Rviz에서 'Add' 버튼을 누른 후 뜨는 팝업에서 RobotModel를 선택 후 'Ok' 버튼을 눌러 RobotModel를 추가합니다.

• RobotModel의 'Description Topic'을 '/robot_description'으로 설정하면 아래와 같이 Robot 형상을 볼 수 있습니다. 이때 'Status' Error가 발생하는데 이유는 두 개의 Joint (left_wheel_joint, right_wheel_joint)에 대한 /joint_states Topic을 수신하지 못했기 때문입니다. 이 문제는 다음 단계에서 해결하도록 하겠습니다.

• Rviz에서 'Add' 버튼을 누른 후 뜨는 팝업에서 Image를 선택 후 'Ok' 버튼을 눌러 Image를 추가합니다. Image도 아무것도 보이지 않습니다. 이 문제도 다음 단계에서 해결하도록 하겠습니다.

• Rviz 메뉴에서 File >> Save Config As를 선택 후 뜨는 팝업에서 현재 설정을 'src/car_tutorial/config/car.rviz'에 저장합니다.

2. FakeDriver 개념

• ROS에 자동차를 제어하고 Rviz로 확인하기 위한 최소의 구성은 다음과 같습니다.
  ◦ Keyboard Controller: 자동차의 방향/속도를 제어하는 명령을 Topic으로 보냅니다.
  ◦ Real Driver: 자동차에 방향/속도를 제어하는 명령을 보내고 실제 자동차의 방향/속도의 측정값 및 카메라 화면 정보를 수신해서 Topic으로 보냅니다.
  ◦ Odometry: 자동차의 방향/속도의 측정값을 이용해서 실제 자동차의 위치를 계산합니다.

• 이번 과정에서는 실제 자동차가 아닌 가상의 Driver를 이용해서 실제 자동차가 있는 것처럼 시뮬레이션해서 ROS의 동작을 확인해 보겠습니다.

3. FakeDriver - joint_states publish

• joint_states는 URDF에서 정의한 두 개의 Joint (left_wheel_joint, right_wheel_joint) 정보입니다. 이번에 제작할 로봇에는 모터의 회전을 측정할 수 있는 encoder가 없기 때문에 실제 로봇에서 정보를 얻을 수 없지만 Rviz오류 해결 및 기능 확인을 위해서 간단하게 구현일 해 보겠습니다.
• 'src/car_tutorial/car_tutorial/fake_driver.py' 파일을 생성하고 아래와 같이 편집합니다. 간단한 코드 설명은 아래와 같습니다.
  ◦ 매 0.1초마다 publish_callback을 실행합니다.
  ◦ publish_callback에서 joint_states Topic을 발송하고 joint 값을 0.05씩 증가시킵니다.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor

from sensor_msgs.msg import JointState


class FakeDriver(Node):
    def __init__(self):
        super().__init__('fake_driver')

        # joint states publisher
        self.pub_joint_states = self.create_publisher(JointState, "joint_states", 10)

        # init variable
        self.joint_states = JointState()
        self.joint_states.header.frame_id = "joint_states"
        self.joint_states.name = ["left_wheel_joint", "right_wheel_joint"]
        self.joint_states.position = [0.0, 0.0]

        # timer
        self.timer = self.create_timer(0.1, self.publish_callback)
    
    def publish_callback(self):
        curr_time = self.get_clock().now()

        # joint states
        self.joint_states.header.stamp = curr_time.to_msg()

        # publish
        self.pub_joint_states.publish(self.joint_states)

        # simulate wheel rotate
        self.joint_states.position[0] += 0.05
        self.joint_states.position[1] += 0.05


def main(args=None):
    rclpy.init(args=args)

    driver = FakeDriver()
    executor = MultiThreadedExecutor()
    rclpy.spin(driver, executor=executor)

    driver.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• 'src/car_tutorial/setup.py'의 console_scripts 부분에 아래 내용을 추가합니다.

            'fake_driver = car_tutorial.fake_driver:main',

• 'src/car_tutorial/launch/fake.launch.py' 파일을 아래와 같이 수정합니다.

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import xacro

def generate_launch_description():
    package_name = 'car_tutorial'

    # robot_state_publisher
    pkg_path = os.path.join(get_package_share_directory(package_name))
    xacro_file = os.path.join(pkg_path, 'urdf', 'car.xacro')
    robot_description = xacro.process_file(xacro_file)
    params = {'robot_description': robot_description.toxml(), 'use_sim_time': False}

    rsp = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params],
    )

    # fake driver
    fake_driver = Node(
        package='car_tutorial',
        executable='fake_driver',
        output='screen',
        parameters=[],
    )

    # rviz2
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', 'src/car_tutorial/config/car.rviz'],
    )

    return LaunchDescription(
        [
            rsp,
            fake_driver,
            rviz,
        ]
    )

• 터미널에서 아래 명령을 실행해서 'src/car_tutorial/launch/fake.launch.py'를 실행합니다.

$ cd ~/Workspace/ros_ws/
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch car_tutorial fake.launch.py

• 아래 그림과 같이 Rviz에서 오류가 없어지고, 로봇이 정상으로 보이면서 바퀴가 회전하는 것을 확인할 수 있습니다.

4. FakeDriver - cmd_vel subscribe and vel_raw publish

• 이번 과정을 키보드(teleop_twist_keyboard)를 이용해서 로봇에게 제어 명령을 내리면 로봇이 이동하는 기능에 대한 과정입니다.
• 실제 로봇은 Driver가 명령을 받으면 자동차에게 그 명령을 따라 동작하도록 명령을 전달하고 로봇으로부터 실제 이동 속도 등의 정보를 받아야 합니다.
• 지금은 가상 로봇이기 때문에 입력받은 명령 그대로 로봇이 이동한다고 가정하겠습니다. 따라서 키보드 입력값 그대로를 로봇의 실제 이동 정보로 사용하겠습니다.
• 'src/car_tutorial/car_tutorial/fake_driver.py' 파일을 아래와 같이 편집합니다. 코드의 내용을 간단히 설명하면 아래와 같습니다.
  ◦ 키보드를 이용한 자동차 제어 명령인 'cmd_vel' Topic을 수신할 subscriber를 등록합니다.
  ◦ 현재 로봇의 속도를 'vel_raw' Topic으로 전송할 publisher를 등록합니다.
  ◦ 0.1초에 한 번씩 'vel_raw' Topic을 발송합니다.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor

from geometry_msgs.msg import Twist, TwistStamped
from sensor_msgs.msg import JointState


class FakeDriver(Node):
    def __init__(self):
        super().__init__('fake_driver')

        # joint states publisher
        self.pub_joint_states = self.create_publisher(JointState, "joint_states", 10)
        # cmd vel subscriber
        self.sub_cmd_vel = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10)
        # vel raw publisher
        self.pub_vel_raw = self.create_publisher(TwistStamped, "vel_raw", 10)

        # init variable
        self.joint_states = JointState()
        self.joint_states.header.frame_id = "joint_states"
        self.joint_states.name = ["left_wheel_joint", "right_wheel_joint"]
        self.joint_states.position = [0.0, 0.0]

        self.vel_raw = TwistStamped()

        # timer
        self.timer = self.create_timer(0.1, self.publish_callback)
    
    def cmd_vel_callback(self, msg):
        self.get_logger().info(f'recv cmd_vel message {msg}')
        self.vel_raw.twist = msg
    
    def publish_callback(self):
        curr_time = self.get_clock().now()

        # joint states
        self.joint_states.header.stamp = curr_time.to_msg()
        
        # vel raw
        self.vel_raw.header.stamp = curr_time.to_msg()

        # publish
        self.pub_joint_states.publish(self.joint_states)
        self.pub_vel_raw.publish(self.vel_raw)

        # simulate wheel rotate
        self.joint_states.position[0] += 0.05
        self.joint_states.position[1] += 0.05


def main(args=None):
    rclpy.init(args=args)

    driver = FakeDriver()
    executor = MultiThreadedExecutor()
    rclpy.spin(driver, executor=executor)

    driver.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• 첫 번째 터미널에서 아래 명령을 실행해서 'src/car_tutorial/launch/fake.launch.py'를 실행합니다.

$ cd ~/Workspace/ros_ws/
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch car_tutorial fake.launch.py

• 두 번째 터미널에서 아래 명령을 실행합니다.

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard

• 두 번째 터미널에서 자동차 제어 명령을 입력하면 첫 번째 터미널에서 cmd_val_callback에서 코딩한 로그가 출력되는 것을 확인할 수 있습니다. 하지만 Rviz에서 자동차가 움직이고 있지는 않습니다. Rviz에서 자동차가 움직이도록 하는 기능은 다음 포스팅에서 설명하겠습니다.

• 세 번째 터미널에서 아래 명령을 실행합니다.

$ rqt_graph

• 아래 그림과 같이 지금까지 실행한 Node들이 어떤 Topic을 주고받는지를 확인할 수 있습니다.

5. FakeDriver - img_raw publish

• 이번 과정을 로봇에 장착된 카마라의 정보를 수신해서 Rviz에 출력하는 기능에 대한 과정입니다.
• 지금은 가상 로봇이기 때문에 카메라 대신에 임의의 사진을 카메라에서 받은 1 프레임의 이미지라고 가정하겠습니다.
• 우선 적당한 크기의 (400 x 300) 이미지를 'src/car_tutorial/car_tutorial/photo.png'에 저장합니다.

• 'src/car_tutorial/car_tutorial/fake_driver.py' 파일을 아래와 같이 편집합니다. 코드의 내용을 간단히 설명하면 아래와 같습니다.
  ◦ 현재 로봇의 카메라 사진을  'img_raw' Topic으로 전송할 publisher를 등록합니다.
  ◦ 0.1초에 한 번씩 사진 데이터를 Topic을 발송합니다.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor

from geometry_msgs.msg import Twist, TwistStamped
from sensor_msgs.msg import JointState, Image

import cv2
from cv_bridge import CvBridge


class FakeDriver(Node):
    def __init__(self):
        super().__init__('fake_driver')

        # joint states publisher
        self.pub_joint_states = self.create_publisher(JointState, "joint_states", 10)
        # cmd vel subscriber
        self.sub_cmd_vel = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10)
        # vel raw publisher
        self.pub_vel_raw = self.create_publisher(TwistStamped, "vel_raw", 10)
        # img raw publisher
        self.pub_img_raw = self.create_publisher(Image, "img_raw", 10)

        # init variable
        self.joint_states = JointState()
        self.joint_states.header.frame_id = "joint_states"
        self.joint_states.name = ["left_wheel_joint", "right_wheel_joint"]
        self.joint_states.position = [0.0, 0.0]

        self.vel_raw = TwistStamped()

        self.bridge = CvBridge()

        # timer
        self.timer = self.create_timer(0.1, self.publish_callback)
    
    def cmd_vel_callback(self, msg):
        self.get_logger().info(f'recv cmd_vel message {msg}')
        self.vel_raw.twist = msg
    
    def publish_callback(self):
        curr_time = self.get_clock().now()

        # joint states
        self.joint_states.header.stamp = curr_time.to_msg()
        
        # vel raw
        self.vel_raw.header.stamp = curr_time.to_msg()

        # image
        image = cv2.imread('src/car_tutorial/car_tutorial/photo.png', cv2.IMREAD_COLOR)
        img_raw = self.bridge.cv2_to_imgmsg(image)

        # publish
        self.pub_joint_states.publish(self.joint_states)
        self.pub_vel_raw.publish(self.vel_raw)
        self.pub_img_raw.publish(img_raw)

        # simulate wheel rotate
        self.joint_states.position[0] += 0.05
        self.joint_states.position[1] += 0.05


def main(args=None):
    rclpy.init(args=args)

    driver = FakeDriver()
    executor = MultiThreadedExecutor()
    rclpy.spin(driver, executor=executor)

    driver.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• 첫 번째 터미널에서 아래 명령을 실행해서 'src/car_tutorial/launch/fake.launch.py'를 실행합니다.

$ cd ~/Workspace/ros_ws/
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch car_tutorial fake.launch.py

• Rviz에서 Image의 Topic을 '/img_raw'로 바꾸면 아래 그림과 같이 위에서 저장한 이미지가 보이은 것을 확인할 수 있습니다.