#プログラミング ROS< URDF: 独立差動二輪車の構築（設計~Gazebo導入まで） >

はじめに

ROS(Robot Operating System)をさらに扱えるようになることが目的である．その第47弾として，「URDF: 独立差動二輪車の構築（設計~Gazebo導入まで）」を扱う．

環境

Docker環境（VScode + Windows10）

Dockerfile

Dockerfile

FROM osrf/ros:noetic-desktop-full

WORKDIR /root/

ENV DISPLAY host.docker.internal:0.0

RUN apt-get update -y && apt-get upgrade -y
RUN apt-get install x11-apps -y
RUN echo "source /opt/ros/noetic/setup.sh" >> .bashrc
RUN mkdir -p catkin_ws/src
RUN cd catkin_ws/src && . /opt/ros/noetic/setup.sh && catkin_init_workspace
RUN cd && cd catkin_ws && . /opt/ros/noetic/setup.sh && catkin_make
RUN echo "source ./catkin_ws/devel/setup.bash" >> .bashrc

RUN apt-get update -y && apt-get upgrade -y && apt-get install git -y

devcontainer.json

devcontainer.json

// For format details, see https://aka.ms/devcontainer.json. For config options, see the README at:
// https://github.com/microsoft/vscode-dev-containers/tree/v0.224.3/containers/docker-existing-dockerfile
{
	"name": "Existing Dockerfile",

	// Sets the run context to one level up instead of the .devcontainer folder.
	"context": "..",

	// Update the 'dockerFile' property if you aren't using the standard 'Dockerfile' filename.
	"dockerFile": "../Dockerfile",

	// Set *default* container specific settings.json values on container create.
	"settings": {},
	
	// Add the IDs of extensions you want installed when the container is created.
	"extensions": [
		"ms-python.python",
		"ms-python.vscode-pylance",
		"ms-iot.vscode-ros"
	],

	"workspaceFolder": "/root/",

	"mounts": [
		"source=${localWorkspaceFolder}/share,target=/root/share,type=bind",
	],
	// // Use 'forwardPorts' to make a list of ports inside the container available locally.
	// // "appPort": ["11411:11411"],

	// // Uncomment the next line to run commands after the container is created - for example installing curl.
	// // "postCreateCommand": "apt-get update && apt-get install -y curl",

	// // Uncomment when using a ptrace-based debugger like C++, Go, and Rust
	"runArgs": ["--gpus=all"],

	// Uncomment to use the Docker CLI from inside the container. See https://aka.ms/vscode-remote/samples/docker-from-docker.
	// "mounts": ["source=../src/,target=/root/src,type=bind"],
	// "mounts": [ "source=../src,target=/root/src,type=bind" ],

	// Uncomment to connect as a non-root user if you've added one. See https://aka.ms/vscode-remote/containers/non-root.
	// "remoteUser": "vscode"
}

参考

コンピュータ


デバイス	MSI
プロセッサ	Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz
実装RAM	16.0 GB (15.9 GB 使用可能)
OS	Windows (Windows 10 Home, バージョン：22H2)

ROS


Distribution	noetic

URDFについて

URDFについては，@srsさんの記事に十分まとめられているため，以下の記事を紹介するだけにする．

前回は，URDFにおいて，自分の思いどおりにロボットモデルの構造を作ることができるように実践しながら，要点をおさえた．今回は，プリミティブ（初めから用意されている単純形状）を使って，LiDAR付の独立差動二輪車のモデルを作成する.

ロボットモデルの構築

STEP0: ロボットの設計

URDFを構築していくにあたって非常に重要になってくるものが，事前の設計である．
（パーツの構成，各パーツの寸法，各パーツ間の位置関係およびつなぎ方）

モデル（ver.1）：独立差動二輪車
最もシンプルな独立差動二輪車である．シミュレータ上で動かしてみると，慣性によってボディの揺れが目立つため，改善が必要である．
モデル（ver.2）：安定のためにキャスターを追加
モデル（ver.1）の課題であったボディの揺れを改善するために簡易なキャスターを設けた．これでロボットとしては安定して動くものができる．
モデル（ver.3）：LiDARを追加
最後にLiDARを設けた．これにより，環境情報を取得できていく．

今回は，モデル（ver.3）を作成していく．

STEP1: 必要なパーツの作成

ここでは，各パーツの「見た目」「衝突判定」「慣性モーメント」についての設定を記述する．

URDF

URDF

