이 글은 ROS2 로봇 패키지 프로그램 개발 방법 정리한다.
URDF 모델링
시뮬레이션을 위한 Universal Robot Description Format (URDF) 을 사용해 모델링한다. URDF는 XML포맷으로 로봇을 물리적으로 정의한다. 이 모델은 주요 두 컴포넌트가 있다.
- LINKS: 강체(RIGID PIECE)를 정의한다. 링크는 뼈와 같은 역할을 하며 각 조인트로 연결된다.
- JOINTS: 조인트는 연결된 링크 간에 운동이 가능하게 한다. 조인트는 서보 모터 등으로 구현된다.
sudo apt install ros-humble-joint-state-publisher-gui
sudo apt install ros-humble-xacro
이동형 로버 모델링
연구를 위해, 간단한 이동형 로버를 모델링한다. 다음 명령을 실행한다.
cd ~/projects/ros2/src
ros2 pkg create --build-type ament_cmake basic_mobile_robot
mkdir config launch maps meshes models params rviz worlds
cd ~/projects/ros2
colcon build
이 모델은 두개의 바퀴(revolute joints), 앞 바퀴(caster wheel), IMU, GPS를 가진다.
cd ~/projects/ros2/src/basic_mobile_robot/models
subl basic_mobile_bot_v1.urdf
다음 코드를 입력한다.
<?xml version="1.0" ?>
<robot name="basic_mobile_bot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- ****************** ROBOT CONSTANTS ******************************* -->
<!-- Define the size of the robot's main chassis in meters -->
<xacro:property name="base_width" value="0.39"/>
<xacro:property name="base_length" value="0.70"/>
<xacro:property name="base_height" value="0.20"/>
<!-- Define the shape of the robot's two back wheels in meters -->
<xacro:property name="wheel_radius" value="0.14"/>
<xacro:property name="wheel_width" value="0.06"/>
<!-- x-axis points forward, y-axis points to left, z-axis points upwards -->
<!-- Define the gap between the wheel and chassis along y-axis in meters -->
<xacro:property name="wheel_ygap" value="0.035"/>
<!-- Position the wheels along the z-axis -->
<xacro:property name="wheel_zoff" value="0.05"/>
<!-- Position the wheels along the x-axis -->
<xacro:property name="wheel_xoff" value="0.221"/>
<!-- Position the caster wheel along the x-axis -->
<xacro:property name="caster_xoff" value="0.217"/>
<!-- Define intertial property macros -->
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
<mass value="${m}"/>
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<mass value="${m}"/>
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}"/>
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
</inertial>
</xacro:macro>
<!-- ****************** ROBOT BASE FOOTPRINT *************************** -->
<!-- Define the center of the main robot chassis projected on the ground -->
<link name="base_footprint"/>
<!-- The base footprint of the robot is located underneath the chassis -->
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link" />
<origin xyz="0.0 0.0 ${(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
</joint>
<!-- ********************** ROBOT BASE ********************************* -->
<link name="base_link">
<visual>
<origin xyz="0 0 -0.05" rpy="1.5707963267949 0 3.141592654"/>
<geometry>
<mesh filename="package://basic_mobile_robot/meshes/robot_base.stl" />
</geometry>
<material name="Red">
<color rgba="${255/255} ${0/255} ${0/255} 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<xacro:box_inertia m="40.0" w="${base_width}" d="${base_length}" h="${base_height}"/>
</link>
<gazebo reference="base_link">
<material>Gazebo/Red</material>
</gazebo>
<!-- *********************** DRIVE WHEELS ****************************** -->
<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
<link name="${prefix}_link">
<visual>
<origin xyz="0 0 0" rpy="1.5707963267949 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<material name="White">
<color rgba="${255/255} ${255/255} ${255/255} 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
</collision>
<xacro:cylinder_inertia m="110.5" r="${wheel_radius}" h="${wheel_width}"/>
</link>
<!-- Connect the wheels to the base_link at the appropriate location, and
define a continuous joint to allow the wheels to freely rotate about
an axis -->
<joint name="${prefix}_joint" type="revolute">
<parent link="base_link"/>
<child link="${prefix}_link"/>
<origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
<limit upper="3.1415" lower="-3.1415" effort="30" velocity="5.0"/>
<axis xyz="0 1 0"/>
</joint>
</xacro:macro>
<!-- Instantiate two wheels using the macro we just made through the
xacro:wheel tags. We also define the parameters to have one wheel
on both sides at the back of our robot (i.e. x_reflect=-1). -->
<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
<!-- *********************** CASTER WHEEL ****************************** -->
<!-- We add a caster wheel. It will be modeled as sphere.
We define the wheel’s geometry, material and the joint to connect it to
base_link at the appropriate location. -->
<link name="front_caster">
<visual>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
<material name="White">
<color rgba="${255/255} ${255/255} ${255/255} 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
</collision>
<xacro:sphere_inertia m="10.05" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</link>
<gazebo reference="front_caster">
<mu1>0.01</mu1>
<mu2>0.01</mu2>
<material>Gazebo/White</material>
</gazebo>
<joint name="caster_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster"/>
<origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
</joint>
<!-- *********************** IMU SETUP ********************************* -->
<!-- Each sensor must be attached to a link. -->
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="-0.10 0 0.05" rpy="0 0 0"/>
</joint>
<link name="imu_link"/>
<!-- *********************** GPS SETUP ********************************** -->
<joint name="gps_joint" type="fixed">
<parent link="base_link"/>
<child link="gps_link"/>
<origin xyz="0.10 0 0.05" rpy="0 0 0"/>
</joint>
<link name="gps_link"/>
</robot>이제 보이는 형태를 표현할 메쉬 파일을 STL 형식으로 다음 폴더에 저장한다. 해당 메쉬파일은 여기(https://drive.google.com/drive/folders/1e9yB8f_LD7ty0M1h_mbd0kB8Hxr4TvfV) 에서 다운로드한다.
cd ~/projects/ros2/src/basic_mobile_robot/meshes
참고로, 이 메쉬 파일은 블렌더(blander)로 모델링된 것이다. 메쉬 저장 결과는 다음과 같다.
다음과 같이 명령 실행한다.
cd ~/projects/ros2/src/basic_mobile_robot
subl package.xml
이제 package.xml 파일을 다음과 같이 수정한다.
<buildtool_depend>ament_cmake</buildtool_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>xacro</exec_depend>
cd ~/projects/ros2/src/basic_mobile_robot/launch
subl basic_mobile_bot_v1.launch.py
실행 코드는 여기(https://drive.google.com/file/d/1pOgjTNuItNbn39nq0ztjxN0XBxj_v6H2/view)서 다운로드해 넣는다.
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Set the path to different files and folders.
pkg_share = FindPackageShare(package='basic_mobile_robot').find('basic_mobile_robot')
default_launch_dir = os.path.join(pkg_share, 'launch')
default_model_path = os.path.join(pkg_share, 'models/basic_mobile_bot_v1.urdf')
robot_name_in_urdf = 'basic_mobile_bot'
default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
# Launch configuration variables specific to simulation
gui = LaunchConfiguration('gui')
model = LaunchConfiguration('model')
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
use_sim_time = LaunchConfiguration('use_sim_time')
# Declare the launch arguments
declare_model_path_cmd = DeclareLaunchArgument(
name='model',
default_value=default_model_path,
description='Absolute path to robot urdf file')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
name='rviz_config_file',
default_value=default_rviz_config_path,
description='Full path to the RVIZ config file to use')
declare_use_joint_state_publisher_cmd = DeclareLaunchArgument(
name='gui',
default_value='True',
description='Flag to enable joint_state_publisher_gui')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
name='use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
name='use_rviz',
default_value='True',
description='Whether to start RVIZ')
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true')
# Specify the actions
# Publish the joint state values for the non-fixed joints in the URDF file.
start_joint_state_publisher_cmd = Node(
condition=UnlessCondition(gui),
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher')
# A GUI to manipulate the joint state values
start_joint_state_publisher_gui_node = Node(
condition=IfCondition(gui),
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui')
# Subscribe to the joint states of the robot, and publish the 3D pose of each link.
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'use_sim_time': use_sim_time,
'robot_description': Command(['xacro ', model])}],
arguments=[default_model_path])
# Launch RViz
start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', rviz_config_file])
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_model_path_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_joint_state_publisher_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_use_sim_time_cmd)
# Add any actions
ld.add_action(start_joint_state_publisher_cmd)
ld.add_action(start_joint_state_publisher_gui_node)
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(start_rviz_cmd)
return ld
RViz를 이용해 모델을 확인할 것이다. 그러므로, RViz 설정 파일을 다음과 같이 만든다.
cd ~/projects/ros2/src/basic_mobile_robot/rviz
subl urdf_config.rviz
설정 파일 내용은 여기(https://drive.google.com/file/d/1YTjMqv9q12kP_JCPLgp03-WqjtJ6vyi7/view)에서 다운로드 한다. 상세 설정은 다음과 같다.
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1/Links1
- /TF1
Splitter Ratio: 0.5
Tree Height: 557
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Name: Grid
- Alpha: 0.6
Class: rviz_default_plugins/RobotModel
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Name: RobotModel
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Name: TF
Marker Scale: 0.3
Show Arrows: true
Show Axes: true
Show Names: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
- Class: nav2_rviz_plugins/GoalTool
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Name: Current View
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Saved: ~
빌드를 위한 cmake 파일을 설정한다.
cd ~/projects/ros2/src/basic_mobile_robot
subl CMakeLists.txt
다음 설정에서 적색 표시 부분을 추가한다.
cmake_minimum_required(VERSION 3.8)
project(basic_mobile_robot)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
install(
DIRECTORY config launch maps meshes models params rviz src worlds
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
빌드 및 실행
다음과 같이 빌드한다.
cd ~/projects/ros2
colcon build
다음과 같이 src 밑의 특정 패키지만 colcon 빌드할 수도 있다.
colcon build --packages-select basic_mobile_robot
colcon build --symlink-install --packages-select basic_mobile_robot
다음과 같이 빌드 패키지 경로를 설정한다.
. install/setup.bash
이제 새 터미널을 실행하고, 이 패키지를 실행해 본다.
ros2 launch -s basic_mobile_robot basic_mobile_bot_v1.launch.py
rviz2를 실행한다.
rviz2 -d ./src/basic_mobile_robot/rviz/urdf_config.rviz
다음과 같이 개발된 로버 모델이 보여진다.
모델 좌표계를 확인하기 위해, 다음 명령을 실행한다.
sudo apt install ros-humble-tf2-tools
ros2 run tf2_tools view_frames.py
evince frames.pdf
다음과 같이 각 링크와 조인트 좌표를 확인해 본다.
ros2 run tf2_ros tf2_echo base_link front_caster
레퍼런스
- Set Up LIDAR for a Simulated Mobile Robot in ROS 2 (code)
- Build the Warehouse Robot
- Create Model
- ROS2 rover
- ROS2 package development with python
- Writing a simple publisher and subscriber (Python)
- URDF tutorial github and explain
- How to Create URDF and Launch Files in ROS2 and Display Them in Rviz
- RVIZ2 Tutorials Episode Learn TF
댓글 없음:
댓글 쓰기