UnityをGazeboみたいに使うには？

## はじめに

本記事はROS 2アドベントカレンダー10日目の記事です．

https://qiita.com/advent-calendar/2024/ros2

本記事では、Unity Technologiesが提供しているリアルタイム3D開発プラットフォームであるUnityを使って、ROS 2で作成した自作の移動ロボットをシミュレーションするまでの手順について説明します。

本記事では、自作のツールを用いてGazeboのようにROS 2コマンドを利用してシミュレーションをおこないます。(あれ？どこかで書いたような……）

https://github.com/hijimasa/ros2-tools-to-use-unity-like-gazebo

わかりにくい部分や間違っている部分などありましたら、気軽にコメントいただけると助かります。

## 本記事の対象読者
- ROS 2の基本的なプログラムに触れたことがある人
- 上記に加えて、GPUは持っていないけれど大規模なシミュレーションをしてみたい人

## Unityとは
Unityとは、Unity Technologiesが提供しているリアルタイム3D開発プラットフォームです。主にゲーム開発で利用されてきており、2D/3Dを問わず様々なゲームに利用されています。今回はその物理エンジンを応用することで、オブジェクトが多い比較的大規模な環境でもシミュレーションが可能となることが期待できます。

一方で、URDF Importerというツールが存在しますが、単純にインポートするだけでは摩擦係数やセンサといったGazeboなら自動で設定された項目が設定されません。

本記事では、自作のユーティリティツールを活用することで、上記のUnity特有の部分に直接触れることなくシミュレーションを行う方法について解説します。

## 開発環境の導入

個人的に開発環境にはDockerを使用することを強く推奨します。
Docker環境を使用することで、PC内の環境をできるだけクリーンな状態に保つことができます。

### Dockerのセットアップ
```
curl -fsSL https://get.docker.com -o get-docker.sh
sudo sh get-docker.sh
sudo groupadd docker
sudo usermod -aG docker $USER
```
お好みでNvidiaコンテナツールキットを導入してください。

### Unity + ROS 2のDockerイメージの作成
1. ツールのリポジトリをクローン
   ```
   git clone https://github.com/hijimasa/ros2-tools-to-use-unity-like-gazebo.git
   ```
2. Gitサブモジュールの初期化
   ```
   cd ros2-tools-to-use-unity-like-gazebo/
   git submodule update --init --recursive
   ```
3. dockerディレクトリに移動
   ```
   cd docker
   ```
4. Dockerイメージをビルド
   ```
   ./build-dokcer-image.bash
   ```
5. Dockerコンテナを実行
   ```
   ./run-docker-container.bash
   ```
6. UnityHubを起動
   ```
   unityhub
   ```
7. Unityにサインイン

## ロボットモデルの作成
URDFの基本的な記述内容は変わらないので、すでに手元にシミュレーションしたいロボットモデルがある場合には、そのURDFを含むパッケージをcolcon_ws/srcにコピーしてください。
新しくパッケージを作成する場合には、以下の手順を参考にしてください。
```
cd docker
./launch_docker.sh
# 立ち上がったコンテナ内で
cd src
ros2 pkg create diffbot_description --build-type ament_cmake
# 作成されたパッケージはroot所有になるので、別の端末から以下のコマンドで所有権を移す
sudo chown -R $USER src/hoge_description
```

### 基本的なロボットモデルの記述

基本的なURDFの記述方法はGazeboのときと一切変わらないです。
ただ、Unityではgazeboタグの記述が無視されるので摩擦係数やセンサ情報が設定できなかったり、Gazebo向けのros2_controlプラグインが利用できなかったりするので、その部分を補間する方法について以下で説明します。

### ジョイントの剛性、ダンパ係数の設定

Unityでは各ジョイントに剛性とダンパ係数と摩擦を設定できます。
剛性とダンパ係数はそれぞれ現在位置・速度と目標位置・速度の差分に対して、どれだけの力をジョイントに出力させるかを決定する係数となっています。式で表すと以下のとおりです。
```math
F=S*(P－P_{target})+D*(V－V_{target})
```
ここで、 $F$はジョイントの出力、$S$,$D$はジョイントの剛性、ダンパ係数、$P$, $P_{target}$は現在位置と目標位置、$V$,$V_{target}$は速度と目標速度を表します。

基本的に位置制御では剛性を高くしてダンパ係数をゼロに、速度制御では剛性をゼロでダンパ係数を高く設定する感じです。
今回作成したツールではデフォルトでフリージョイントになるように、剛性とダンパ係数は設定されていません。

URDFでの記述では以下のサンプルのようにjointタグの中にunity_drive_apiタグで設定します。
```
    <joint name="${prefix}_joint" type="continuous">
      <origin xyz="${xyz}" rpy="${radians(-90)} 0 0"/>
      <parent link="${parent}"/>
      <child  link="${prefix}_link"/>
      <axis xyz="0 0 1" />
      <unity_drive_api stiffness="0" damping="30000" force_limit="1000000"/>
    </joint>
```
force_limitパラメータはジョイントの最大出力に関するパラメータです。使用するアクチュエータに合わせて設定しても良いですが、デフォルトはとても高い値で問題ないと思います。

### 摩擦係数の設定

最初の方で述べたように、Isaac SimではGazeboのときの摩擦係数の記述は無視されます。
また、Isaac SimではGazeboのようにリンクごとに摩擦係数を設定するわけではなく、マテリアルに紐付ける形で摩擦係数を設定します。
そのため、今回作成したツールでは、materialタグ内で摩擦係数の設定に関する項目を設定するようにしました。
```
  <physics_material name="wheel">
    <friction static="1.0" dynamic="1.0"/>
  </physics_material>
  <physics_material name="ball">
    <friction static="0.0" dynamic="0.0"/>
  </physics_material>
```
これによって、滑ってほしいボールキャスタ部分は摩擦をゼロにしつつ、車輪部分の摩擦は高く設定できます。

### ros2_controlの設定

Gazeboでは専用のプラグインを使用してロボットを制御していましたが、今回のツールではtopic_based_ros2_contorolパッケージを使用しています。これによって、各ジョイントの指令や状態をUnityとトピックでやり取りすることが出来ます。

以下がros2_controlタグのサンプルです。
```
    <ros2_control name="diffbot" type="system">
      <hardware>
        <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
        <param name="joint_commands_topic">/diffbot/joint_command</param>
        <param name="joint_states_topic">/diffbot/joint_states</param>
        <param name="sum_wrapped_joint_states">true</param>
      </hardware>
      <joint name="left_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
      </joint>
      <joint name="right_wheel_joint">
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="effort"/>
      </joint>
    </ros2_control>
```

### センサ（と追加機能）の設定

Unityでのセンサの設定はGazeboとは異なるパラメータを持つため、isaacタグで記述することにしました。
以下がサンプルです。
```
    <unity>
      <sensor name="lidar_link" type="lidar">
        <update_rate>10</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>400</samples>
              <resolution>1</resolution>
              <min_angle>-3.1415</min_angle>
              <max_angle>3.1415</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.05</min>
            <max>20.0</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </ray>
        <topicName>/lidar/scan</topicName>
        <frameName>lidar_link</frameName>
      </sensor>

      <sensor name="camera_link" type="camera">
        <update_rate>30.0</update_rate>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>800</width>
          <height>600</height>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <cameraName>/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_link</frameName>
      </sensor>

      <sensor name="depth_camera_link" type="depth_camera">
        <update_rate>10.0</update_rate>
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>320</width>
          <height>240</height>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <cameraName>/depth_camera</cameraName>
        <imageTopicName>depth_image_raw</imageTopicName>
        <cameraInfoTopicName>depth_camera_info</cameraInfoTopicName>
        <frameName>depth_camera_link</frameName>
      </sensor>
      
    </unity>
```
各センサの設定方法について説明します。
- LiDARセンサ
  LiDARセンサは現状では設定されているconfigファイルを選択する形で使用できます。センサの次元を選択することで、scanメッセージを出力するかpointcloudメッセージを出力するかを選択できます。

- カメラおよびデプスカメラ
  視野角や解像度に加えて、焦点距離といったパラメータを設定できます。

## シミュレーション上でのロボットモデルの生成・テスト
この節では、これまでの手順で作成したURDFファイルをIsaac Sim上に実際に生成する方法について説明します。

### launchファイルの作成

作成したURDFモデルをIsaac Simに生成するlaunchファイルのサンプルを以下に示します。

```diffbot_spawn.launch.py
import os
import pathlib

from ament_index_python.packages import get_package_share_directory


from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

import xacro

def generate_launch_description():
    isaac_diffbot_description_path = os.path.join(
        get_package_share_directory('diffbot_description'))

    xacro_file = os.path.join(isaac_diffbot_description_path,
                              'robots',
                              'diffbot.urdf.xacro')
    urdf_path = os.path.join(isaac_diffbot_description_path, 'robots', 'diffbot.urdf')
    # xacroをロード
    doc = xacro.process_file(xacro_file, mappings={'use_sim' : 'true'})
    # xacroを展開してURDFを生成
    robot_desc = doc.toprettyxml(indent='  ')
    f = open(urdf_path, 'w')
    f.write(robot_desc)
    f.close()
    relative_urdf_path = pathlib.Path(urdf_path).relative_to(os.getcwd())

    params = {'robot_description': robot_desc}

    robot_controllers = PathJoinSubstitution(
        [
            FindPackageShare("unity_diffbot_sim"),
            "config",
            "unity_diffbot.yaml",
        ]
    )

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[params, robot_controllers],
        output={
            "stdout": "screen",
            "stderr": "screen",
        },
    )
    
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )
    
    diff_drive_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["diff_drive_controller", "--controller-manager", "/controller_manager"],
    )

    velocity_converter = Node(
        package='velocity_pub',
        name='velocity_pub',
        executable='velocity_pub',
       remappings=[
            ('cmd_vel_stamped', '/diff_drive_controller/cmd_vel'),
        ],
    )
        
    isaac_spawn_robot = Node(
        package="unity_ros2_scripts",
        executable="spawn_robot",
        parameters=[{'urdf_path': urdf_path,
                    'package_name' : "diffbot_description",
                    'unity_project_path' : "~/work/Robot_Unity_App",
                    'x' : 0.0,
                    'y' : 0.0,
                    'z' : 0.0,
                    'R' : 0.0,
                    'P' : 0.0,
                    'Y' : 1.57,
                    }],
    )

    image_republish = Node(
        package='image_transport',
        executable='republish',
        name='image_republisher',
        arguments=['compressed', 'raw'],
        remappings=[
            ('in/compressed', '/diffbot/camera_link/image_raw'),
            ('out', '/camera_link/image_raw'),
        ],
        output='screen',
    )

    depth_image_republish = Node(
        package='image_transport',
        executable='republish',
        name='depth_image_republisher',
        arguments=['compressed', 'raw'],
        remappings=[
            ('in/compressed', '/diffbot/depth_camera_link/depth_image_raw'),
            ('out', '/depth_camera_link/depth_image_raw'),
        ],
        output='screen',
    )

    return LaunchDescription([
        control_node,
        node_robot_state_publisher,
        joint_state_broadcaster_spawner,
        diff_drive_controller_spawner,
        velocity_converter,
        isaac_spawn_robot,
        image_republish,
        depth_image_republish,
    ])
```

ここで、Isaac Simにロボットモデルを生成するために重要なノードであるspawn_robotについて説明します。

- spawn_robotは文字通りIsaac Sim上にロボットを生成するノードです。パラメータとして、生成するロボットのURDFのパスを必要とします。また、パラメータでロボットの出現位置姿勢を設定できます。

上記のlaunchファイルでは、twistメッセージをtwist_stampedメッセージに変換するために、velocity_pubという自作ノードを使用しています。ノードの本体は以下にあるので適宜コピーして使ってください。

https://github.com/hijimasa/isaac-ros2-control-sample/tree/main/colcon_ws/src/velocity_pub

### launchファイルを立ち上げる手順

1. Unityの立ち上げ
   ```bash
   ros2 run unity_ros2_scripts launcher
   ```

2. ロボットモデルの立ち上げ
   ```bash
   ros2 launch test_launch_pkg diffbot_spawn.launch.py 
   #ここでパッケージ名はlaunchファイルが存在しているパッケージ名に適宜変更してください
   ```

3. tcp_endpointの立ち上げ
   ```
   ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=0.0.0.0
   ```

4. （オプション）teleop_twist_keyboardの立ち上げ
   ```
   ros2 run teleop_twist_keyboard teleop_twist_keyboard
   ```
## Unity用自作パッケージに本ツールを導入する際のヒント

Unityでは、環境を独自のパッケージとして保存するため、すでに構築済みのUnity用自作パッケージを使用したいという要望もあるのではないでしょうか。本節では、すでに構築したUnity用パッケージに本ツールを導入するまでの簡単な手順を示します。

### アセットの導入

今回のツールを導入する際に必要なアセットは下記の5つです。

- ROS TCP Connector
- Unity Robotics Visulalizations
- URDF Importer
- Unity Sensors
- Unity SensorsROS

上記のパッケージはUnityのメニューバーの[Window]->[Package Manager]で開くPackage Managerウインドウを使って導入できます。
以下はPackage Managerウインドウの例です。
![Screenshot from 2024-11-30 20-44-33.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3326389/43bd928a-dcd9-7f78-ff03-b976338d4e9f.png)

左上の[+]の右の▼ボタンからドロップダウンリストを開き、[Install pacckage from git...]を選択することでGitHubからインストールすることができます。GitHubからインストールするのは以下の3つです。

- ROS TCP Connector ()
- Unity Robotics Visulalizations ()
- URDF Importer ()

また、左上の[+]の右の▼ボタンからドロップダウンリストを開き、[Install pacckage from disk...]を選択することでローカルファイルからインストールすることができます。ローカルファイルからインストールするのは以下の2つです。

- Unity Sensors (ros2-tools-to-use-unity-like-gazebo/work/UnitySensors/Assets/UnitySensors)
- Unity SensorsROS (ros2-tools-to-use-unity-like-gazebo/work/UnitySensors/Assets/UnitySensorsROS)

導入が完了すると、上で示したウインドウの例のように、[Packages-Other]の場所にインストールしたパッケージが表示されます。

### スクリプトの導入

### 物理演算ステップの変更

Unityのデフォルトの物理演算のパラメータはシミュレーションするにはちょっと物足りないスペックなので、Unityの[Edit]->[Project Settings]からProject Settingsウインドウを開き、以下の設定を参考に物理演算のステップ(単位時間あたりの演算回数)などを変更してみてください。
![Screenshot from 2024-11-30 19-50-36.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3326389/ee028be0-29c8-d429-b124-78d0104f9151.png)


### 生成する3Dオブジェクトを原点に生成する設定

Unityの[Edit]->[Preferences]からPreferencesウインドウを開き、[Create Objects at Origin]にチェックを入れることで生成する3Dオブジェクト（平面やキューブ、球など）を原点に生成できるようになります。2024年11月30日時点では、~/.local/share/unity3dのディレクトリが自動で生成できないために設定が保存されない問題があるようですが、私のリポジトリでは生成済みの設定ファイルをマウントして対策しています。
![Screenshot from 2024-11-30 19-49-45.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3326389/3390661f-0acb-3659-6900-cc144b5c71ea.png)


## おわりに
本記事では、URDFの記述を用いてUnity特有のツールに直接触れることなくシミュレーションを行うツールの導入と使用方法について解説しました。
ゲーム開発で培われてきた物理エンジンの力をぜひとも今後のロボット開発に活用してみてはいかがでしょうか。