<!-- ロボットの名前 -->
<robot name="diff_drive_robot">
  <!-- ロボットの基準とするベースリンク -->
  <link name="base_link"/>

  <!-- ボディの作成 -->
  <link name="body">
    <visual>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.011875000000000002" iyy="0.011875000000000002" izz="0.020000000000000004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- タイヤの作成 -->
  <link name="wheel_left">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- タイヤの作成 -->
  <link name="wheel_right">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="cyan">
        <color rgba="0.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- キャスターの作成 -->
  <link name="caster_forward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="green">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  
  <!-- キャスターの作成 -->
  <link name="caster_backward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black">
        <color rgba="0.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidar台の作成 -->
  <link name="lidar_base">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="yellow">
        <color rgba="1.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.25" iyy="0.25" izz="0.25" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidarの作成 -->
  <link name="lidar_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="purple">
        <color rgba="1.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.125" iyy="0.125" izz="0.188" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

</robot>

STEP2: ジョイント設定の追加

ここでは，各パーツ間の関係を表現するジョイントの設定を記述する．

URDF

URDF

<!-- ロボットの名前 -->
<robot name="diff_drive_robot">
  <!-- ロボットの基準とするベースリンク -->
  <link name="base_link"/>

  <!-- ボディの作成 -->
  <link name="body">
    <visual>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.011875000000000002" iyy="0.011875000000000002" izz="0.020000000000000004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- タイヤの作成 -->
  <link name="wheel_left">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- タイヤの作成 -->
  <link name="wheel_right">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="cyan">
        <color rgba="0.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- キャスターの作成 -->
  <link name="caster_forward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="green">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  
  <!-- キャスターの作成 -->
  <link name="caster_backward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black">
        <color rgba="0.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidar台の作成 -->
  <link name="lidar_base">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="yellow">
        <color rgba="1.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.25" iyy="0.25" izz="0.25" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidarの作成 -->
  <link name="lidar_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="purple">
        <color rgba="1.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.125" iyy="0.125" izz="0.188" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
<!-- ################# ジョイント設定 ################## -->

	<joint name="base_joint" type="fixed">
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="body"/>
 	</joint>

	<joint name="wheel_left_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 -0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_left"/>
 	</joint>

	<joint name="wheel_right_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_right"/>
	</joint>

	<joint name="caster_forward_joint" type="fixed">
    <origin xyz="-0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_forward"/>
	</joint>

	<joint name="caster_backward_joint" type="fixed">
    <origin xyz="0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_backward"/>
	</joint>

	<joint name="lidar_base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.125" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_base"/>
	</joint>

	<joint name="lidar_joint" type="fixed">
    <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_link"/>
	</joint>

</robot>

追加部分

URDF（追加部分）

<!-- ################# ジョイント設定 ################## -->

	<joint name="base_joint" type="fixed">
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="body"/>
 	</joint>

	<joint name="wheel_left_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 -0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_left"/>
 	</joint>

	<joint name="wheel_right_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_right"/>
	</joint>

	<joint name="caster_forward_joint" type="fixed">
    <origin xyz="-0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_forward"/>
	</joint>

	<joint name="caster_backward_joint" type="fixed">
    <origin xyz="0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_backward"/>
	</joint>

	<joint name="lidar_base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.125" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_base"/>
	</joint>

	<joint name="lidar_joint" type="fixed">
    <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_link"/>
	</joint>

ここまでで，Gazeboへの反映が可能である．
ただし，URDFでの色情報とGazeboでの色情報は別物であるため，白色ですべてのパーツが表現される．

Gazeboへ反映させるlaunchファイル

<arg name="model" default="$(find diff_drive_pkg)/urdf/robot.urdf"/>
の部分でロードしたいurdfを指定している．

robot.launch（追加部分）

<launch>

    <!-- 引数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>
    <arg name="model" default="$(find diff_drive_pkg)/urdf/robot.urdf"/>

    <!-- Gazeboの起動 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!--ロボットのURDFモデルをパラメータサーバにロードする-->
    <param name = "robot_description" textfile = "$(arg model)"/>

    <!--Gazeboでロボットを生成し，パラメータサーバからのその記述を受ける-->
    <node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model diff_drive_robot"/>

    <!-- robot_state_publisherの起動 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
        <param name="publish_frequency" type="double" value="30.0" />
    </node>
</launch>

右には，VScodeの拡張機能であるURDF previewにより表示させたものである．
左には，Gazeboに読み込んだものを表示している．
色がついていないことがわかる．

STEP3: Gazebo上での色情報と摩擦を追加

