【ROS2】Gazeboにオブジェクトをspawn/deleteする

# はじめに
GazeboとROSを使用してとシミュレーションを行う場合に、Gazebo上にオブジェクトをspawn/deleteする方法がいくつかあるのでまとめます。
本記事では、以下の方法を紹介します。
- spawnする方法
    - spawn_entity.pyノードを使用する
    - ROSサービスを使用する
- deleteする方法
    - ROSサービスを使用する

# 環境
以下の環境で動作を確認しています。

| 項目 | バージョン |
|:-: | :-:|
| Ubuntu | 22.04 |
| ROS2 | Humble |
| Gazebo (Gazebo Classic) | Gazebo 11 |

Gazeboのバージョンについては以下の記事が参考になります。

https://qiita.com/porizou1/items/b97e7a5e282a6a749a7d

# spawnする方法
## ROSノードを使用する
`gazebo_ros`パッケージ内に、`spawn_entity.py`ノードがあります。

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ros2/gazebo_ros/scripts/spawn_entity.py

`-h`オプションで起動方法を確認することができます。
最低限、以下の3つのうちどれかでオブジェクトの定義ファイルを指定して、
`(-file FILE_NAME | -topic TOPIC_NAME | -database ENTITY_NAME | -stdin)` 
以下のオプションでオブジェクトの名前を指定すればspawnすることができます。
`-entity ENTITY_NAME`

```shell
$ ros2 run gazebo_ros spawn_entity.py -h
usage: spawn_entity.py [-h] (-file FILE_NAME | -topic TOPIC_NAME | -database ENTITY_NAME | -stdin) -entity ENTITY_NAME
                       [-reference_frame REFERENCE_FRAME] [-gazebo_namespace GAZEBO_NAMESPACE]
                       [-robot_namespace ROBOT_NAMESPACE] [-timeout TIMEOUT] [-unpause] [-wait ENTITY_NAME]
                       [-spawn_service_timeout TIMEOUT] [-x X] [-y Y] [-z Z] [-R R] [-P P] [-Y Y] [-package_to_model] [-b]

Spawn an entity in gazebo. Gazebo must be started with gazebo_ros_init, gazebo_ros_factory and gazebo_ros_state for all
functionalities to work

options:
  -h, --help            show this help message and exit
  -file FILE_NAME       Load entity xml from file
  -topic TOPIC_NAME     Load entity xml published on topic                                                                    -database ENTITY_NAME                                                                                                                             Load entity XML from specified entity in GAZEBO_MODEL_PATH or Gazebo Model Database
  -stdin                Load entity from stdin                                                  
  -entity ENTITY_NAME   Name of entity to spawn                                                 
  -reference_frame REFERENCE_FRAME
                        Name of the model/body where initial pose is defined. If left empty or specified as "world",
                        gazebo world frame is used
  -gazebo_namespace GAZEBO_NAMESPACE
                        ROS namespace of gazebo offered ROS interfaces. Default is without any namespace
  -robot_namespace ROBOT_NAMESPACE
                        change ROS namespace of gazebo-plugins
  -timeout TIMEOUT      Number of seconds to wait for the spawn and delete services to become available
  -unpause              unpause physics after spawning entity
  -wait ENTITY_NAME     Wait for entity to exist
  -spawn_service_timeout TIMEOUT
                        DEPRECATED: Use '-timeout' instead.
  -x X                  x component of initial position, meters
  -y Y                  y component of initial position, meters
  -z Z                  z component of initial position, meters
  -R R                  roll angle of initial orientation, radians
  -P P                  pitch angle of initial orientation, radians
  -Y Y                  yaw angle of initial orientation, radians
  -package_to_model     convert urdf <mesh filename="package://..." to <mesh filename="model://..."
  -b                    bond to gazebo and delete the entity when this program is interrupted
```

`~/ros2_ws/src`に`turtlebot3_simulations_jp_custom`がある環境だと以下でロボットをspawnすることができます。
また、ここでは`-file`オプションでロボットのSDFファイルを指定しています。

```shell
ros2 run gazebo_ros spawn_entity.py -file ~/ros2_ws/src/turtlebot3_simulations_jp_custom/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf -entity robot
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/426354/d869608e-d16a-7454-2dc3-e4e675fbadf4.png)

ほかにも、以下の方法でspawnするオブジェクトを指定できます。
- `-topic`: `robot_description`トピック
- `-database`: GAZEBO_MODELパスが通っているディレクトリ内のオブジェクト
- `-stdin`: 標準入力でXMLを指定

## ROSサービスを使用する
GazeboをROSのライブラリと一緒に起動すると、`/spawn_entity`というROSサービスのサーバも一緒に起動されます。このサービスをcallすることでオブジェクトをspawnすることができます。

サービスのTypeは`gazebo_msgs/srv/SpawnEntity`です。
実は上記のROSノードもこのサービスを使用しているので、指定できるパラメータの中身は実質同一です。

```shell
$ ros2 service type /spawn_entity
gazebo_msgs/srv/SpawnEntity
```

```:gazebo_msgs/srv/SpawnEntity
string name                       # Name of the entity to be spawned (optional).
string xml                        # Entity XML description as a string, either URDF or SDF.
string robot_namespace            # Spawn robot and all ROS interfaces under this namespace
geometry_msgs/Pose initial_pose   # Initial entity pose.
string reference_frame            # initial_pose is defined relative to the frame of this entity.
                                  # If left empty or "world" or "map", then gazebo world frame is
                                  # used.
                                  # If non-existent entity is specified, an error is returned
                                  # and the entity is not spawned.
