# Navigation Helpers in the Artefacts Toolkit

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

---

The Artefacts Toolkit Navigation Helpers provide convenient functions, as well as a CLI tool, for automated navigation tests. Record a route directly in RViz, generate a simulator world from its 2D map, then load and drive the route in your tests.

Import with:

```python
from artefacts_toolkit.navigation import <function>
```

> [!NOTE]
> * The navigation helpers are currently heavily dependent on Nav2 and have been tested on ROS 2 Humble (Python 3.10) and Jazzy (Python 3.12). `ros-<distro>-nav2-simple-commander` and `ros-<distro>-tf2-ros` are required.
> * Routes are recorded with the `artefacts-route` command from `artefacts-route-recorder`, which is included with the toolkit. See [Recording a route](#recording-a-route).

## Recording a route

A route is a YAML file holding the 2D Pose Estimate it was recorded from (`initial_pose`), the waypoints, and which map it was recorded on. You will need both the map `yaml` and `pgm` files. Record one from the directory you will run your tests from:

```bash
artefacts-route record [save/to/route.yaml] --map <map/to/load.yaml>
```

A Nav2 `map_server` with your map plus RViz will be launched. Set the start with **2D Pose Estimate**, then click the waypoints with **Publish Point**. Close RViz or press enter in the terminal to save the route to `routes/<date>_<time>.yaml`, or pass a path (`[save/to/route.yaml]`) to save to a specific location.

| **Option**          | **Description**                                                                                  |
| ------------------- | ------------------------------------------------------------------------------------------------ |
| `FILE`              | Where to save the route (letters, digits, `-` and `_`). Default: `./routes/<date>_<time>.yaml`  |
| `--map MAP_YAML`    | Launch `map_server` with this map plus RViz, and record the map reference                        |
| `--map-ref MAP_YAML`| Record this map's path and hash without launching anything (RViz and the map are already up) |
| `--force`           | Overwrite `FILE` if it exists                                                                    |

Other subcommands: 

* `artefacts-route list`: list saved routes.
* `artefacts-route show`: print a route and any problems with it.
* `artefacts-route replay`: publishes a route as RViz markers.

## Functions

Launch file helpers:
* [`load_route`](#load_route)
* [`route_launch_args`](#route_launch_args)
* [`route_start_pose`](#route_start_pose)
* [`check_map_match`](#check_map_match)
* [`map_to_world`](#map_to_world)

Test helpers:
* [`follow_route`](#follow_route)
* [`assert_route_completed`](#assert_route_completed)
* [`assert_each_waypoint_reached`](#assert_each_waypoint_reached)
* [`assert_final_pose`](#assert_final_pose)
* [`assert_min_clearance`](#assert_min_clearance)
* [`assert_max_recoveries`](#assert_max_recoveries)

## Function Reference

### `load_route`

Loads a route recorded with `artefacts-route record`.

```python
load_route(
    route=None
)
```

#### Parameters

| **Parameter** | **Type**                     | **Description**                                                                                      | **Default** |
| ------------- | ---------------------------- | ---------------------------------------------------------------------------------------------------- | ----------- |
| `route`       | `str` or `Path` | Path to a route file. `None` loads the most recently recorded route from `./routes/` | `None`      |

#### Returns

`Route`: The loaded route, with `name`, `frame_id`, `initial_pose`, `waypoints` and `map` attributes.

> [!NOTE]
> * Raises `UserError` if there is no such route or its file cannot be read.

#### Example

```python
from artefacts_toolkit.navigation import load_route

route = load_route()                     # the most recently recorded route
route = load_route("routes/lab.yaml")    # a specific route
```

---

### `route_launch_args`

Returns the launch arguments that spawn the robot at the route's start.

```python
route_launch_args(
    route,
    x=None,
    y=None,
    yaw=None,
    names=("x_pose", "y_pose", "yaw"),
    map_arg="map"
)
```

#### Parameters

| **Parameter** | **Type**        | **Description**                                                                       | **Default**                    |
| ------------- | --------------- | ------------------------------------------------------------------------------------- | ------------------------------ |
| `route`       | `Route`         | The route, from [`load_route`](#load_route)                                           | Required                       |
| `x`           | `float`         | Spawn the robot at this x instead of the recorded start                               | `None`                         |
| `y`           | `float`         | Spawn the robot at this y instead of the recorded start                               | `None`                         |
| `yaw`         | `float`         | Spawn the robot facing this heading (radians) instead of the recorded start           | `None`                         |
| `names`       | `Sequence[str]` | The names your launch file declares for the spawn pose arguments, if different from default | `("x_pose", "y_pose", "yaw")`  |
| `map_arg`     | `str` or `None` | Name of the map launch argument. `None` leaves the map out                            | `"map"`                        |

#### Returns

`dict[str, str]`: For example `{"x_pose": "0.59", "y_pose": "-1.45", "yaw": "1.5708", "map": "/maps/lab.yaml"}`.

#### Example

```python
from artefacts_toolkit.navigation import load_route, route_launch_args

route = load_route()
args = route_launch_args(route)
IncludeLaunchDescription(my_sim_launch, launch_arguments=args.items())
```

---

### `route_start_pose`

Returns the route's start pose (the 2D Pose Estimate it was recorded from), optionally overriding any of its components.

```python
route_start_pose(
    route,
    x=None,
    y=None,
    yaw=None
)
```

#### Parameters

| **Parameter** | **Type** | **Description**                           | **Default** |
| ------------- | -------- | ----------------------------------------- | ----------- |
| `route`       | `Route`  | The route                                 | Required    |
| `x`           | `float`  | Override the start x                      | `None`      |
| `y`           | `float`  | Override the start y                      | `None`      |
| `yaw`         | `float`  | Override the start yaw (radians)          | `None`      |

#### Returns

`Pose2D`: The start pose with `x`, `y` and `yaw` attributes.

> [!NOTE]
> * If the route was recorded without a 2D Pose Estimate, `x`, `y` and `yaw` must all be given, otherwise `UserError` is raised.

---

### `check_map_match`

Raises `UserError` if `map_yaml` is not the map the route was recorded on. Useful to ensure a route is never driven on the wrong map.

```python
check_map_match(
    route,
    map_yaml,
    ignore_mismatch=False
)
```

#### Parameters

| **Parameter**     | **Type**        | **Description**                                           | **Default** |
| ----------------- | --------------- | --------------------------------------------------------- | ----------- |
| `route`           | `Route`         | The route                                                 | Required    |
| `map_yaml`        | `str` or `Path` | The map your launch file loads                            | Required    |
| `ignore_mismatch` | `bool`          | Warn instead of raising when the maps differ              | `False`     |

#### Returns

`list[str]`: Warnings, such as the route having no map hash to compare against, or the mismatch being ignored.

> [!NOTE]
> * Only needed when your launch file does not take the map from the route (see [`route_launch_args`](#route_launch_args)).

---

### `map_to_world`

Writes a simulator world extruded up from a Nav2 map (the `yaml` plus `pgm`, e.g. saved from the real robot). Compatible with Isaac Sim, Newton, MuJoCo, and Gazebo.
Creating the world is relatively cheap (milliseconds) and so it can be generated at runtime each time.

```python
map_to_world(
    map_yaml,
    out=None,
    format=None,
    wall_height=1.0,
    fill_gaps=True,
    crop=None
)
```

#### Parameters

| **Parameter** | **Type**                | **Description**                                                                                                              | **Default**                  |
| ------------- | ----------------------- | ---------------------------------------------------------------------------------------------------------------------------- | ---------------------------- |
| `map_yaml`    | `str` or `Path`         | The Nav2 map yaml. (The `pgm` needs to be in the same location as well.)                                                                                                            | Required                     |
| `out`         | `str` or `Path`         | Where to write the world, if required.                                                                                                     | `worlds/<map name>.<ext>`    |
| `format`      | `str`                   | `"usda"` (Isaac Sim, Newton), `"mjcf"` (MuJoCo, Newton) or `"sdf"` (Gazebo). Taken from the extension of `out` when given  | `"usda"`                     |
| `wall_height` | `float`                 | Wall height in metres                                                                                                        | `1.0`                        |
| `fill_gaps`   | `bool`                  | Fill gaps of up to two cells between occupied cells (scan noise) before extruding                                            | `True`                       |
| `crop`        | `tuple[Route, float]`   | `(route, metres)`: keep only the walls within that many metres of the way the robot will go along the route. Reduces physics calculations for large maps.                 | `None`                       |

#### Returns

`str`: The absolute path of the world file written, to hand to the simulator.

> [!NOTE]
> * `usda` and `mjcf` worlds hold the ground and walls only, the robot and its sensors being the simulator's. `sdf` is a complete Gazebo world.
> * Generating a world takes milliseconds, so do it at runtime in your launch file and treat the file as disposable.
> * The world is only good for driving around in, it is not designed to be "nice".

#### Example

The following launch file loads the route recorded last, extrudes a Gazebo world from the route's map, and includes Nav2's TurtleBot3 demo in that world with the robot at the route's start:

```python
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration

from artefacts_toolkit.navigation import load_route, map_to_world, route_launch_args


def include_demo(context):
    route = load_route()  # the route recorded last
    headless = LaunchConfiguration("headless").perform(context)

    # x_pose, y_pose, yaw and map, named as the demo names them.
    args = route_launch_args(route)
    args["headless"] = headless
    args["use_rviz"] = str(headless.lower() not in ("true", "1", "yes"))
    # The world: the route's map, extruded into walls (milliseconds), so the
    # simulation matches the map the route was recorded on.
    args["world"] = map_to_world(route.map.file, format="sdf")

    demo = os.path.join(
        get_package_share_directory("nav2_bringup"), "launch", "tb3_simulation_launch.py")
    return [IncludeLaunchDescription(
        PythonLaunchDescriptionSource(demo), launch_arguments=args.items())]


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument(
            "headless", default_value="True", description="no Gazebo GUI and no RViz"),
        OpaqueFunction(function=include_demo),
    ])
```

---

### `follow_route`

Drives a route through the running Nav2 stack and returns a [`RouteResult`](#routeresult).
Use in your test file.

```python
follow_route(
    route=None,
    timeout_s=300.0,
    startup_timeout_s=60.0,
    label=None,
    results_dir=None,
    set_initial_pose=True,
    base_frame="base_link",
    scan_topic="scan"
)
```

#### Parameters

| **Parameter**       | **Type**                     | **Description**                                                                                        | **Default**     |
| ------------------- | ---------------------------- | ------------------------------------------------------------------------------------------------------ | --------------- |
| `route`             | `str` or `Path`              | Path to the route to drive, as for [`load_route`](#load_route). `None` drives the route recorded last | `None`          |
| `timeout_s`         | `float` or `None`            | After this many seconds the goal is cancelled and the outcome is `"timeout"`. `None` waits forever     | `300.0`         |
| `startup_timeout_s` | `float`                      | How long to wait for Nav2 to become active before raising `EnvError`                                   | `60.0`          |
| `label`             | `str`                        | A label added to the result and its file name                                                          | `None`          |
| `results_dir`       | `str` or `Path`              | Directory to also write the result to, as a YAML file                                                  | `None`          |
| `set_initial_pose`  | `bool`                       | Tell AMCL the robot is at the route's start before driving. `False` on a robot that is already localised | `True`        |
| `base_frame`        | `str`                        | The robot frame, for the final pose error                                                              | `"base_link"`   |
| `scan_topic`        | `str` or `None`              | The `LaserScan` topic to measure clearance from. `None` to skip                                        | `"scan"`        |

#### Returns

[`RouteResult`](#routeresult): What happened. A failed run is a result, not an exception.

> [!NOTE]
> * Nav2 must already be up. In order, `follow_route` waits for `amcl` and `bt_navigator` to be active, publishes the route's initial pose and waits for AMCL to take it, sends the waypoints as one `FollowWaypoints` goal, waits for it to end, and reads the robot's pose from tf for the final pose error.
> * Clearance is measured from the lidar, not from the robot's edge.
> * Raises `UserError` for a bad route, and `EnvError` when ROS is not sourced, Nav2's Python API is missing, or no stack shows up.
> * Setting `results_dir` to the `ARTEFACTS_SCENARIO_UPLOAD_DIR` environment variable uploads the result YAML to the Artefacts Dashboard when the scenario finishes.

#### Example

The following test runs the launch file from the [`map_to_world`](#map_to_world) example in a fixture, drives the route recorded last, and asserts on the result:

```python
import os
import signal
import subprocess
import time
from pathlib import Path

import pytest

from artefacts_toolkit.navigation import (
    assert_each_waypoint_reached, assert_final_pose, assert_max_recoveries,
    assert_min_clearance, follow_route)

HEADLESS = os.environ.get("HEADLESS", "false").lower() not in ("0", "false", "no")
RESULTS_DIR = os.environ.get("ARTEFACTS_SCENARIO_UPLOAD_DIR", "results")
LAUNCH_FILE = Path(__file__).with_name("nav2_sim.launch.py")


@pytest.fixture
def sim(tmp_path):
    """The TurtleBot3 demo, up for one test and shut down after it."""
    command = ["ros2", "launch", str(LAUNCH_FILE), "headless:={}".format(HEADLESS)]
    with (tmp_path / "sim.log").open("w") as out:
        proc = subprocess.Popen(command, stdout=out, stderr=subprocess.STDOUT,
                                start_new_session=True)
    time.sleep(15)  # let Gazebo and Nav2 come up
    try:
        yield
    finally:
        os.killpg(proc.pid, signal.SIGINT)  # ros2 launch shuts its processes down in order
        proc.wait(timeout=30)


def test_route_is_completed(sim):
    result = follow_route(timeout_s=300, results_dir=RESULTS_DIR)  # the route recorded last
    assert_each_waypoint_reached(result, tolerance_m=0.5)
    assert_final_pose(result, max_pos_error_m=0.5)
    assert_min_clearance(result, 0.15)  # from the lidar: the TurtleBot3's is near its middle
    assert_max_recoveries(result, 2)
```

---

> [!NOTE]
> The `assert_*` helpers check a [`RouteResult`](#routeresult), which only [`follow_route`](#follow_route) produces. They rely on the toolkit having driven the route itself and so cannot be used if you are driving the robot in another way.
>
> Each helper raises an `AssertionError` on a failed assertion.

### `assert_route_completed`

Raises `AssertionError` unless Nav2 finished the route and skipped no waypoint.

```python
assert_route_completed(
    result
)
```

#### Parameters

| **Parameter** | **Type**      | **Description**                        | **Default** |
| ------------- | ------------- | -------------------------------------- | ----------- |
| `result`      | `RouteResult` | The result from [`follow_route`](#follow_route) | Required |

---

### `assert_each_waypoint_reached`

Raises `AssertionError` unless every waypoint was reached, and within `tolerance_m` of it when given.

```python
assert_each_waypoint_reached(
    result,
    tolerance_m=None
)
```

#### Parameters

| **Parameter** | **Type**      | **Description**                                            | **Default** |
| ------------- | ------------- | ---------------------------------------------------------- | ----------- |
| `result`      | `RouteResult` | The result from [`follow_route`](#follow_route)            | Required    |
| `tolerance_m` | `float`       | How close the robot must have come to each waypoint        | `None`      |

> [!NOTE]
> * A tolerance looser than Nav2's `xy_goal_tolerance` will almost never fail.

---

### `assert_final_pose`

Raises `AssertionError` unless the robot ended close enough to the last waypoint.

```python
assert_final_pose(
    result,
    max_pos_error_m=0.5,
    max_yaw_error_rad=None
)
```

#### Parameters

| **Parameter**       | **Type**      | **Description**                                      | **Default** |
| ------------------- | ------------- | ---------------------------------------------------- | ----------- |
| `result`            | `RouteResult` | The result from [`follow_route`](#follow_route)      | Required    |
| `max_pos_error_m`   | `float`       | Maximum distance from the last waypoint              | `0.5`       |
| `max_yaw_error_rad` | `float`       | Maximum yaw difference from the last waypoint        | `None`      |

---

### `assert_min_clearance`

Raises `AssertionError` if any laser return was ever nearer than `min_m`.

```python
assert_min_clearance(
    result,
    min_m
)
```

#### Parameters

| **Parameter** | **Type**      | **Description**                                    | **Default** |
| ------------- | ------------- | -------------------------------------------------- | ----------- |
| `result`      | `RouteResult` | The result from [`follow_route`](#follow_route)    | Required    |
| `min_m`       | `float`       | Minimum allowed distance to an obstacle, in metres | Required    |

> [!NOTE]
> * Measured from the lidar, so add the distance from the lidar to the robot's edge.

---

### `assert_max_recoveries`

Raises `AssertionError` if Nav2 ran more than `max_recoveries` recovery behaviours (spin, back up, wait, ...).

```python
assert_max_recoveries(
    result,
    max_recoveries=0
)
```

#### Parameters

| **Parameter**    | **Type**      | **Description**                                    | **Default** |
| ---------------- | ------------- | -------------------------------------------------- | ----------- |
| `result`         | `RouteResult` | The result from [`follow_route`](#follow_route)    | Required    |
| `max_recoveries` | `int`         | Maximum number of recovery behaviours allowed      | `0`         |

## `RouteResult`

What one [`follow_route`](#follow_route) run produced.

| **Attribute**         | **Type**                | **Description**                                                                         |
| --------------------- | ----------------------- | --------------------------------------------------------------------------------------- |
| `succeeded`           | `bool`                  | Nav2 finished the route and skipped no waypoint.                   |
| `outcome`             | `str`                   | `"succeeded"`, `"failed"`, `"canceled"` or `"timeout"`                                  |
| `error`               | `str` or `None`         | Error message when outcome is not `"succeeded"`                                                    |
| `duration_s`          | `float`                 | Wall-clock seconds from sending the route to its end                                    |
| `waypoints_total`     | `int`                   | Number of waypoints in the route                                                        |
| `missed_waypoints`    | `list[int]`             | Indices of the waypoints Nav2 skipped                                                   |
| `waypoints`           | `list[WaypointResult]`  | How each waypoint went (see below)                                                      |
| `final_pos_error_m`   | `float` or `None`       | Distance from the robot to the last waypoint at the end, from tf                        |
| `final_yaw_error_rad` | `float` or `None`       | Yaw difference from the last waypoint at the end                                        |
| `recoveries`          | `int`                   | Recovery behaviours Nav2 ran over the whole route                                       |
| `min_clearance_m`     | `float` or `None`       | Nearest laser return over the whole route                                               |
| `warnings`            | `list[str]`             | Anything that could not be measured, and why                                            |

Each `WaypointResult` in `result.waypoints` has:

| **Attribute**        | **Type**          | **Description**                                              |
| -------------------- | ----------------- | ------------------------------------------------------------ |
| `index`              | `int`             | The waypoint's index in the route                            |
| `reached`            | `bool`            | Whether Nav2 reached it                                      |
| `time_s`             | `float` or `None` | Seconds from the start of the route to reaching it           |
| `closest_approach_m` | `float` or `None` | Nearest the robot came to the waypoint on its way there      |
| `recoveries`         | `int`             | Recovery behaviours Nav2 ran on the way there                |
| `min_clearance_m`    | `float` or `None` | Nearest laser return on the way there                        |

### Methods

* `result.metrics()`: A flat `dict` of the run's numbers (`duration_s`, `waypoints_reached`, `waypoints_total`, `final_pos_error_m`, `final_yaw_error_rad`, `recoveries`, `min_clearance_m`), ready to write to an Artefacts `metrics.json`.
* `result.save(results_dir)`: Writes the result as `<results_dir>/<route>_<YYYYmmdd-HHMMSS>[_<label>].yaml` and returns the path.
* `result.summary()`: A one-line description of the run.

### Example

```python
import json

result = follow_route(timeout_s=300)
assert result.succeeded, result.summary()

with open("output/metrics.json", "w") as f:
    json.dump(result.metrics(), f)
```