ここでは，各パーツに対してGazebo上で反映できる摩擦係数（クーロン摩擦モデルに関わる）と色情報を記述している．
摩擦係数は0だとつるつるに滑り，無限大だと全く滑らない．
mu1とmu2は接触時における2方向のそれぞれの摩擦係数らしい．
どの方向にも同様な滑りを再現したい場合は，mu1もmu2も同じ値に設定しておくとよいらしい．

URDF

URDF

<!-- ロボットの名前 -->
<robot name="diff_drive_robot">
  <!-- ロボットの基準とするベースリンク -->
  <link name="base_link"/>

  <!-- ボディの作成 -->
  <link name="body">
    <visual>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.011875000000000002" iyy="0.011875000000000002" izz="0.020000000000000004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- タイヤの作成 -->
  <link name="wheel_left">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- タイヤの作成 -->
  <link name="wheel_right">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="cyan">
        <color rgba="0.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- キャスターの作成 -->
  <link name="caster_forward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="green">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  
  <!-- キャスターの作成 -->
  <link name="caster_backward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black">
        <color rgba="0.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidar台の作成 -->
  <link name="lidar_base">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="yellow">
        <color rgba="1.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.25" iyy="0.25" izz="0.25" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidarの作成 -->
  <link name="lidar_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="purple">
        <color rgba="1.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.125" iyy="0.125" izz="0.188" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
<!-- ################# ジョイント設定 ################## -->

	<joint name="base_joint" type="fixed">
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="body"/>
 	</joint>

	<joint name="wheel_left_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 -0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_left"/>
 	</joint>

	<joint name="wheel_right_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_right"/>
	</joint>

	<joint name="caster_forward_joint" type="fixed">
    <origin xyz="-0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_forward"/>
	</joint>

	<joint name="caster_backward_joint" type="fixed">
    <origin xyz="0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_backward"/>
	</joint>

	<joint name="lidar_base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.125" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_base"/>
	</joint>

	<joint name="lidar_joint" type="fixed">
    <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_link"/>
	</joint>


<!-- ################# Gazebo上の設定 ################## -->
	<!-- 各パーツの情報を設定 -->
	<gazebo reference="body">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Red</material>
	</gazebo>

	<gazebo reference="wheel_left">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="wheel_right">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="caster_forward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Green</material>
	</gazebo>

	<gazebo reference="caster_backward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Black</material>
	</gazebo>
  
	<gazebo reference="lidar_base">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Yellow</material>
	</gazebo>

	<gazebo reference="lidar_link">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Purple</material>
	</gazebo>

</robot>

追加部分

URDF（追加部分）

<!-- ################# Gazebo上の設定 ################## -->
	<!-- 各パーツの情報を設定 -->
	<gazebo reference="body">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Red</material>
	</gazebo>

	<gazebo reference="wheel_left">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="wheel_right">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="caster_forward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Green</material>
	</gazebo>

	<gazebo reference="caster_backward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Black</material>
	</gazebo>
  
	<gazebo reference="lidar_base">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Yellow</material>
	</gazebo>

	<gazebo reference="lidar_link">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Purple</material>
	</gazebo>

いい感じに色付けができた．URDF previewで左右の車輪の色を変えているのは，作成しているときに左右前後が視覚的にわかりやすくするためである．

STEP4: 差動駆動プラグインの追加

ここでは，差動駆動させるための設定をおこなう．

実際のロボットでは，ハードウェアドライバが/cmd_velや/odomインタフェースを実装することになるが，シミュレーションでも同じようなことをする必要がある．これには，Gazeboのプラグインをロードするだけという単純作業で済む．特に差動駆動プラグインをロードすれば，cmd_velメッセージを介してロボットを制御することができる．

URDF

URDF

<!-- ロボットの名前 -->
<robot name="diff_drive_robot">
  <!-- ロボットの基準とするベースリンク -->
  <link name="base_link"/>

  <!-- ボディの作成 -->
  <link name="body">
    <visual>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.011875000000000002" iyy="0.011875000000000002" izz="0.020000000000000004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- タイヤの作成 -->
  <link name="wheel_left">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- タイヤの作成 -->
  <link name="wheel_right">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="cyan">
        <color rgba="0.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- キャスターの作成 -->
  <link name="caster_forward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="green">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  
  <!-- キャスターの作成 -->
  <link name="caster_backward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black">
        <color rgba="0.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidar台の作成 -->
  <link name="lidar_base">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="yellow">
        <color rgba="1.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.25" iyy="0.25" izz="0.25" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidarの作成 -->
  <link name="lidar_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="purple">
        <color rgba="1.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.125" iyy="0.125" izz="0.188" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
