IgnitionGazebo講座01 worldファイルを起動する

# 環境

この記事は以下の環境で動いています。

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-3470  |
| Ubuntu | 22.04 |
| GPU| GTX 1050 Ti |
| GPU driver | 535.86.05  |
| ROS2 | Humble |
| Ignition Gazebo| Fortress |

# 概要

起動確認
worldファイルの読み込み
modelの作成
パスの通し方

# 予備知識

## Ignitionのバージョン

Ignitionのバージョンも定期的にリリースがされていて、ROS本体などと同じようにそれぞれのリリースに名前がついています。またROS2のバージョンとメインで対応するバージョンも決まっています。

| ROS2 | Ignition |
|:-:|:-:|
| Foxy | Citadel |
| Galactic | Edifice |
| Humble | Fortress |
| Iron | Garden |

※例えばUbuntu22でのignition単体での推奨バージョンはGardenなのですが、Ubuntu22が推奨バージョンであるROS2 Humbleとの推奨の組み合わせはFortressとずれがあることに注意です。

## Gazebo/Ignitionの名前

Gazebo/Igitinioの名前が非常にややこしいのでまずまとめます。

* 旧バージョン(Gazebo Classic)/新バージョン(Ignition Gazebo)の混同
ROS1で主に使われていたGazebo(以下区別のためにGazebo Classicと呼びます)がありましたが、そのシリーズはGazebo11まででアップデートがとまり、新たにリファクタしたIgnitionというシミュレーターのプロジェクトが始まりました。しかし新しいIgnitionを構成する一部の機能やUIにGazeboという名前が使われているために、このシミュレーターもGazeboと呼ばれることがあります（以下区別のためにIgnition Gazeboと呼びます）。
このためにフォーラムなどでただGazeboと呼んだ時に何を指しているのかわからないという問題がありました。
* 旧バージョン(Gazebo classic)でもIgnitionという名前を使っている
Gazebo Classicでも一部のライブラリでIgnitionという名前が使われています。そのためにignitionというキーワードが入っていてもIgnition Gazeboのコードを指すとは限りません。
* 新バージョン(Ignition Gazebo)でIgnitionという名前をやめて、リネームする問題
[開発チームのアナウンス](https://community.gazebosim.org/t/a-new-era-for-gazebo/1356)にあるように新バージョンについてIgnitionという名前をやめて、元のGazeboという名前に戻るとのことです。
プロジェクト名が変わるだけでなく、これに伴ってFortress->Gardenのバージョンの間でソースコードやコマンドでingnition->gazeboへ名前のリネームが行われています。
これによってFortress用のignition gazeno用のコードがGardenでは全く動きません。
またignitionを起動するコマンドがFortressでは`ign`だったものが、Garden以降では`gz`と変わるのも注意です。

当記事では混乱を避けるために 旧バージョンをGazebo Classic、新バージョンをIgnition Gazeboと呼びます。

## 実行環境
描画系に注意が必要です。
* Gazebo Classicは描画はOgre(1)で行っていたために、OpenGLの対応バージョンが低くてもよくvmware上でも実行可能でした。
* Ignition Gazeboでは描画がOgre2になったことから、OpenGLの対応バージョンが上がり、vmware17では正常に描画できません。オプションでogre(1)を使う設定もできますが、テクスチャ等対応できていない機能があるようで、完全には動きません。
IntelCPU環境では動くことは確認しましたが処理重めです。ここではGPUあり環境を前提としています。

## ros-gzブリッジ
Gazebo Classicではpluginが直接ROSトピックをpub/subして、ROSノードと接続するのが主な使われ方でしたが、Ignition GazeboではGazeboの内部ではGazeboTopicでやり取りをして、ign-ros2ブリッジを用いてGazeboTopicをROS2トピックに変換するという使い方を行います。

## ignitionのゾンビ化
Ignitionを終了する時にignitionのサーバー部分が終了できずに残ることがあります。この場合は新たにignitionを起動しても、昔の構成で起動したように見えます。
`ps aux|grep ign`でゾンビ化しているプロセス番号を取得して、`kill -9 XXXX`でkillしましょう。

# インストール&動作確認

* aptでインストールできます。
    * ROS2のインストールで`sudo apt install ros-humnble-desktop-full`を実行した場合はこのコマンドでIgnition Gazeboがインストールされます。
    * `sudo apt install ros-humble-simulation`でもインストール可能です。
* ターミナルで`ign gazebo`を実行することで以下の画面が開きます。

![basic_sample1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/e2f90f89-519b-8a5b-1172-0b74e995f172.png)

* 画面の中から適当に1つ選択して「Run」を押すと、そのサンプルが実行されます。
    * 初回起動ではデータのダウンロードをするのか、simの起動までしばらくかかります。

![basic_sample2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/1d7891b0-bb8d-be55-e502-5891c15c473f.png)

* `sudo apt-get install ros-humble-ros-ign-bridge`でros-ign-bridgeをインストールする。

# worldファイルの実行

* 事前にインストールされているサンプルのworldファイルを実行してみます。
    * ターミナルで`ign gazebo -r visualize_lidar.sdf`を実行することで以下の画面が開きます。
    * sdfファイル自体は`/usr/share/ignition/ignition-gazebo6/worlds/`にあります。ここにパスが通っているために、このフォルダのsdfファイルが呼ばれています。

![basic_visualize_lidar.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/39684884-894f-19ce-f1df-03da4e4db460.png)

## Twistの送信
* 以下のコマンドでブリッジを起動します。

```shell
source /opt/ros/humble/setup.bash
ros2 run ros_gz_bridge parameter_bridge /model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist
```

* 以下のコマンドででTwistを送信します。

```shell
source /opt/ros/humble/setup.bash
ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/Twist '{linear: {x: 0.1}}'
```

* GUI画面上でロボットが前進します

## LaserScanの受信

* ブリッジを起動します。

```shell
source /opt/ros/humble/setup.bash
ros2 run ros_gz_bridge parameter_bridge /lidar2@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan /clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock
```

* Rviz2を起動します。この時にuse_sim_timeをtrueにします。

```shell
source /opt/ros/humble/setup.bash
ros2 run rviz2 rviz2 --ros-args -p  use_sim_time:=true
```

* displayでLaserScanの表示（表示が見にくいのでサイズを0.01->0.1に変更）、`Fixed Frame`を`vehicle_blue/lidar_link/gpu_lidar`に変更すると以下のようにLidar情報が表示されます。

![basic_rviz.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/94f1bb52-1186-a566-9f6c-e14b7ee97ea4.png)


# 自作のworldファイル、modelファイルを使いROS2から起動

## ソースコード

### worldファイル

```xml:ignition_lecture/worlds/basic_world1.sdf
<?xml version="1.0" ?>

<sdf version="1.6">
  <world name="visualize_lidar_world">

    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="libignition-gazebo-physics-system.so"
      name="ignition::gazebo::systems::Physics">
    </plugin>
    <plugin
      filename="libignition-gazebo-sensors-system.so"
      name="ignition::gazebo::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin
      filename="libignition-gazebo-scene-broadcaster-system.so"
      name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>

    <light name="sun" type="directional">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <pose>0 0 0 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>20 20 0.1</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>20 20 0.1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <model name="box">
      <pose>0 -1 0.5 0 0 0</pose>
      <link name="box_link">
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <include>
      <pose>5 0 0 0 0 1.57</pose>
      <uri>https://fuel.gazebosim.org/1.0/nate/models/Valve</uri>
    </include>

    <include>
      <uri>package://ignition_lecture/models/my_box</uri>
      <pose>0 2 0 0 0 0</pose>
    </include>

  </world>
</sdf>
```

* `https://fuel.gazebosim.org/1.0/nate/models/Valve`はfuel(gazebo用のモデルを格納したwebサイト)からモデルを取得します。一度取得するとローカルにキャッシュされ次回からはそれを使います。
* `package://ignition_lecture/models/my_box`の様にするとローカルのパッケージからファイルを読み込みます。

## modelファイル

```xml:ignition_lecture/models/my_box/model.config
<?xml version="1.0"?>
<model>
  <name>my_box</name>
  <version>1.0</version>
  <sdf version="1.8">model.sdf</sdf>

  <author>
    <name>project-srs</name>
    <email>project@srs.com</email>
  </author>

  <description>
    this is laser marker.
  </description>
</model>
```

```xml:ignition_lecture/models/my_box/model.sdf
<?xml version="1.0" ?>
<sdf version="1.8">
    <model name="my_box">
      <link name="box_link">
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <mesh>
              <uri>meshes/laser_marker.stl</uri>
              <!-- <uri>package://ignition_lecture/models/my_box/meshes/laser_marker.stl</uri> -->
              <scale>0.001 0.001 0.001</scale>
            </mesh>
          </geometry>
          <material>
            <ambient>0 1 0 1</ambient>
            <diffuse>0 1 0 1</diffuse>
            <specular>0 1 0 1</specular>
          </material>
        </visual>
      </link>
    </model>
</sdf>
```

* メッシュファイルは`meshes/laser_marker.stl`の様に相対パスでアクセスすることが推奨されています。またコメントアウト部分にあるように、packageから引くこともできます。

### hook系

モデルにアクセスできるために環境変数をセットする必要があります。今回はhook機能を使って環境変数をセットします。

```text:ignition_lecture/hooks/ignition_lecture.dsv.in
prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share
```

```cmake:ignition_lecture/CMakeLists.txtの一部
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
```

### launch

```python:ignition_lecture/launch/basic_world1.launch.py
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    world_file_path = PathJoinSubstitution([
        get_package_share_directory('ignition_lecture'),
        'worlds',
        'basic_world1.sdf'
    ])

    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
            launch_arguments=[
                ('gz_args', world_file_path)]
        )
    ])
```

## ビルド&実行

```shell:ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
```

```shell:実行
source ~/ros2_ws/install/setup.bash
ros2 launch ignition_lecture sim.launch.py
```

以下のように指定のオブジェクトがあるworldを出現できました。

![basic_world1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/1d25ede8-07a2-593c-f232-1a9b79808aa0.png)

# 参考
[ros2 tutorial / simulation](https://docs.ros.org/en/humble/Tutorials/Advanced/Simulators/Gazebo/Gazebo.html)
[modelでのパスの扱い](https://github.com/gazebosim/gz-fuel-tools/issues/77)
[colcon 環境変数の扱い](https://colcon.readthedocs.io/en/released/developer/environment.html)


# 目次ページへのリンク
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)