Daddy Makers

이 글은 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">
  
  
  <xacro:property name="base_width" value="0.39"/>
  <xacro:property name="base_length" value="0.70"/>
  <xacro:property name="base_height" value="0.20"/>
	
  
  <xacro:property name="wheel_radius" value="0.14"/>
  <xacro:property name="wheel_width" value="0.06"/>
  
  
  <xacro:property name="wheel_ygap" value="0.035"/>
  
  <xacro:property name="wheel_zoff" value="0.05"/>
  
  <xacro:property name="wheel_xoff" value="0.221"/>
  
  <xacro:property name="caster_xoff" value="0.217"/>
  
  <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>
  
  	
  <link name="base_footprint"/>
  
  <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>
  
  <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>
  
  <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>
    
    <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>
  
  <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
  <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
  
  
  <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>
  
  
   
  
  <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"/>
  
  <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>
패키지 실행 파일(launch)을 생성한다. 
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

ROS2 로봇 패키지 프로그램 개발 방법 정리

댓글