ROS2のrobot_descriptionパッケージを作成する手順

ROS2のrobot_descriptionパッケージを作成する手順をまとめます。

パッケージの作成

まずは、ワークスペースを作成します。
今回はmy_robot_wsとします。

mkdir -p ~/my_robot_ws/src
cd ~/my_robot_ws/src

次に、パッケージを作成します。

ros2 pkg create --build-type ament_cmake my_robot_description

URDFファイルの追加

urdfディレクトリを作成し、URDFファイルを追加します。
今後の拡張性を考えて、URDFファイルはxacro形式で作成します。
my_robot.urdf.xacroというファイル名で作成します。

mkdir -p my_robot_description/urdf
touch my_robot_description/urdf/my_robot.urdf.xacro

<?xml version="1.0"?>
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- Constants -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  
  <!-- Link Properties -->
  <xacro:property name="link_radius" value="0.02"/>
  <xacro:property name="link_length" value="0.2"/>
  <xacro:property name="link_mass" value="1.0"/>
  <xacro:property name="link_ixx" value="0.01"/>
  <xacro:property name="joint_limit" value="${PI/2}"/>
  <xacro:property name="joint_velocity" value="1.0"/>
  <xacro:property name="joint_effort" value="10"/>

  <!-- Joint Offsets -->
  <xacro:property name="joint_offset_xy" value="0.042"/>
  <xacro:property name="base_z_offset" value="0.05"/>
  <xacro:property name="link_z_offset" value="0.2"/>
  <xacro:property name="link_z_offset_ext" value="0.22"/>
  
  <!-- Colors -->
  <xacro:property name="color_blue" value="0.0 0.0 1.0 1.0"/>
  <xacro:property name="color_red" value="1.0 0.0 0.0 1.0"/>
  <xacro:property name="color_green" value="0.0 1.0 0.0 1.0"/>
  <xacro:property name="color_gray" value="0.7 0.7 0.7 1.0"/>

  <!-- Link Macro -->
  <xacro:macro name="cylinder_link" params="name color">
    <link name="${name}">
      <visual>
        <origin xyz="0 0 0.1" rpy="0 0 0"/>
        <geometry>
          <cylinder length="${link_length}" radius="${link_radius}"/>
        </geometry>
        <material name="${color}">
          <color rgba="${color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0.1" rpy="0 0 0"/>
        <geometry>
          <cylinder length="${link_length}" radius="${link_radius}"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="${link_mass}"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <inertia ixx="${link_ixx}" ixy="0.0" ixz="0.0"
                 iyy="${link_ixx}" iyz="0.0"
                 izz="${link_ixx}"/>
      </inertial>
    </link>
  </xacro:macro>

  <!-- Joint Macro -->
  <xacro:macro name="revolute_joint" params="name parent child xyz axis">
    <joint name="${name}" type="revolute">
      <origin xyz="${xyz}" rpy="0 0 0"/>
      <parent link="${parent}"/>
      <child link="${child}"/>
      <axis xyz="${axis}"/>
      <limit lower="-${joint_limit}" upper="${joint_limit}" 
             effort="${joint_effort}" velocity="${joint_velocity}"/>
    </joint>
  </xacro:macro>

  
  <!-- Base World Joint -->
  <link name="world"/>
  <joint name="world_to_base_link" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.2 0.2 0.1"/>
      </geometry>
      <material name="gray">
        <color rgba="${color_gray}"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.2 0.2 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.0"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="0.02" ixy="0.0" ixz="0.0"
               iyy="0.02" iyz="0.0"
               izz="0.02"/>
    </inertial>
  </link>

  <!-- Links -->
  <xacro:cylinder_link name="link1" color="${color_blue}"/>
  <xacro:cylinder_link name="link2" color="${color_red}"/>
  <xacro:cylinder_link name="link3" color="${color_green}"/>
  <xacro:cylinder_link name="link4" color="${color_blue}"/>
  <xacro:cylinder_link name="link5" color="${color_red}"/>
  <xacro:cylinder_link name="link6" color="${color_green}"/>
  <xacro:cylinder_link name="link7" color="${color_blue}"/>

  <!-- Joints -->
  <xacro:revolute_joint name="joint1" parent="base_link" child="link1" 
                        xyz="0 0 ${base_z_offset}" axis="0 0 1"/>
  <xacro:revolute_joint name="joint2" parent="link1" child="link2" 
                        xyz="0 ${joint_offset_xy} ${link_z_offset}" axis="0 1 0"/>
  <xacro:revolute_joint name="joint3" parent="link2" child="link3" 
                        xyz="${joint_offset_xy} 0 ${link_z_offset}" axis="1 0 0"/>
  <xacro:revolute_joint name="joint4" parent="link3" child="link4" 
                        xyz="0 0 ${link_z_offset_ext}" axis="0 0 1"/>
  <xacro:revolute_joint name="joint5" parent="link4" child="link5" 
                        xyz="0 -${joint_offset_xy} ${link_z_offset}" axis="0 1 0"/>
  <xacro:revolute_joint name="joint6" parent="link5" child="link6" 
                        xyz="-${joint_offset_xy} 0 ${link_z_offset}" axis="1 0 0"/>
  <xacro:revolute_joint name="joint7" parent="link6" child="link7" 
                        xyz="0 0 ${link_z_offset_ext}" axis="0 0 1"/>
</robot>

CMakeLists.txtの編集

作成したURDFファイルをインストールするようにCMakeLists.txtを編集します。

cmake_minimum_required(VERSION 3.8)
project(my_robot_description)

find_package(ament_cmake REQUIRED)

install(DIRECTORY urdf
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

ビルドと実行

ビルドします。

cd ~/my_robot_ws
colcon build

installディレクトリにURDFファイルがインストールされていることを確認します。

ls install/my_robot_description/share/my_robot_description/urdf

以上で、robot_descriptionパッケージの作成が完了しました。