URDF, XACRO 마스터 하기 ! (ADDBOT Rviz에서 구현하기) (2/2)

이 글은 pinkLab의 pinkwink강사님의 강의자료를 참고하여 작성되었습니다.

https://pinkwink.kr/

PinkWink

---

 

일단 이전글까지 끝마친 상태라면 이상태일 것입니다.

<?xml version="1.0"?>
<robot name="ADDbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:property name="wheel_diameter" value="0.068"/>
    <xacro:property name="wheel_speration" value="0.180"/>
    <!-- 가장 상위의 footprint  -->
    <link name="base_footprint"/>
    <!-- base_link  -->
    <link name="base_link">
        <visual>
        <!-- 원점의 위치로 정의합니다.  -->
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <!-- meshes에서 visual 파일을 불러온다. -->
                <mesh filename="package://Addbot_description/meshes/visual/base_link.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <!-- meshes에서 collision 파일을 불러온다. -->
                <mesh filename="package://Addbot_description/meshes/collision/base_link.stl" scale="1 1 1"/>
            </geometry>
        </collision>
        <inertial>
            <!-- 관성에 대한 표시 위치는 임의로 지정한 것인가? 실행을 해봐야 알 것 같다. -->
            <origin xyz="-0.02 0 0.028" rpy="0 0 0"/>
            <mass value="2.0"/>
            <!--<inertia ixx="0.000213" ixy="0" ixz="0" iyy="0.007034" iyz="0" izz="0.008104"/> -->
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/> 
        </inertial>
    </link>

    <joint name="base_link_fixed_joint" type="fixed">
            <parent link="base_footprint"/>
            <child link="base_link"/>
            <origin xyz="-0.3 0 ${wheel_diameter/2}" rpy="0 0 0"/>
            <!-- 수정된 부분 -->
    </joint>


</robot>

앞으로 작성되는 모든 코드는 이 코드에서 추가되는 부분만 다룹니다.

 

이제 이 로봇에 바퀴도 달아보고 카메라도 달아보고

나머지 부속품들을 다 달아봅시다.

 

먼저 바퀴를 달아봅시다.

그 전에 이 코드에서 type부분을 정리하고 가도록 하겠습니다.

 <joint name="base_link_fixed_joint" type="fixed">
            <parent link="base_footprint"/>
            <child link="base_link"/>
            <origin xyz="0 0 ${wheel_diameter/2}" rpy="0 0 0"/>
    </joint>

 

휠 : continuous

고정된 경우 : fixed

회전 : revolute

직선 : prismatic

 

우선 왼쪽 바퀴를 다는 코드를 살펴보자.

<!-- 왼쪽 바퀴를 달자.  -->
    <link name="l_wheel">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 ${1 * pi/2}"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/visual/wheel.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 ${1 * pi/2}"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/collision/wheel.stl" scale="1 1 1"/>
            </geometry>
        </collision>
            <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.200"/>
                <!-- 가제보에 에러라고 해야하나. 시뮬레이션에서 로봇이 쓰러지는 경우가 있어서 바퀴를 좀 무겁게 세팅할 수 있다.-->
                <!-- <inertia ixx="0.000067" ixy="0" ixz="0" iyy="0.000113" iyz="0" izz="0.000067"/> -->
                <!-- For stable on GAZEBO. Force Inertial value to LARGE value -->
                <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
            </inertial>
        </link>

        <!-- 왼쪽 바퀴의 관절  -->
        <joint name="l_wheel_joint" type="continuous">
            <parent link="base_link"/>
            <child link="l_wheel"/>
            <axis xyz="0 1 0"/>
            <!-- 왼쪽 바퀴 관절의 위치  -->
            <origin xyz="0 ${1 * wheel_speration/2} 0" rpy="0 0 0"/>
        </joint>

뭐 요런식으로 생겼다.

이상태로 한번 빌드를 해보자.

 

 

왼쪽에 바퀴를 달았다.

TF를 제거하고 다시 살펴보자.

 

결국 무언가를 하나 추가할때는

링크만 추가하면 오류가 나는 것 같다. 관절도 같이 넣어줘야한다.

 

이제 오른쪽 바퀴를 달아야한다.

여기서 xacro의 함수 사용을 보자.

오른쪽 바퀴를 달때는 그냥 지금 코드에서 값만 변경하면 된다.

변경될 부분을 생각해보자. 우선 이름이 r_wheel로 변경된다.

