ROS2 nav2(3)

nav2를 custom robot으로 사용해보겠다.

https://docs.nav2.org/setup_guides/urdf/setup_urdf.html

Setting Up The URDF — Nav2 1.0.0 documentation

 

초기 작업은 저번에 만들었던 로봇과 비슷하지만 다시 해보겠다.

먼저 docker 환경(22.04 + ros2 humble)이고 추가 패키지를 설치 해주었다.

sudo apt install ros-humble-joint-state-publisher-gui
sudo apt install ros-humble-xacro

 

다음으로 프로젝트에 대한 디렉토리를 만들고, ros2 작업공간에 패키지를 생성해주었다.

ros2 pkg create --build-type ament_cmake sam_bot_description

 

src/description 아래에 sam_bot_description.urdf 이름의 파일을 만들어주고 아래의 내용을 추가한다.

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



</robot>

 

다음으로 URDF전체에서 재사용될 xacro 속성을 사용해서 일부 상수를 정의하였다.

  <!-- Define robot constants -->
  <xacro:property name="base_width" value="0.31"/>
  <xacro:property name="base_length" value="0.42"/>
  <xacro:property name="base_height" value="0.18"/>

  <xacro:property name="wheel_radius" value="0.10"/>
  <xacro:property name="wheel_width" value="0.04"/>
  <xacro:property name="wheel_ygap" value="0.025"/>
  <xacro:property name="wheel_zoff" value="0.05"/>
  <xacro:property name="wheel_xoff" value="0.12"/>

  <xacro:property name="caster_xoff" value="0.14"/>

base_*는 모두 로봇의 주 섀시 크기를 정의한다.

wheel_radius, wheel_width는 로봇의 두 뒷바퀴 모양을 정의한다.

wheel_ygap는 y축을 따라 바퀴와 섀시 사이의 간격을 조정하고

wheel_zoff, wheel_xoff는 뒷바퀴를 z축과 x축을 따라 적절하게 배치한다.

마지막으로 caster_xoff는 x축을 따라 앞쪽 캐스터 휠을 배치한다.

 

  <!-- Robot Base -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
  </link>

로봇의 base이다.

 

  <!-- Robot Footprint -->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
  </joint>

로봇의 footprint이다.

차원이나 충돌 영역이 없는 가상(비물리적)링크이다. 주요 목적은 다양한 패키지가 지면에 투사된 로봇의 중심을 결정할 수 있도록 하는 것이다.

예를 들어, navigation2는 이 링크를 사용하여 장애물 회피 알고리즘에 사용되는 원형 발자국의 중심을 결정한다.

 

  <!-- Wheels -->
  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
    <link name="${prefix}_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="Gray">
          <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
      </visual>
    </link>

    <joint name="${prefix}_joint" type="continuous">
      <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"/>
      <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" />

로봇의 구동 휠 부분이다. 코드를 깔끔하게 만들고 반복을 피하기 위해 매크로를 사용하였다.

매크로에는 세 가지 매개변수가 있다. 조인트 이름에 접두사를 추가하는 매개변수 prefix링크와 각각 x축과 y축에 대해 휠의 위치를 뒤집을 수 있는 매개 변수 x_reflect, y_reflect이다.

또 휠이 축을 중심으로 자유롭게 회전할 수 있도록 조인트도 정의한다(continuous)

 

  <!-- Caster Wheel -->
  <link name="front_caster">
    <visual>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
  </link>

  <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>

마지막으로 로봇 앞쪽에 캐스터 휠을 추가한다. 이 휠은 간단하게 구로 모델링했고 fixed 상태로 유지한다.

 

종속성을 추가해보겠다.package.xml을 열고 <buildtool_depend>뒤에 아래 내용을 추가한다.

<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>xacro</exec_depend>

 

다음으로, 실행 파일을 만들어 보겠다. 먼저 패키지 내에 launch 폴더를 만들고 display.launch.py를 생성한 뒤 아래 내용을 작성해준다.

import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='sam_bot_description').find('sam_bot_description')
    default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')

    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
    )
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', default_model_path])}],
        condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
    )
    joint_state_publisher_gui_node = launch_ros.actions.Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
    )
    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
                                            description='Flag to enable joint_state_publisher_gui'),
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                            description='Absolute path to robot urdf file'),
        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
                                            description='Absolute path to rviz config file'),
        joint_state_publisher_node,
        joint_state_publisher_gui_node,
        robot_state_publisher_node,
        rviz_node
    ])

 

rviz를 실행시켰을때 자동으로 rviz를 구성해주는 rviz 구성파일도 미리 생성해준다.

rviz폴더 생성 후 urdf_config_rviz 내부에 아래 내용을 작성한다.

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
  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: ~

 

마지막으로 CMakeLists.txt파일을 수정해준다. (가급적이면 if(BUILD_TESTING)위에 추가)

install(
  DIRECTORY src launch rviz
  DESTINATION share/${PROJECT_NAME}
)

 

이제 실행시켜본다.

cd ~/nav2_ws
colcon build
source install/setup.bash
ros2 launch sam_bot_description display.launch.py

 

rviz와 joint_state_publisher_gui 창이 뜬것을 볼 수있다.

gui창에 있는 슬라이더를 움직여보면 로봇이 어떻게 움직일지 알 수 있다.

 

이제 물리적 속성을 추가해주겠다

먼저 관성 모멘트 관련 매크로를 정의하였다.

  <!-- Define inertial 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>

 

그리고 각 링크마다 collision을 추가해주었다.

아까 만들었던 urdf파일의 visual밑 부분에 아래 내용을 추가해주면 된다.

base_link

    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>

 

wheel

      <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="0.5" r="${wheel_radius}" h="${wheel_width}"/>

 

front_caster

    <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="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>

 

프로젝트를 다시 빌드한 다음 다시 실행시켜보면

똑같이 나오게 되는데 옆의 RobotModel창을 누른 뒤 visual Enabled와 collision Enabled를 눌러 보았을 때 모두 로봇이 화면에 나타나는 것을 볼 수 있다.

 

다음엔 오도메트리 설정을 해주겠다.