ROS 2におけるシミュレーション環境の構築

## はじめに
今回の記事では、ROS 2におけるシミュレーション環境の構築の方法について記載します。
ROS 2では、ROSと同様にURDFを使用し、ロボットの形状やジョイント、センサーなどを定義します。
シミュレーション環境の構築を行うことで、ロボットが現実世界でどのような挙動をするのかを確認することができたり、現実世界でロボットが行う可能性のある動きを、実際のハードウェアや環境に負荷をかけることなくバーチャル環境でテストすることができたりします。
今回は以下の流れでGazeboを利用したシミュレーションを行います。詳細は後述します。

1. Gazeboにロボットを表示させるための設定
 1)パッケージの作成
 2)URDFについて
 3)プラグインの設定
 4)launchファイルの作成
 5)setup.pyの編集（urdfファイルとlaunchファイルの登録）
 6)ビルド・実行
 7)setup.pyの編集（rvizファイルの登録）
 
2. シミュレーションのためのワールドの設定
 1)事前準備
 2)worldファイルの作成
 3)launchファイルの作成
 4)setup.pyの編集（worldファイルの登録）
 5)ビルド・実行

  
:::note info
この記事は[ROSに関する投稿](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d)	の一部です。
目次は[こちら](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d#%E7%9B%AE%E6%AC%A1)
:::

#### 前提条件
今回の記事は以下の環境で動かすことを前提に記載しています。

|            | 条件        | 
|:-----------|-------------|
|OS          |Ubuntu 22.04 |
| ROS        |ROS 2 humble |

## Gazeboにロボットを表示させるための設定

### 1）パッケージ作成
以下のコマンドでPython用パッケージを作成します。<br>今回、パッケージ名は「sim_py_01」としています。

```
cd ~/ros2_ws/src/
ros2 pkg create --build-type ament_python sim_py_01
```

### 2）URDFについて
URDF(Unified Robot Description Format)とは、ロボットの形状と構造を指定するためのファイル形式で、LinkとJointから構成されます。
Linkはロボットを構成するパーツ部分になります。パーツの形状や慣性モーメント、衝突プロパティを定義します。
Jointはパーツ部分にあたるLink同士を接続する関節部分を定義します。親リンクと子リンクを定義し、親リンクから見た子リンクの位置や姿勢を記述することで関節部分を表現します。

https://wiki.ros.org/ja/urdf/Tutorials/Create%20your%20own%20urdf%20file


#### urdfファイルを格納するディレクトリの作成

以下のコマンドでパッケージ内にurdfディレクトリを作成します。<br>今回は、先ほど作成したsim_py_01パッケージ内にurdfディレクトリを作成しています。

```
cd ~/ros2_ws/src/sim_py_01/
mkdir urdf
```
 
#### urdfファイルの作成と解説

以下のファイルを~/ros2_ws/src/sim_py_01/urdfに配置します。<br>今回、ファイル名は「wheel_robot_simple.urdf」としています。

```xml:~/ros2_ws/src/sim_py_01/urdf/wheel_robot_simple.urdf
<robot name="wheel_robot_simple">
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>
  <material name="red">
    <color rgba="1.0 0.0 0.0 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 1.0 1.0"/>
  </material> 
  <link name="base_link"/>
  <joint name="body_joint" type="fixed">
    <origin xyz="-0.1 0.0 0.072" rpy="0.0 0.0 0.0"/>
    <parent link="base_link"/>
    <child link="body_link"/>
  </joint>
  <link name="body_link">
    <visual>
      <geometry>
        <box size="0.4 0.3 0.1"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.4 0.3 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.008333333" ixy="0.0" ixz="0.0" iyy="0.014166667" iyz="0.0" izz="0.020833333"/>
    </inertial>
  </link>
  <joint name="back_ball_joint" type="fixed">
    <origin xyz="-0.13 0.0 -0.022" rpy="0.0 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="back_ball_link"/>
  </joint>
  <link name="back_ball_link">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
  </link>
  <joint name="left_wheel_joint" type="continuous">
    <origin xyz="0.1 0.18 0.0" rpy="-1.5707 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0.0 0.0 1.0"/>
  </joint>
  <link name="left_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
      <material name="red"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000285867" ixy="0.0" ixz="0.0" iyy="0.000285867" iyz="0.0" izz="0.0005184"/>
    </inertial>
  </link>
  <joint name="right_wheel_joint" type="continuous">
    <origin xyz="0.1 -0.18 0.0" rpy="1.5707 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0.0 0.0 -1.0"/>
  </joint>
  <link name="right_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
      <material name="red"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000285867" ixy="0.0" ixz="0.0" iyy="0.000285867" iyz="0.0" izz="0.0005184"/>
    </inertial>
  </link>
  <joint name="front_laser_joint" type="fixed">
    <origin xyz="0.1 0.0 0.08" rpy="0.0 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="front_laser_link"/>
  </joint>
  <link name="front_laser_link">
    <visual>
      <geometry>
        <cylinder radius="0.03" length="0.02"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.03" length="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001292" ixy="0.0" ixz="0.0" iyy="0.00001292" iyz="0.0" izz="0.0000225"/>
    </inertial>
  </link>
</robot>
```

- **\<robot>**　ロボット定義のすべての要素を\<robot>内に記述する必要がある
　　　　（属性：name　ロボットの名前を定義）


- **\<material>**　ロボットの色などのビジュアル要素を指定
　　　　　（属性：color　rgbaで指定し、各数値の範囲は0~1で指定）


- **\<link>**　visual、collision、inertialの3要素を持つロボットの一つの部分を定義
　　　　（属性：name　ロボットの一つの部品の名前を定義）

    - **\<visual>**　linkの見た目部分を定義

        - **\<origin>**　位置と姿勢を定義
            　　　　（属性：xyz　xyz距離[m]）
            　　　　（属性：rpy　回転させる角度[red]）

        - **\<geometry>**　オブジェクトの形状を定義
            - **\<box>**　直方体。中心が原点になる。
            　　（属性： size 　ボックスの3辺の長さを設定）

            - **\<cylinder>**　円柱。中心が原点になる。
            　　　　（属性： radius 　半径を指定）
            　　　　（属性：length　長さを指定）

            - **\<sphere>**　球体。中心が原点になる。
            　　　　（属性： radius 　半径を指定）

            - **\<mesh>**　メッシュファイルをもとにしたメッシュ要素
            　　　　（属性： filename 　メッシュファイルのパスを指定）
            　　　　（属性： scale 　各軸の伸縮倍率を指定）

        - **\<material>**　オブジェクトの見た目を定義

            - **\<color>**　色を指定する
            　　　　（属性： rgba 　rgbaで指定し、各数値の範囲は0~1で指定）

            - **\<texture>**　テクスチャを指定
            　　　　（属性： filename 　ファイルのパスを指定）


    - **\<collision>**　linkの衝突を定義

        - **\<origin>**　位置と姿勢を定義

        -  **\<geometry>**　オブジェクトの形状を定義

            - **\<box>**　直方体。中心が原点になる。
            　　（属性： size 　ボックスの3辺の長さを設定）

            - **\<cylinder>**　円柱。中心が原点になる。
            　　　　（属性： radius 　半径を指定）
            　　　　（属性：length　長さを指定）

            - **\<sphere>**　球体。中心が原点になる。
　            　　（属性： radius 　半径を指定）

            - **\<mesh>**　メッシュファイルをもとにしたメッシュ要素
　            　　（属性： filename 　メッシュファイルのパスを指定）
　            　　（属性： scale 　各軸の伸縮倍率を指定）


    - **\<inertial>**　linkの質量やその重心の位置、慣性モーメントを定義

        - **\<origin>**　位置と姿勢を定義

        - **\<mass>**　　質量[kg]を定義

        - **\<inertia>**　慣性モーメントを定義
<br>

- **\<joint>**　linkとlinkの関係を定義
　　　（属性：name　jointの名前を定義）
　　　（属性：type　以下の6種類のjointのタイプのうちいずれかを定義）
　　　　・ revolute　軸に沿って回転し制限範囲を持つ
　　　　・continuous　軸を中心に回転し制限範囲を持たない
　　　　・prismatic　軸に沿ってスライドし制限範囲を持つ
　　　　・fixed　すべての自由度がロックされていて移動しない
　　　　・floating　何も拘束がないため自由な動きが可能
　　　　・planar　軸に対して垂直な平面の中での動きが可能

    - **\<origin>**　位置と姿勢を定義

    - **\<parent>**　親リンクを定義
　　　（属性：link　jointの根元となる親リンク名）

    - **\<child>**　子リンクを定義
　　　（属性：link　jointの先につながる子リンクの名前）

    - **\<axis>**　軸を定義
　　（属性：xyz　軸の方向を3次元ベクトルを指定）

 <br>
 
https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/URDF-Main.html

https://wiki.ros.org/urdf/XML

 

### 3）プラグインについて
プラグインとは、共有ライブラリとしてコンパイルされ、シミュレーション環境で特定の機能を実現するために必要なコードをまとめたものです。
プラグインを入れて必要なパラメータを設定することで、センサーの動作をGazebo上でシミュレーションできます。urdfファイルの\</robot>タグ直前に追記します。

https://classic.gazebosim.org/tutorials?tut=plugins_hello_world&cat=write_plugin

 

#### 差動駆動プラグインの追加

駆動方法が2輪差動駆動の車輪移動ロボットの基本的なコントロールを提供するプラグインです。
以下のコードを\<link name="right_wheel_link"> ~\</link>の下に追記してください。

```xml:~/ros2_ws/src/sim_py_01/urdf/wheel_robot_simple.urdf
  <gazebo>
    <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so ">
      <ros>
        <namespace>/wheel_robot_simple</namespace>
        <remapping>cmd_vel:=cmd_vel</remapping>
        <remapping>odom:=odom</remapping>
      </ros>
      <left_joint>left_wheel_joint</left_joint>
      <right_joint>right_wheel_joint</right_joint>
      <wheel_separation>0.36</wheel_separation>
      <wheel_diameter>0.144</wheel_diameter>
      <max_wheel_torque>20</max_wheel_torque>
      <max_wheel_acceleration>1.0</max_wheel_acceleration>
      <publish_odom>true</publish_odom>
      <publish_odom_tf>true</publish_odom_tf>
      <publish_wheel_tf>true</publish_wheel_tf>
      <update_rate>30</update_rate>
      <odometry_frame>odom</odometry_frame>
      <robot_base_frame>base_link</robot_base_frame>
    </plugin>
  </gazebo>
```

|タグ                               　  |説明                                          | 
|:--------------------------------------|:----------------------------------------------|
|remapping                         |トピック名を変更                                    |
|namespace                         |名前空間を指定                                      |
|left_joint                        |左車輪を制御するジョイントの名前を指定                |
|right_joint                       |右車輪を制御するジョイントの名前を指定                |
|wheel_separation                  |左右の車輪間の距離を指定する [m]　（デフォルト：1.0m） |
|wheel_diameter                    |車輪の直径を指定 [m]                                 |
|max_wheel_torque                  |ホイールが生成できる最大トルク [Nm]（デフォルト：5Nm） |
|max_wheel_acceleration            |ホイールの加速度                                     |
|publish_odom                     |オドメトリを公開するか（デフォルト：true）            |
|publish_odom_tf                   |オドメトリの変換を公開するか（デフォルト：true）       |
|publish_wheel_tf                  |ホイールリンクの変換を公開するか（デフォルト：false ） |
|update_rate                       |プラグインの更新速度 [Hz]                            |
|odometry_frame                    |remappingタグのodomに合わせて設定する                |
|robot_base_frame                  |ベースフレーム                                       |



https://gazebosim.org/docs/harmonic/migrating_gazebo_classic_ros2_packages/#libgazebo_ros_diff_drive-so

https://classic.gazebosim.org/tutorials?tut=ros_gzplugins

 
#### LiDARの追加

シミュレーター環境上でLiDARを模擬するためのプラグインです。
以下のコードを\<link name="front_laser_link"> ~\</link>の下に追記してください。

```xml:~/ros2_ws/src/sim_py_01/urdf/wheel_robot_simple.urdf
  <gazebo reference="front_laser_link">
    <sensor type="ray" name="front_laser">
      <pose>0 0 0 0 0 0</pose>
      <always_on>true</always_on>
      <update_rate>10</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3.14159</min_angle>
            <max_angle>3.14159</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</min>
          <max>10.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lidar" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <remapping>~/out:=scan</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>front_laser_link</frame_name>
      </plugin>
    </sensor>
  </gazebo>
```

|タグ                          |説明                                                   | 
|:-----------------------------|:-------------------------------------------------------|
|always_on                     |trueの場合、センサーは常に\<update_rate>に従って更新される|
|update_rate                   |データが生成される頻度                                   |
|visualize                   |Gazebo上にLidarの光線が表示するか                          |
|horizontal                  |水平方向のLidarの光線のプロパティを定義（3DLidarの場合は垂直方向のLidarの光線のプロパティを定義する<vertical>も記載する）<br>samples　　一周で生成されるLidarの光線の数<br>resolution　resolution*samplesの値が、一周で取得するデータの点の数<br>min_angle　最小角度<br>max_angle　最大角度（min_angleより大きくなければならない）      |
|range             |シミュレーションされた各光線の範囲を設定 <br>min 　 　　Lidarの光線の最小距離を定義 <br>max　　　 Lidarの光線の最大距離を定義 <br>resolution　Lidarの光線の解像度を定義 |
|noise             |生成されたスキャンに適用するノイズモデルのプロパティ<br>type　　ノイズの種類　（デフォルト：gaussian）<br>mean　 ノイズの平均値　（デフォルト：0）<br>stddv　 ノイズの標準偏差　（デフォルト：0）|


https://enpm-662introduction-to-robot-modelling.readthedocs.io/en/latest/plugin.html

https://classic.gazebosim.org/tutorials?tut=guided_i1

http://sdformat.org/spec?ver=1.6&elem=sensor#horizontal_resolution

https://gazebosim.org/docs/latest/sensors/

 
<br>
wheel_robot_simple.urdfファイルのコード全文を記載します。

```xml:~/ros2_ws/src/sim_py_01/urdf/wheel_robot_simple.urdf
<robot name="wheel_robot_simple">
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>
  <material name="red">
    <color rgba="1.0 0.0 0.0 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 1.0 1.0"/>
  </material> 
  <link name="base_link"/>
  <joint name="body_joint" type="fixed">
    <origin xyz="-0.1 0.0 0.072" rpy="0.0 0.0 0.0"/>
    <parent link="base_link"/>
    <child link="body_link"/>
  </joint>
  <link name="body_link">
    <visual>
      <geometry>
        <box size="0.4 0.3 0.1"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.4 0.3 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.008333333" ixy="0.0" ixz="0.0" iyy="0.014166667" iyz="0.0" izz="0.020833333"/>
    </inertial>
  </link>
  <joint name="back_ball_joint" type="fixed">
    <origin xyz="-0.13 0.0 -0.022" rpy="0.0 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="back_ball_link"/>
  </joint>
  <link name="back_ball_link">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
  </link>
  <joint name="left_wheel_joint" type="continuous">
    <origin xyz="0.1 0.18 0.0" rpy="-1.5707 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0.0 0.0 1.0"/>
  </joint>
  <link name="left_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
      <material name="red"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000285867" ixy="0.0" ixz="0.0" iyy="0.000285867" iyz="0.0" izz="0.0005184"/>
    </inertial>
  </link>
  <joint name="right_wheel_joint" type="continuous">
    <origin xyz="0.1 -0.18 0.0" rpy="1.5707 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0.0 0.0 -1.0"/>
  </joint>
  <link name="right_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
      <material name="red"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.072" length="0.04"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000285867" ixy="0.0" ixz="0.0" iyy="0.000285867" iyz="0.0" izz="0.0005184"/>
    </inertial>
  </link>
  <gazebo>
    <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
      <ros>
        <namespace>/wheel_robot_simple</namespace>
        <remapping>cmd_vel:=cmd_vel</remapping>
        <remapping>odom:=odom</remapping>
      </ros>
      <left_joint>left_wheel_joint</left_joint>
      <right_joint>right_wheel_joint</right_joint>
      <wheel_separation>0.36</wheel_separation>
      <wheel_diameter>0.144</wheel_diameter>
      <max_wheel_torque>20</max_wheel_torque>
      <max_wheel_acceleration>1.0</max_wheel_acceleration>
      <publish_odom>true</publish_odom>
      <publish_odom_tf>true</publish_odom_tf>
      <publish_wheel_tf>true</publish_wheel_tf>
      <update_rate>30</update_rate>
      <odometry_frame>odom</odometry_frame>
      <base_frame_id>base_link</base_frame_id>
    </plugin>
  </gazebo>
  <joint name="front_laser_joint" type="fixed">
    <origin xyz="0.1 0.0 0.08" rpy="0.0 0.0 0.0"/>
    <parent link="body_link"/>
    <child link="front_laser_link"/>
  </joint>
  <link name="front_laser_link">
    <visual>
      <geometry>
        <cylinder radius="0.03" length="0.02"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.03" length="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.00001292" ixy="0.0" ixz="0.0" iyy="0.00001292" iyz="0.0" izz="0.0000225"/>
    </inertial>
  </link>
  <gazebo reference="front_laser_link">
    <sensor type="ray" name="front_laser">
      <pose>0 0 0 0 0 0</pose>
      <always_on>true</always_on>
      <update_rate>10</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3.14159</min_angle>
            <max_angle>3.14159</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</min>
          <max>10.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lidar" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <remapping>~/out:=scan</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>front_laser_link</frame_name>
      </plugin>
    </sensor>
  </gazebo>
</robot>
```

### 4）launchの作成
作成したurdfファイルをRviz2上に表示するためのlaunchファイルを作成します。
以下のコマンドでパッケージ内にlaunchディレクトリを作成します。

```
cd ~/ros2_ws/src/sim_py_01/
mkdir launch
```

以下のファイルを~/ros2_ws/src/sim_py_01/launchに配置します。<br>今回、ファイル名は「wheel_robot_simple_rviz.launch.py」としています。

```python:~/ros2_ws/src/sim_py_01/launch/wheel_robot_simple_rviz.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
    package_dir = get_package_share_directory("sim_py_01")
    urdf = os.path.join(package_dir, "urdf", "wheel_robot_simple.urdf")
    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            arguments=[urdf]),
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            name='joint_state_publisher_gui',
            arguments=[urdf]),
        Node(
            package="rviz2",
            executable="rviz2",
            name="rviz2"),
    ])
```

### 5）setup.pyの編集（urdfファイルとlaunchファイルの登録）
Rviz2を起動する準備のために、setup.pyに作成したurdfファイルとlaunchファイルのパスを追記します。

```python:~/ros2_ws/src/sim_py_01/setup.py
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'sim_py_01'
setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='',
    maintainer_email='',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)
```

### 6）ビルド・実行
以下のコマンドでsim_py_01パッケージをビルドし、パッケージのパスをシステムに登録したら、launchファイルを実行してください。

```
cd ~/ros2_ws
colcon build --packages-select sim_py_01
source ~/ros_ws/install/setup.bash
ros2 launch sim_py_01 wheel_robot_simple_rviz.launch.py
```

この段階では正常に表示されないため、Rviz2上で以下の3つの項目を設定します。

* FixedFrameをmapから/base_linkに変更
* Addをクリックし、By display typeからRobotModelとTFを追加
* RobotModelのDescription Topicを/robot_descriptionに変更

![Screenshot from 2024-06-26 16-49-56.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3855298/4db8a2a0-8824-6669-d162-ea3905aa489b.png)


Rviz2上に作成したロボットが表示されたら、以下の手順で保存します。<br>今回、ファイル名は「wheel_robot_simple.rviz」 としています。

①sim_py_01パッケージ内にrvizフォルダを作成
②file > save config asでファイル名を設定し保存

### 7）setup.pyの編集（rvizファイルの登録）

setup.pyにrvizファイルの登録を追加します。
以下のコードをsetup.pyの(os.path.join('share', package_name, 'urdf'), glob('urdf/*')),<br>の下に追記してください。

```
 (os.path.join('share', package_name, 'rviz'), glob('rviz/*')),
```

## シミュレーションのためのワールドの設定
Gazebo上にロボットが表示できたので、次はワールドを表示させていきます。

### 1）事前準備

#### 3Dモデルの準備
STLファイル（拡張子：stl）は3Dモデルやオブジェクトのサーフェスジオメトリを記述する、三角形の結合（三角形メッシュ）で構成されています。ファイルサイズが小さく処理速度は速いですが、色とテクスチャなどはないため、比較的機能が限られます。
一方、COLLADAファイル（拡張子：dae）は画像、テクスチャ、3Dモデルなど、様々なコンテンツを保存することができるため、モデルの見た目の部分を表現しています。しかし、COLLADAファイルは書き出しに時間がかかったり、多くのストレージ容量が必要だったりします。

https://www.adobe.com/jp/creativecloud/file-types/image/vector/stl-file.html

https://www.adobe.com/jp/creativecloud/file-types/image/vector/collada-file.html

STLファイルとCOLLADAファイルを以下のディレクトリにそれぞれ保存します。

ros2_ws/
　┗ src/
　　　┗ sim_py_01/
　　　　　┗ models/
　　　　　　　┗ maze/
　　　　　　　　　┣ DAE/
　　　　　　　　　┃　┗ maze.dae
　　　　　　　　　┗ STL/
　　　　　　　　　　　┗ maze.stl
 

#### ファイルの準備（sdfファイル、configファイル）
3DモデルをGazebo上に表示するため、sdfファイルとconfigファイルを作成します。
以下の2つのファイルを~/ros2_ws/src/sim_py_01/models/mazeに配置します。<br>今回、ファイル名は「model.sdf」と「model.config」としています。


```xml:~/ros2_ws/src/sim_py_01/models/maze/model.sdf
<?xml version="1.0"?>
<sdf version='1.5'>
  <model name="maze">
    <static>true</static>
    <link name="maze">
      <collision name="collision">
        <geometry>
          <mesh>
            <uri>model://maze/STL/maze.stl</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <mesh>
            <uri>model://maze/DAE/maze.dae</uri>
          </mesh>
        </geometry>
      </visual>
    </link>
  </model>
</sdf>
```

```xml:~/ros2_ws/src/sim_py_01/models/maze/model.config
<?xml version="1.0"?>
<model>
  <name>maze</name>
  <version>1.0</version>
  <sdf version="1.5">model.sdf</sdf>
  <author>
    <name></name>
    <email></email>
  </author>
  <description>
    this is a maze world.
  </description>
</model>
```

 #### setup.pyの編集
build時、workspace_name/install/package_name/share/package_name/にmodelsディレクトリ中のモデルファイル（COLLADAファイルやSTLファイルを含む）がコピーされるようにする。

 ```python:~/ros2_ws/src/sim_py_01/setup.py
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'sim_py_01'

def generate_data_files(share_path, dir):
    data_files = []
    for path, _, files in os.walk(dir):
        list_entry = (share_path + path, [os.path.join(path, f) for f in files if not f.startswith('.')])
        data_files.append(list_entry)
    return data_files


setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
        (os.path.join('share', package_name, 'rviz'), glob('rviz/*')),
    ] + generate_data_files('share/' + package_name + '/', 'models'),
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='',
    maintainer_email='',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)
```


#### TurtleBot3に関連するパッケージのインストール
以下のコマンドで、TurtleBot3に関連するパッケージをインストールしてください。

```
source ~/.bashrc
sudo apt install ros-humble-dynamixel-sdk
sudo apt install ros-humble-turtlebot3-msgs
sudo apt install ros-humble-turtlebot3
ROBOTIS e-Manual 
```

### 2）worldファイルの作成
worldファイルを作成します。以下のファイルを~/ros2_ws/src/sim_py_01/worldに配置します。今回、ファイル名は「maze.world」としています。

```xml:~/ros2_ws/src/sim_py_01/world/maze.world
<?xml version="1.0"?>
<sdf version="1.5">
  <world name="maze">
    <include>
      <name>sun</name>
      <uri>model://sun</uri>
    </include>
    <light name="point_light" type="point">
      <pose>0 0 1 0 0 0</pose>
      <diffuse>1 1 1 1</diffuse>
      <specular>0.1 0.1 0.1 1.0</specular>
      <attenuation>
        <range>10</range>
        <constant>0.5</constant>
        <linear>0.5</linear>
        <quadratic>0</quadratic>
      </attenuation>
      <cast_shadows>false</cast_shadows>
      <direction>0 0 -1</direction>
    </light>
    <include>
      <name>sim_world</name>
      <uri>model://maze</uri>
      <static>true</static>
      <pose>0 0 0 0 0 0</pose>
    </include>
  </world>
</sdf>
```

### 3）launchファイルの作成
ワールドをGazeboに表示するためのlaunchファイルを作成します。
以下のファイルを~/ros2_ws/src/sim_py_01/launchに配置します。<br>今回、ファイル名は「wheel_robot_simple_in_sim_world.launch.py」としています。

```python:~/ros2_ws/src/sim_py_01/launch/wheel_robot_simple_in_sim_world.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
    package_dir = get_package_share_directory('sim_py_01')
    urdf = os.path.join(package_dir, 'urdf', 'wheel_robot_simple.urdf')
    rviz = os.path.join(package_dir, 'rviz', 'wheel_robot_simple.rviz')
    world = os.path.join(package_dir, 'world', 'maze.world')
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    os.environ['GAZEBO_MODEL_PATH'] = os.path.join(package_dir, 'models')
    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen',
            arguments=[urdf]),
        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
            arguments=[urdf]),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz]),
        ExecuteProcess(
            cmd=['gazebo', '--verbose', '-s',
                 'libgazebo_ros_factory.so', world],
            output='screen'),
        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            name='urdf_spawner',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-topic', 'robot_description',
                       '-entity', 'wheel_robot_simple'])
    ])
```

### 4）setup.pyの編集（worldファイルの登録）

setup.pyにworldファイルの登録を追加します。
以下のコードをsetup.pyの(os.path.join('share', package_name, 'rviz'), glob('rviz/*')),<br>の下に追記してください。

```
(os.path.join('share', package_name, 'world'), glob('world/*')),
```

### 5）ビルド・実行
以下のコマンドでsim_py_01パッケージをビルドし、パッケージのパスをシステムに登録したら、
Gazebo上にロボットとワールドを表示します。

```
cd ~/ros2_ws
colcon build --packages-select sim_py_01
source ~/ros2_ws/install/setup.bash
ros2 launch sim_py_01 wheel_robot_simple_in_sim_world.launch.py
 ```

![865f978c-0bcf-4ca3-b30d-d86d93226699.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3855298/d8e2fde5-d9a4-fb7b-0f40-36b85f43dc61.png)

Gazebo上に表示したロボットが動くか確認します。別のターミナルを開き、キー入力で動作するプログラムを実行してください。<br>今回は~/ros2_ws/src/sim_py_01/urdf/wheel_robot_simple.urdfのプログラムでトピック名を/wheel_robot_simple/cmd_velにしているため、teleop_keyboardを実行する際に、トピック名を変更する必要があります。

```
ros2 run turtlebot3_teleop teleop_keyboard cmd_vel:=/wheel_robot_simple/cmd_vel
```
![04167edb-ba5c-4792-ac0c-07e53408f3cb.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3855298/584aa800-3127-c2fd-d4e3-7cc63286bc55.gif)


## おわりに
今回は、ROS 2におけるシミュレーション環境の構築の方法について紹介しました。
次回の記事はSLAMについて投稿する予定です。お楽しみに！