그러면 l이냐 r이냐가 필요하다. (변수)

그리고 rpy의 값도 바뀌어야한다.  (변수)

(rpy는  Roll , Pitch, Yaw라는 놈으로 질문을 해야겠다.) 

 

그러면 먼저 함수를 선언하자.

<xacro:macro name="insert_wheel" params="docking left_right dir">

insert_wheel이라는 함수와 매개변수 docking , left_right, dir이다.

 

    <!-- 바퀴 함수를 만들자.  -->
    <xacro:macro name="insert_wheel" params="docking left_right dir">
        <link name="${left_right}_wheel">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 ${dir * pi/2}"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/visual/wheel.dae" scale="1 1 1"/>
                </geometry>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 ${dir * pi/2}"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/collision/wheel.stl" scale="1 1 1"/>
                </geometry>
            </collision>
            <inertial>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <mass value="0.200"/>
                    <!-- 가제보에 에러라고 해야하나. 시뮬레이션에서 로봇이 쓰러지는 경우가 있어서 바퀴를 좀 무겁게 세팅할 수 있다.-->
                    <!-- <inertia ixx="0.000067" ixy="0" ixz="0" iyy="0.000113" iyz="0" izz="0.000067"/> -->
                    <!-- For stable on GAZEBO. Force Inertial value to LARGE value -->
                    <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
            </inertial>
        </link>

        <!-- 왼쪽 바퀴의 관절  -->
        <joint name="${left_right}_wheel_joint" type="continuous">
            <parent link="${docking}"/>
            <child link="${left_right}_wheel"/>
            <axis xyz="0 1 0"/>
            <!-- 왼쪽 바퀴 관절의 위치  -->
            <origin xyz="0 ${dir * wheel_speration/2} 0" rpy="0 0 0"/>
        </joint>
    </xacro:macro>

이렇게 생긴 함수로 만들어진다.

그러면 함수를 두번 실행하면 바퀴가 달린다.

<xacro:insert_wheel docking="base_link"  left_right="l" dir="1" />
<xacro:insert_wheel docking="base_link"  left_right="r" dir="-1" />

 

build 하고 보자.

 

이쁘게 달렸다.

굿

 

 

 

다음은 카메라를 달아보자.

카메라를 달 수 있는 공간이 있고 그곳에 카메라가 달린다.

우선 카메라가 달리는 곳(마운트)를 달아보자.

    <!-- 카메라 붙이는 곳을 만들자(마운트).  -->
    <!-- 이전과 크게 다를게 없어서 언급은 안해도 될 듯하다.  -->
    <link name="front_camera_mount">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/visual/front_camera.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/collision/front_camera.stl" scale="1 1 1"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.005 0 0.013" rpy="0 0 0"/>
            <mass value="0.050000"/>
            <inertia ixx="0.000005" ixy="0" ixz="-0.000001" iyy="0.000004" iyz="0" izz="0.000003"/>
        </inertial>
    </link>

    <joint name="front_camera_mount_fixed_joint" type="fixed">
        <parent link="base_link"/>
        <child link="front_camera_mount"/>
        <origin xyz="0.045 0 0.085" rpy="0 0 0"/>
    </joint>

빌드하고 실행해보자.

 

카메라가 달리는 곳이라더니 카메라가 달린거 같은데.. 아마 그럼 이게 카메라고

 

카메라 링크가 따로 있나봅니다.

    <!-- 카메라 링크를 붙이자..  -->
    <!-- 이전과 크게 다를게 없어서 언급은 안해도 될 듯하다.  -->
    <link name="camera_link"/>

    <!-- 카메라 링크와 카메라 마운트의 joint를 붙이자..  -->
    <joint name="camera_link_fixed_joint" type="fixed">
        <parent link="front_camera_mount"/>
        <child link="camera_link"/>
        <origin xyz="0.0111 0 0.0193" rpy="0 ${pi/2} 0"/>
    </joint>

 

따로 그림이 들어가는건 없네요.

그래서 그림은 따로 안그리고 

 

 