<!-- ################# ジョイント設定 ################## -->

	<joint name="base_joint" type="fixed">
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="body"/>
 	</joint>

	<joint name="wheel_left_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 -0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_left"/>
 	</joint>

	<joint name="wheel_right_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_right"/>
	</joint>

	<joint name="caster_forward_joint" type="fixed">
    <origin xyz="-0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_forward"/>
	</joint>

	<joint name="caster_backward_joint" type="fixed">
    <origin xyz="0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_backward"/>
	</joint>

	<joint name="lidar_base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.125" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_base"/>
	</joint>

	<joint name="lidar_joint" type="fixed">
    <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_link"/>
	</joint>


<!-- ################# Gazebo上の設定 ################## -->
	<!-- 各パーツの情報を設定 -->
	<gazebo reference="body">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Red</material>
	</gazebo>

	<gazebo reference="wheel_left">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="wheel_right">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="caster_forward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Green</material>
	</gazebo>

	<gazebo reference="caster_backward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Black</material>
	</gazebo>
  
	<gazebo reference="lidar_base">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Yellow</material>
	</gazebo>

	<gazebo reference="lidar_link">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Purple</material>
	</gazebo>

<!--差動駆動プラグインをロードするための追加要素-->
  <gazebo>
    <!--差動駆動プラグインを指定-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <!--left_wheel_jointとright_wheel_jointをそれぞれ左関節右関節に指定-->
      <leftJoint>wheel_left_joint</leftJoint>
      <rightJoint>wheel_right_joint</rightJoint>
      <!--台座であるbase_linkをrobotBaseFrameに指定-->
      <robotBaseFrame>base_link</robotBaseFrame>
      <!--トレッドを0.45m，車輪直径0.30mとする-->
      <wheelSeparation>0.45</wheelSeparation>
      <wheelDiameter>0.30</wheelDiameter>
      <!--車輪の状態を配信する設定にする-->
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </gazebo>

</robot>

追加部分

URDF（追加部分）

<!--差動駆動プラグインをロードするための追加要素-->
  <gazebo>
    <!--差動駆動プラグインを指定-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <!--left_wheel_jointとright_wheel_jointをそれぞれ左関節右関節に指定-->
      <leftJoint>wheel_left_joint</leftJoint>
      <rightJoint>wheel_right_joint</rightJoint>
      <!--台座であるbase_linkをrobotBaseFrameに指定-->
      <robotBaseFrame>base_link</robotBaseFrame>
      <!--トレッドを0.45m，車輪直径0.30mとする-->
      <wheelSeparation>0.45</wheelSeparation>
      <wheelDiameter>0.30</wheelDiameter>
      <!--車輪の状態を配信する設定にする-->
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </gazebo>

STEP5: LiDARの追加

ここでは，センサを追加するための設定を記述する．
今回はセンサとしてLiDARを模擬する．LiDARを模擬するにあたって，
Gazeboのプラグインを利用する．

URDF

URDF

<!-- ロボットの名前 -->
<robot name="diff_drive_robot">
  <!-- ロボットの基準とするベースリンク -->
  <link name="base_link"/>

  <!-- ボディの作成 -->
  <link name="body">
    <visual>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.2" length="0.15"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.011875000000000002" iyy="0.011875000000000002" izz="0.020000000000000004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- タイヤの作成 -->
  <link name="wheel_left">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>]
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- タイヤの作成 -->
  <link name="wheel_right">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="cyan">
        <color rgba="0.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.15" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.002916666666666667" iyy="0.002916666666666667" izz="0.005625" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>


  <!-- キャスターの作成 -->
  <link name="caster_forward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="green">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  
  <!-- キャスターの作成 -->
  <link name="caster_backward">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black">
        <color rgba="0.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.5"/>
        <inertia ixx="3.000e-4" iyy="3.000e-4" izz="3.000e-4" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidar台の作成 -->
  <link name="lidar_base">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="yellow">
        <color rgba="1.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.25" iyy="0.25" izz="0.25" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- Lidarの作成 -->
  <link name="lidar_link">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="purple">
        <color rgba="1.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <cylinder radius="0.05" length="0.05"/>
      </geometry>
    </collision>
    <!--慣性-->
    <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.125" iyy="0.125" izz="0.188" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
