# Gazebo Helpers in the Artefacts Toolkit

Site [llms.txt](/llms.txt)

---

The Artefacts Toolkit Gazebo Helpers provide convenient functions to interact with your Gazebo simulations while running tests. These utilities allow you to inspect simulation objects, access model positions, as well as bridge topics between Gazebo and ROS2.

Import with:
```python
from artefacts_toolkit.gazebo import bridge, gz
```

## Functions
* [`bridge.get_camera_bridge`](#bridgeget_camera_bridge)
* [`gz.add_entity`](#gzadd_entity)
* [`gz.load_world`](#gzload_world)
* [`gz.get_sim_objects`](#gzget_sim_objects)
* [`gz.get_model_location`](#gzget_model_location)
* [`gz.kill_gazebo`](#gzkill_gazebo)
* [`gz.make_actor`](#gzmake_actor)

## Function Reference

### `bridge.get_camera_bridge`

Creates a gazebo / ros2 topic bridge for a camera topic

```python
bridge.get_camera_bridge(
    topic_name,
    condition=None
)
```

#### Parameters

| **Parameter** | **Type** | **Description**                                                            | **Default** |
| ------------- | -------- | -------------------------------------------------------------------------- | ----------- |
| `topic_name`  | `str`    | The camera topic name to bridge from Gazebo to ROS2                        | Required    |
| `condition`   | `str`    | Optional launch condition that determins when this bridge should be created| `None`      |

#### Returns

`Node`: Returns a ROS 2 Node object that runs the parameter_bridge for the specified camera topic. This node can be included in your launch description.

> [!NOTE]
> * The bridge converts between Gazebo's image messages (gz.msgs.Image) and ROS 2's image messages (sensor_msgs/msg/Image)
> * You need to have the ros_gz_bridge package installed in your environment

#### Example

The following example shows a camera bridge being created conditionally based on a launch argument. When `record_video` is set to "true", the bridge will be activated, allowing ROS 2 nodes to subscribe to camera images from Gazebo. The returned `Node` is added to the LaunchDescription to be included in the launch process.

```python
@pytest.mark.launch_test
def generate_test_description():
    from artefacts_toolkit.gazebo import bridge
    ...

    camera_topic = "/observation_camera/image"
    bag_recorder, rosbag_filepath = get_bag_recorder([camera_topic])
    sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(["bringup", "/robot.launch.py"])
    )

    record_video_launch_arg = DeclareLaunchArgument(
        "record_video", default_value="true"
    )
    record_video = LaunchConfiguration("record_video")
    camera_bridge = bridge.get_camera_bridge(camera_topic, condition=IfCondition(record_video))
    
    return LaunchDescription(
        [
            record_video_launch_arg,
            sim,
            camera_bridge,
            controller_process,
            launch_testing.actions.ReadyToTest(),
        ]
    )
```

---

### `gz.add_entity`

Inserts an entity XML element into a Gazebo world XML structure. This is an in-place operation that modifies the provided world XML directly.

```python
gz.add_entity(world_xml, entity_xml)
```

#### Parameters

| **Parameter** | **Type** | **Description**                                                            | **Default** |
| ------------- | -------- | -------------------------------------------------------------------------- | ----------- |
| `world_xml`   | `xml.etree.ElementTree.Element` | The world XML structure in `<sdf>...</sdf>` format | Required    |
| `entity_xml`  | `xml.etree.ElementTree.Element` | The entity XML element to insert into the world   | Required    |

#### Returns

`None`: This function modifies `world_xml` in-place and doesn't return any value.

#### Example

The following example shows how to add an actor to an existing world file. After loading the world file with `gz.load_world`, an actor is created and then inserted into the world XML structure.

```python
from artefacts_toolkit.gazebo import gz

# Load existing world file
world_xml = gz.load_world("worlds/env.sdf")

# Create an actor with waypoints
waypoints = [
    [4, -3.2, 1.0],
    [0.8, -3.2, 1.0],
    [0.8, -6, 1.0],
    [4, -6, 1.0],
]
actor_xml = gz.make_actor("pedestrian_1", waypoints)

# Add the actor to the world
gz.add_entity(world_xml, actor_xml)
```

---

### `gz.load_world`

Loads and parses a Gazebo world file, returning the root XML element.

```python
gz.load_world(world_file)
```

#### Parameters

| **Parameter** | **Type** | **Description**                                                            | **Default** |
| ------------- | -------- | -------------------------------------------------------------------------- | ----------- |
| `world_file`  | `str`    | Path to the Gazebo world file to load                                      | Required    |

#### Returns

`xml.etree.ElementTree.Element`: Returns the root XML element of the parsed world file in `<sdf>...</sdf>` format.

#### Raises

`FileNotFoundError`: If the specified world file does not exist.

#### Example

```python
from artefacts_toolkit.gazebo import gz

# Load a world file
world_xml = gz.load_world("worlds/env.sdf")

# Now you can modify the world and add entities
actor_xml = gz.make_actor("pedestrian", [[0, 0, 0], [1, 1, 0]])
gz.add_entity(world_xml, actor_xml)
```

---

### `gz.get_sim_objects`

Extracts model information from a Gazebo world file by parsing its XML structure. This function returns the name and original pose of all models defined in the world file.

This can useful when you need to know the initial poses of models as defined in the world file, rather than querying the running simulation (where positions might have changed due to physics, randomness, or interactions).

```python
gz.get_sim_objects(world_file)
```

#### Parameters

| **Parameter** | **Type** | **Description**                                                            | **Default** |
| ------------- | -------- | -------------------------------------------------------------------------- | ----------- |
| `world_file`  | `str`    | Path to the Gazebo world file to parse                                     | Required    |

#### Returns

`tuple`: The function returns a tuple with two values that should be unpacked:

* A list of dictionaries: 
```python
objects = [
    {
        "name": "model_1",
        "pose": "0 0 0 0 0 0"
    }, 
    {
        "name": "model_2",
        "pose": "1 2 3 0 0 0"
    },
    ...
]
```

* A dictionary mapping model names to poses: 

```python
objects_positions = {
    "model_1": "0 0 0 0 0 0", 
    "model_2": "1 2 3 0 0 0",
    ...
}
```

#### Example

```python
# Working with the objects_position dict
_, objects_positions = gz.get_sim_objects("world.sdf")
model_pose = objects_positions["model_1"]  # "0 0 0 0 0 0"

# Working with the objects list
objects, _ = gz.get_sim_objects("world.sdf")
for model in objects:
    print(f"{model['name']} is at position {model['pose']}")
```

---

### `gz.get_model_location`

Gets the current (x, y, z) position of a model in a running Gazebo simulation. Unlike `get_sim_objects` which reads original positions from a world file, this function queries the live simulation to get the model's current position.

```python
gz.get_model_location(model_name)
```

#### Parameters

| **Parameter** | **Type** | **Description**                                                            | **Default** |
| ------------- | -------- | -------------------------------------------------------------------------- | ----------- |
| `model_name`  | `str`    | Name of the model to query in the simulation                               | Required    |

#### Returns

`tuple`: Returns a tuple of three float values representing the (x, y, z) position in meters:
```python
(x_position, y_position, z_position)
```

> [!NOTE]
> * This function requires a Gazebo simulation to be running
> * Returns (0.0, 0.0, 0.0) if the model position cannot be determined

#### Example

The example below demonstrates how to combine `get_sim_objects` and `get_model_location` to validate a pick-and-place task. It shows how to:

1. Get initial positions from the world file
2. Wait for a robot to complete its task
3. Compare initial positions with current positions to detect which objects were moved

```python
class TestProcOutput(unittest.TestCase):
    def test_moved_meatballs(self, proc_output, controller_process):
        # Original locations
        sim_objects, sim_objects_positions = gz.get_sim_objects("worlds/env.sdf") # this includes models poses
        meatball_models = [obj["name"] for obj in sim_objects if "karaage" in obj["name"]]
        #  Wait for the control loop to finish
        proc_output.assertWaitFor("Done with tasks execution", timeout=300)
        picked_meatballs = 0 # karaage moved for more than 10cm
        for meatball in meatball_models:
            x, y, z = gz.get_model_location(meatball)
            x_original, y_original, z_original = sim_objects_positions[meatball]
            dist = ((x - x_original) ** 2 + (y - y_original) ** 2 + (z - z_original) ** 2) ** 0.5
            if dist > 0.1: #10cm
                # Likely to have been picked and moved somewhere else
                picked_meatballs += 1
        self.assertEqual(picked_meatballs, 4)
```

---

### `gz.kill_gazebo`

Kills the currently running gazebo process.

```python
gz.kill_gazebo()
```

#### Returns

`None`: This function doesn't return any value.

#### Example

```python
from artefacts_toolkit.gazebo import gz
...

@launch_testing.post_shutdown_test()
class TestProcOutputAfterShutdown(unittest.TestCase):
    def test_exit_code(self, proc_info, controller_process, rosbag_filepath):
        gz.kill_gazebo()
        ...
```

---

### `gz.make_actor`

Creates the necessary XML to add in an actor to your simulation. A path for the actor to move will be created based on provided waypoints, walking speed, and speed of rotation.

```python
gz.make_actor(
    actor_name,
    waypoints,
    walk_speed=0.8,
    rotate_speed=1.8,
    enable_loop=True,
    output_type="xml"
)
```

#### Parameters

| **Parameter** | **Type** | **Description**                                                            | **Default** |
| ------------- | -------- | -------------------------------------------------------------------------- | ----------- |
| `actor_name`  | `str`    | The name to call your actor. Noted in xml as `<actor name="{actor_name}">` | Required    |
| `waypoints`   | `list`   | List of waypoint coordinates. Can be either `[[x,y,z], ...]` for position only, or `[[x,y,z,roll,pitch,yaw], ...]` for position and rotation | Required    |
| `walk_speed`  | `float`  | Walking speed of the actor in meters per second                            | `0.8`       |
| `rotate_speed`| `float`  | Rotation speed of the actor in radians per second                          | `1.8`       |
| `enable_loop` | `bool`   | Whether the actor should continuously repeat the waypoint path             | `True`      |
| `output_type` | `str`    | Whether to return as `xml` object or `str` string                          | `"xml"`     |

#### Returns

`output_type` can be set to `"xml"` or `"str"`:

* `xml`: Returns an XML object representation of a Gazebo actor (`xml.etree.ElementTree.Element`)
* `str` : Returns an XML string representation of a Gazebo actor

```xml
<actor name="{actor_name}">
    <skin>
        <filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
    </skin>
    
    <animation name="walking">
        <filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
        <interpolate_x>true</interpolate_x>
    </animation>

    <script>
        <loop>true / false</loop>
        <delay_start>0.0</delay_start>
        <auto_start>true</auto_start>
        
        <trajectory id="0" type="walking">
            <!-- The generated waypoints given the provided inputs -->
        </trajectory>
    </script>
</actor>
```

#### Example

Given the following waypoints:
```python
waypoints = [
    [4, -3.2, 1.0],
    [0.8, -3.2, 1.0],
    [0.8, -6, 1.0],
    [4, -6, 1.0],
]
```
```python
actor_xml = gz.make_actor("my_actor", waypoints, walk_speed=0.8, rotate_speed=1.8, enable_loop=True)
```
will return the following xml:

```xml
<actor name="my_actor">
    <skin>
        <filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
    </skin>
    
    <animation name="walking">
        <filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
        <interpolate_x>true</interpolate_x>
    </animation>

    <script>
        <loop>true</loop>
        <delay_start>0.0</delay_start>
        <auto_start>true</auto_start>
        
        <trajectory id="0" type="walking">
            <waypoint>
                <time>0</time>
                <pose>4 -3.2 1.0 0 0 3.141592653589793</pose>
            </waypoint>
            <waypoint>
                <time>4.0</time>
                <pose>0.8 -3.2 1.0 0 0 3.141592653589793</pose>
            </waypoint>
            <waypoint>
                <time>6.617993877991495</time>
                <pose>0.8 -3.2 1.0 0 0 -1.5707963267948966</pose>
            </waypoint>
            <waypoint>
                <time>10.117993877991495</time>
                <pose>0.8 -6 1.0 0 0 -1.5707963267948966</pose>
            </waypoint>
            <waypoint>
                <time>10.99065850398866</time>
                <pose>0.8 -6 1.0 0 0 0.0</pose>
            </waypoint>
            <waypoint>
                <time>14.99065850398866</time>
                <pose>4 -6 1.0 0 0 0.0</pose>
            </waypoint>
            <waypoint>
                <time>15.863323129985826</time>
                <pose>4 -6 1.0 0 0 1.5707963267948966</pose>
            </waypoint>
            <waypoint>
                <time>19.363323129985826</time>
                <pose>4 -3.2 1.0 0 0 1.5707963267948966</pose>
            </waypoint>
            <waypoint>
                <time>20.23598775598299</time>
                <pose>4 -3.2 1.0 0 0 3.141592653589793</pose>
            </waypoint>
            
        </trajectory>
    </script>
</actor>
```