다음은 센서를 그려넣어봅시다.

 

    <!-- 센서를 붙이자..  -->
    <link name="ul_sensor_mount">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/visual/ul_sensor.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/collision/ul_sensor.stl" scale="1 1 1"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.005 0 0.013" rpy="0 0 0"/>
            <mass value="0.050000"/>
            <inertia ixx="0.000005" ixy="0" ixz="-0.000001" iyy="0.000004" iyz="0" izz="0.000003"/>
        </inertial>
    </link>

    <joint name="ul_sensor_mount_fixed_joint" type="fixed">
        <parent link="base_link"/>
        <child link="ul_sensor_mount"/>
        <origin xyz="0.03 0 0.01" rpy="0 0 0"/>
    </joint>

    <link name="ul_sensor_link"/>

    <joint name="ul_sensor_link_fixed_joint" type="fixed">
        <parent link="ul_sensor_mount"/>
        <child link="ul_sensor_link"/>
        <origin xyz="0.02 0 0" rpy="0 0 0"/>
    </joint>

 

이상태로 빌드를 하고 한 번 결과를 봅시다.

앞에 두 센서가 붙었습니다.

 

 

이제 라이다를 붙일려고 합니다.

근데, 라이다를 붙이기 전에 

상황을 하나 가정을 하고 들어갑시다.

만약 라이다가 자주 변경되는 환경이라면?

 

지금까지 만든 파일을 하나의 함수파일로 만들어버리고 새로운 파일에서 라이다만 부르면 됩니다.

이 과정을 한 번 진행을 해봅시다.

 

지금까지 만든 파일말고 하나를 추가로 만듭시다.

base.urdf.xacro

그리고 이 파일에 지금까지 만든 코드를 함수화 시켜 집어넣습니다.

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

        <xacro:property name="wheel_diameter" value="0.068"/>
        <xacro:property name="wheel_speration" value="0.180"/>

        <link name="${prefix}base_footprint"/>

        <link name="${prefix}base_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/visual/base_link.dae" scale="1 1 1"/>
                </geometry>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/collision/base_link.stl" scale="1 1 1"/>
                </geometry>
            </collision>
            <inertial>
                <origin xyz="-0.02 0 0.028" rpy="0 0 0"/>
                <mass value="2.0"/>
                <!-- <inertia ixx="0.000213" ixy="0" ixz="0" iyy="0.007034" iyz="0" izz="0.008104"/> -->
                <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
            </inertial>
        </link>

        <joint name="${prefix}base_link_fixed_joint" type="fixed">
            <parent link="${prefix}base_footprint"/>
            <child link="${prefix}base_link"/>
            <origin xyz="0 0 ${wheel_diameter/2}" rpy="0 0 0"/>
        </joint>

        <xacro:macro name="insert_wheel" params="parent prefix prefix_dir dir">
            <link name="${prefix}${prefix_dir}_wheel">
                <visual>
                    <origin xyz="0 0 0" rpy="0 0 ${dir * pi/2}"/>
                    <geometry>
                        <mesh filename="package://Addbot_description/meshes/visual/wheel.dae" scale="1 1 1"/>
                    </geometry>
                </visual>
                <collision>
                    <origin xyz="0 0 0" rpy="0 0 ${dir * pi/2}"/>
                    <geometry>
                        <mesh filename="package://Addbot_description/meshes/collision/wheel.stl" scale="1 1 1"/>
                    </geometry>
                </collision>
                <inertial>
                    <origin xyz="0 0 0" rpy="0 0 0"/>
                    <mass value="0.200"/>
                    <!-- <inertia ixx="0.000067" ixy="0" ixz="0" iyy="0.000113" iyz="0" izz="0.000067"/> -->
                    <!-- For stable on GAZEBO. Force Inertial value to LARGE value -->
                    <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
                </inertial>
            </link>

            <joint name="${prefix}${prefix_dir}_wheel_joint" type="continuous">
                <parent link="${parent}"/>
                <child link="${prefix}${prefix_dir}_wheel"/>
                <axis xyz="0 1 0"/>
                <origin xyz="0 ${dir * wheel_speration/2} 0" rpy="0 0 0"/>
            </joint>
        </xacro:macro>

        <xacro:insert_wheel parent="${prefix}base_link" prefix="${prefix}" prefix_dir="l" dir="1" />
        <xacro:insert_wheel parent="${prefix}base_link" prefix="${prefix}" prefix_dir="r" dir="-1" />

        <link name="${prefix}front_camera_mount">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/visual/front_camera.dae" scale="1 1 1"/>
                </geometry>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/collision/front_camera.stl" scale="1 1 1"/>
                </geometry>
            </collision>
            <inertial>
                <origin xyz="0.005 0 0.013" rpy="0 0 0"/>
                <mass value="0.050000"/>
                <inertia ixx="0.000005" ixy="0" ixz="-0.000001" iyy="0.000004" iyz="0" izz="0.000003"/>
            </inertial>
        </link>

        <joint name="${prefix}front_camera_mount_fixed_joint" type="fixed">
            <parent link="${prefix}base_link"/>
            <child link="${prefix}front_camera_mount"/>
            <origin xyz="0.045 0 0.085" rpy="0 0 0"/>
        </joint>

        <link name="${prefix}camera_link"/>

        <joint name="${prefix}camera_link_fixed_joint" type="fixed">
            <parent link="${prefix}front_camera_mount"/>
            <child link="${prefix}camera_link"/>
            <origin xyz="0.0111 0 0.0193" rpy="0 ${pi/2} 0"/>
        </joint>

        <link name="${prefix}ul_sensor_mount">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/visual/ul_sensor.dae" scale="1 1 1"/>
                </geometry>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <mesh filename="package://Addbot_description/meshes/collision/ul_sensor.stl" scale="1 1 1"/>
                </geometry>
            </collision>
            <inertial>
                <origin xyz="0.005 0 0.013" rpy="0 0 0"/>
                <mass value="0.050000"/>
                <inertia ixx="0.000005" ixy="0" ixz="-0.000001" iyy="0.000004" iyz="0" izz="0.000003"/>
            </inertial>
        </link>

        <joint name="${prefix}ul_sensor_mount_fixed_joint" type="fixed">
            <parent link="${prefix}base_link"/>
            <child link="${prefix}ul_sensor_mount"/>
            <origin xyz="0.03 0 0.01" rpy="0 0 0"/>
        </joint>

        <link name="${prefix}ul_sensor_link"/>

        <joint name="${prefix}ul_sensor_link_fixed_joint" type="fixed">
            <parent link="${prefix}ul_sensor_mount"/>
            <child link="${prefix}ul_sensor_link"/>
            <origin xyz="0.02 0 0" rpy="0 0 0"/>
        </joint>

    </xacro:macro>