<!-- ################# ジョイント設定 ################## -->

	<joint name="base_joint" type="fixed">
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="body"/>
 	</joint>

	<joint name="wheel_left_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 -0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_left"/>
 	</joint>

	<joint name="wheel_right_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <origin xyz="0 0.225 0" rpy="1.57 0 0"/>
    <parent link="body"/>
    <child link="wheel_right"/>
	</joint>

	<joint name="caster_forward_joint" type="fixed">
    <origin xyz="-0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_forward"/>
	</joint>

	<joint name="caster_backward_joint" type="fixed">
    <origin xyz="0.15 0 -0.1" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="caster_backward"/>
	</joint>

	<joint name="lidar_base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.125" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_base"/>
	</joint>

	<joint name="lidar_joint" type="fixed">
    <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
    <parent link="body"/>
    <child link="lidar_link"/>
	</joint>


<!-- ################# Gazebo上の設定 ################## -->
	<!-- 各パーツの情報を設定 -->
	<gazebo reference="body">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Red</material>
	</gazebo>

	<gazebo reference="wheel_left">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="wheel_right">
		<mu1>0.2</mu1>
		<mu2>0.2</mu2>
		<material>Gazebo/Blue</material>
	</gazebo>

	<gazebo reference="caster_forward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Green</material>
	</gazebo>

	<gazebo reference="caster_backward">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Black</material>
	</gazebo>
  
	<gazebo reference="lidar_base">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Yellow</material>
	</gazebo>

	<gazebo reference="lidar_link">
		<mu1>0.0</mu1>
		<mu2>0.0</mu2>
		<material>Gazebo/Purple</material>
	</gazebo>

<!--差動駆動プラグインをロードするための追加要素-->
  <gazebo>
    <!--差動駆動プラグインを指定-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <!--left_wheel_jointとright_wheel_jointをそれぞれ左関節右関節に指定-->
      <leftJoint>wheel_left_joint</leftJoint>
      <rightJoint>wheel_right_joint</rightJoint>
      <!--台座であるbase_linkをrobotBaseFrameに指定-->
      <robotBaseFrame>base_link</robotBaseFrame>
      <!--トレッドを0.25m，車輪直径0.07mとする-->
      <wheelSeparation>0.45</wheelSeparation>
      <wheelDiameter>0.15</wheelDiameter>
      <!--車輪の状態を配信する設定にする-->
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </gazebo>


  <!-- Lidar(ydlidarを例にパラメータを設定) -->
  <!-- GPU rayというタイプで作成 -->
  <gazebo reference="lidar_link">
    <sensor type="gpu_ray" name="head_ydlidar_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>700</samples>
            <resolution>1</resolution>
            <min_angle>-3.14159265358979</min_angle>
            <max_angle>3.14159265358979</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.12</min>
          <max>8</max>
          <resolution>0.72</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_ydlidar_controller" filename="libgazebo_ros_gpu_laser.so">
        <topicName>/scan</topicName>
        <frameName>lidar_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>

追加部分

URDF（追加部分）


  <!-- Lidar(ydlidarを例にパラメータを設定) -->
  <!-- GPU rayというタイプで作成 -->
  <gazebo reference="lidar_link">
    <sensor type="gpu_ray" name="head_ydlidar_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>700</samples>
            <resolution>1</resolution>
            <min_angle>-3.14159265358979</min_angle>
            <max_angle>3.14159265358979</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.12</min>
          <max>8</max>
          <resolution>0.72</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_ydlidar_controller" filename="libgazebo_ros_gpu_laser.so">
        <topicName>/scan</topicName>
        <frameName>lidar_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

これでURDFは完成である．
ここで，Gazebo, Rvizで確認したい．

robot_LRF.launch

robot_LRF.launch

<launch>

    <!-- 引数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>
    <arg name="model" default="$(find diff_drive_pkg)/urdf/robot_v04.urdf"/>
    <!-- <arg name="model" default="$(find diff_drive_pkg)/urdf/robot_with_lidar.urdf"/> -->
    <arg name="x" default="2.0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>

    <!-- Gazeboの起動 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!--ロボットのURDFモデルをパラメータサーバにロードする-->
    <param name = "robot_description" textfile = "$(arg model)"/>

    <!--Gazeboでロボットを生成し，パラメータサーバからのその記述を受ける-->
    <node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model diff_drive_robot -x $(arg x) -y $(arg y) -z $(arg z)"/>

    <!-- robot_state_publisherの起動 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
        <param name="publish_frequency" type="double" value="30.0" />
    </node>

    <!-- Rvizを起動 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find diff_drive_pkg)/rviz/robot.rviz"/>
