이번 포스팅은 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'로 바꾸면 아래 그림과 같이 위에서 저장한 이미지가 보이은 것을 확인할 수 있습니다.