</robot>

 

자.. 이상황에서 

파일을 하나 다시 만듭니다.

robot.urdf.xacro

 

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

    <xacro:include filename="$(find Addbot_description)/urdf/common/inertia_macro.urdf.xacro"/>
    <xacro:include filename="$(find Addbot_description)/urdf/base.urdf.xacro"/>

 

이 상태라면 방금 함수를 include 시킨 상태입니다.

하지만 아직 사용하는 상태는 아닙니다.

<xacro:arg name="prefix" default=""/>
<xacro:property name="prefix" value="$(arg prefix)"/>
<xacro:insert_robot prefix="$(arg prefix)"/>

이제 본격적으로 사용하는 상태가 됩니다.

<!-- 라이다를 넣자. -->
    <link name="${prefix}ydlidar_lidar_mount">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/visual/ydlidar_x2_assy.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://Addbot_description/meshes/collision/ydlidar_x2_assy.stl" scale="1 1 1"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.007 0 0.025" rpy="0 0 0"/>
            <mass value="0.150"/>
            <inertia ixx="0.000058" ixy="0" ixz="0.000005" iyy="0.000099" iyz="0" izz="0.000103"/>
        </inertial>
    </link>

    <joint name="${prefix}idar_mount_fixed_joint" type="fixed">
        <parent link="${prefix}base_link"/>
        <child link="${prefix}ydlidar_lidar_mount"/>
        <origin xyz="-0.01 0 0.085" rpy="0 0 0"/>
    </joint>

    <link name="${prefix}laser_link"/>

    <joint name="${prefix}laser_link_fixed_joint" type="fixed">
        <parent link="${prefix}ydlidar_lidar_mount"/>
        <child link="${prefix}laser_link"/>
        <origin xyz="0 0 0.04245" rpy="0 0 0"/>
    </joint>
</robot>

이제 라이다를 구성하는 코드를 넣으면 끝이납니다.

 

그.. 뭐지 이제 실행하는 파일은 robot.urdf.xacro 임으로

view_robot.launch.py 에서 실행할 urdf파일을 변경해주어야합니다.

여기서 ADDBOT.urdf.xacro 파일을

다음으로 수정합니다.

 

그럼 이제 빌드하고 다시 실행해봅시다.

 

 

라이다 까지 올라간 모습을 보여줍니다.

 

 

TF도 이쁘게 나옵니다.

 

다음은 이걸 가제보상으로 올리는걸 한 번 알아보도록 하겠습니다.

그건 다음에 계속