---
bool success                      # Return true if spawned successfully.
string status_message             # Comments if available.
```

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ros2/gazebo_msgs/srv/SpawnEntity.srv

以下がシンプルなspwanのスクリプトです。

:::note warn
あくまでとてもシンプルな例です。以下の点に気を付けてください。
  - `spawn_client.call_async(req)`の返り値の`Future`を適切に処理した方がよいです
  - nodeを`destroy_node()`で適切に終了した方がよいです
:::

```python:spawn_sample.py
import os

import rclpy
from gazebo_msgs.srv import SpawnEntity
from ament_index_python import get_package_share_directory

rclpy.init()
node = rclpy.create_node("spawn_node")

spawn_client = node.create_client(SpawnEntity, "/spawn_entity")
req = SpawnEntity.Request()
file_path = os.path.join(
    get_package_share_directory("turtlebot3_gazebo"),
    "models",
    "turtlebot3_burger",
    "model.sdf",
)
req.name = "robot"
req.xml = open(file_path, "r").read()
spawn_client.call_async(req)
```


複数台のオブジェクトをspawnする場合は `name`,`robot_namespace`,`initial_pose`をそれぞれのオブジェクトで指定しましょう。

```python:spawn_multi_sample.py
import os

import rclpy
from gazebo_msgs.srv import SpawnEntity
from ament_index_python import get_package_share_directory

rclpy.init()
node = rclpy.create_node("spawn_node")

spawn_client = node.create_client(SpawnEntity, "/spawn_entity")
req = SpawnEntity.Request()
file_path = os.path.join(
    get_package_share_directory("turtlebot3_gazebo"),
    "models",
    "turtlebot3_burger",
    "model.sdf",
)

for i in range(5):
    req.name = f"robot{i}"
    req.xml = open(file_path, "r").read()
    req.robot_namespace = f"robot_{i}"
    req.initial_pose.position.x = 0.0
    req.initial_pose.position.y = float(i)
    spawn_client.call_async(req)
```

以下のように5台spawnすることができました。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/426354/8b43c6ce-5999-f180-9247-34c8305134f7.png)

namespaceも反映されています。

```shell
$ ros2 node list
/gazebo
/robot_0/turtlebot3_diff_drive
/robot_0/turtlebot3_imu
/robot_0/turtlebot3_joint_state
/robot_0/turtlebot3_laserscan
/robot_1/turtlebot3_diff_drive
/robot_1/turtlebot3_imu
/robot_1/turtlebot3_joint_state
/robot_1/turtlebot3_laserscan
/robot_2/turtlebot3_diff_drive
/robot_2/turtlebot3_imu
/robot_2/turtlebot3_joint_state
/robot_2/turtlebot3_laserscan
/robot_3/turtlebot3_diff_drive
/robot_3/turtlebot3_imu
/robot_3/turtlebot3_joint_state
/robot_3/turtlebot3_laserscan
/robot_4/turtlebot3_diff_drive
/robot_4/turtlebot3_imu
/robot_4/turtlebot3_joint_state
/robot_4/turtlebot3_laserscan
```

コマンドラインから実行するためには、以下のコマンドでオブジェクトのXMLを指定する必要があるのですが、直接XMLを記載するとYAMLをパースする際にエラーが出てしまいます。なにか解決方法がわかったら追記します。

```shell
ros2 service call /spawn_entity gazebo_msgs/srv/SpawnEntity "{name: "robot" xml: **XML**}"
```

```
yaml.scanner.ScannerError: while scanning for the next token
found character '>' that cannot start any token
  in "<unicode string>", line 1, column 43:
     ... ple, xml: "<?xml version="1.0" ?><sdf version="1.5"><model name= ...
                                         ^
```

# deleteする方法
## ROSサービスを使用する
deleteする方法は単純で、Gazebo上で管理されている名前を指定してROSサービスをcallします。

こちらもspawnの際と同様、GazeboをROSのライブラリと一緒に起動すると、`/delete_entity`というROSサービスのサーバも一緒に起動されます。このサービスをcallすることでオブジェクトをdeleteすることができます。

サービスのTypeは`gazebo_msgs/srv/SpawnEntity`です。

```shell
$ ros2 service type /delete_entity
gazebo_msgs/srv/DeleteEntity
```

```:gazebo_msgs/srv/DeleteEntity
string name                       # Name of the Gazebo entity to be deleted. This can be either
                                  # a model or a light.
---
bool success                      # Return true if deletion is successful.
string status_message             # Comments if available.
```

ここで指定する名前はGazebo側で管理されている名前です。spawn_entity.pyで指定した`entity`、またはROSサービスで指定した`name`です。
Gazeboの左側のメニューからも確認することができます。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/426354/e0f5b4cd-164d-4de4-c40e-490fbf31a9ca.png)

以下のコマンドで`robot0`のオブジェクトを消すことができます。

```shell
ros2 service call /delete_entity gazebo_msgs/srv/DeleteEntity "{name: robot0}"
```

Pythonだと以下のように書くことができます。

```python:delete_sample.py
import rclpy
from gazebo_msgs.srv import DeleteEntity

rclpy.init()
node = rclpy.create_node("delete_node")

delete_client = node.create_client(DeleteEntity, "/delete_entity")
req = DeleteEntity.Request()
req.name = "robot"
delete_client.call_async(req)
```

# おわりに
Gazebo上にオブジェクトをspwan/deleteする以下の方法をまとめました。
- spawnする方法
    - spawn_entity.pyノードを使用する
    - ROSサービスを使用する
- deleteする方法
    - ROSサービスを使用する

# 参考
今回出てきたTurtlebot3は以下のリポジトリのものです。

https://github.com/ROBOTIS-JAPAN-GIT/turtlebot3_simulations_jp_custom/tree/humble-jp-devel