</launch>

注意）Rvizの設定を保存したrvizファイルはあるものとしている．

本記事でのrvizファイル

robot.rviz

Panels:
  - Class: rviz/Displays
    Help Height: 85
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
      Splitter Ratio: 0.5
    Tree Height: 674
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Name: Time
    SyncMode: 0
    SyncSource: LaserScan
Preferences:
  PromptSaveOnExit: true
Toolbars:
  toolButtonStyle: 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/LaserScan
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Min Color: 0; 0; 0
      Name: LaserScan
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.05000000074505806
      Style: Flat Squares
      Topic: /scan
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        body:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        caster_backward:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        caster_forward:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        lidar_base:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        lidar_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        wheel_left:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        wheel_right:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        base_link:
          Value: true
        body:
          Value: true
        caster_backward:
          Value: true
        caster_forward:
          Value: true
        lidar_base:
          Value: true
        lidar_link:
          Value: true
        odom:
          Value: true
        wheel_left:
          Value: true
        wheel_right:
          Value: true
      Marker Alpha: 1
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        odom:
          base_link:
            body:
              caster_backward:
                {}
              caster_forward:
                {}
              lidar_base:
                lidar_link:
                  {}
              wheel_left:
                {}
              wheel_right:
                {}
      Update Interval: 0
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: odom
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 9.972949981689453
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Field of View: 0.7853981852531433
      Focal Point:
        X: 0
        Y: 0
        Z: 0
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.9503982067108154
      Target Frame: <Fixed Frame>
      Yaw: 0.01539910864084959
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 991
  Hide Left Dock: false
  Hide Right Dock: true
  QMainWindow State: 000000ff00000000fd00000004000000000000015a00000338fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000338000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004400000338000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004090000003efc0100000002fb0000000800540069006d0065010000000000000409000003a200fffffffb0000000800540069006d00650100000000000004500000000000000000000002a80000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1033
  X: 9
  Y: 38

Gazebo上で見えている青色はLiDARで使用されているレーザを可視化したものである．可視化するか否かはURDFの<visualize>true</visualize>で指定できる．trueが可視化して，falseが可視化しない．

このときに見られるトピック一覧

rostopic list

/clicked_point
/clock
/cmd_vel
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/initialpose
/joint_states
/move_base_simple/goal
/odom
/rosout
/rosout_agg
/scan
/tf
/tf_static

/cmd_velがあり，そのトピックにメッセージを配信すれば，動かせる．

実験

「/cmd_velがあり，そのトピックにメッセージを配信すれば，動かせる．」
ということなので，turtlebot3_teleopパッケージのノードを使わせていただくこととする．

robot_LRF_Move.launch

robot_LRF_Move.launch

<launch>

    <!-- 引数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>
    <arg name="model" default="$(find diff_drive_pkg)/urdf/robot_v04.urdf"/>
    <!-- <arg name="model" default="$(find diff_drive_pkg)/urdf/robot_with_lidar.urdf"/> -->
    <arg name="x" default="2.0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>

    <!-- Gazeboの起動 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!--ロボットのURDFモデルをパラメータサーバにロードする-->
    <param name = "robot_description" textfile = "$(arg model)"/>

    <!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
    <node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model diff_drive_robot -x $(arg x) -y $(arg y) -z $(arg z)"/>

    <!-- robot_state_publisherの起動 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
        <param name="publish_frequency" type="double" value="30.0" />
    </node>

    <!-- Rvizを起動 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find diff_drive_pkg)/rviz/robot.rviz"/>
</launch>

    <!-- teleop nodeを起動 -->
    <node name="teleop" pkg="turtlebot3_teleop" type="turtlebot3_teleop_key" output="screen" />

今はいくつかの設定をしていないことによるWarningがあるが，とりあえず無視して実験している．
見づらい場合は，別ターミナルでteleopだけ起動させることもできる．
PCのスペックのためか少し動作が遅いため，動画は16倍速にしている．

感想

今回初めて，URDFの中でセンサ類を記述した．いい感じにロボット作成からGazeboでのシミュレーション環境の構築にまで落とし込むことができた．
この一連の流れはロボットがいくら複雑になろうとも変わらないところであるため，記録用としてまとめた．
ただ，これ以上複雑になってくると，URDFが非常に膨大になって管理しにくくなってくることが懸念される．
そのため，次回ではURDFをxacroファイルで作り直したものをまとめることとする．
非常にコンパクトになるとともに，管理もしやすくなる．

参考文献