Metadata-Version: 2.4
Name: artefacts_toolkit_navigation
Version: 0.2.1
Summary: Helpers for automated navigation tests: load a recorded route in a launch file or test, and drive it through Nav2.
Author-email: TN <tomo@artefacts.com>
Project-URL: Homepage, https://github.com/art-e-fact/artefacts-toolkit-navigation
Project-URL: Bug Tracker, https://github.com/art-e-fact/artefacts-toolkit-navigation/issues
Classifier: Programming Language :: Python :: 3
Classifier: License :: OSI Approved :: Apache Software License
Classifier: Operating System :: POSIX :: Linux
Requires-Python: >=3.10
Description-Content-Type: text/markdown
Requires-Dist: artefacts_route_recorder<0.3.0,>=0.2.0
Requires-Dist: PyYAML>=5.1
Provides-Extra: dev
Requires-Dist: build; extra == "dev"
Requires-Dist: pytest; extra == "dev"
Requires-Dist: setuptools-scm; extra == "dev"
Requires-Dist: twine; extra == "dev"

# Artefacts Toolkit (Navigation)

Helpers for automated navigation tests, around the routes you record by
clicking in RViz with `artefacts-route` from
[artefacts-route-recorder](https://github.com/art-e-fact/artefacts-route-recorder)
(installed with this package). Load a route in your launch file to bring the
simulator up at its start, and in your test to drive it through Nav2 and check
what happened.

Works on ROS 2 Humble (Python 3.10) and Jazzy (Python 3.12).

## Installation

```bash
source /opt/ros/<ros-distro>/setup.bash         # or zsh
python3 -m venv --system-site-packages venv   # if using a venv
source venv/bin/activate # if using
pip install artefacts-toolkit-navigation
export PYTHONPATH=~/path-to/my-project/venv/lib/python3.12/site-packages:$PYTHONPATH #if using a venv
```

## Usage

Record a route, from the directory you will test from:

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

In your launch file, spawn the robot where the route starts, on the route's map:

```python
from artefacts_toolkit_navigation import load_route, route_launch_args

# the route recorded last; or load_route("path/to/route.yaml") for a specific one
route = load_route()
args = route_launch_args(route)    # {"x_pose": "0.59", "y_pose": "-1.45", "yaw": "1.57", "map": "path/to/map.yaml"}
IncludeLaunchDescription(my_sim_launch, launch_arguments=args.items())
```

In your test, drive it and assert:

```python
from artefacts_toolkit_navigation import (
    assert_each_waypoint_reached, assert_final_pose, assert_min_clearance, follow_route)

def test_route_is_completed():
    result = follow_route(timeout_s=300)
    assert_each_waypoint_reached(result, tolerance_m=0.3)
    assert_final_pose(result, max_pos_error_m=0.5)
    assert_min_clearance(result, 0.25)
```

`follow_route()` expects Nav2 to be running already (your launch file's job),
so the same test works on a real robot and in any simulator.

## The helpers

**`load_route(route)`** : returns a recorded route.

If `route` is not passed, the last recorded route will be loaded. Alternatively a path to a route e.g `load_route("routes/lab.yaml")` can be passed.

**`route_launch_args(route, names)`** : returns the launch arguments that spawn the robot where the route starts, on the route's map.

If your launch file uses naming other than `x_pose`, `y_pose`, `yaw`, override with `names=()`

**`route_start_pose(route, x, y, yaw)`** : Used to override or create the start 2d pose for a given `route`. E.g. `route_start_pose(route, x=1.0)`

Required if no starting pose is set when using `artefacts-route record`

**`check_map_match(route, map_yaml)`** : raises if `map_yaml` is not the map the
route was recorded on. Only needed when your launch file does not take the map
from the route.

**`map_to_world(map_yaml, out, format, wall_height, fill_gaps, crop)`** : writes a simulator
world extruded up from a map the robot made (a Nav2 map yaml plus pgm, e.g.
saved from the real robot), so a route recorded on that map can be driven in
simulation, and returns the path of the file written. Use it at runtime in
your launch file (milliseconds to create) and treat the file as disposable.

- `format`: `usda` (default; Isaac Sim, Newton) and `mjcf` (MuJoCo, Newton)
  hold the ground and walls only, the robot and its lidar being the
  simulator's; both load in Newton with `ModelBuilder.add_usd()` /
  `add_mjcf()` (Newton 1.6, installed as `pip install 'newton[examples]'`,
  which pins its MuJoCo backend). `sdf` is a complete Gazebo world.
- `out`: `worlds/<map name>.<ext>` by default. `wall_height`: 1 m by default.
- `fill_gaps`: fills gaps of up to two cells between occupied cells. Default is `true`
- `crop=(route, metres)`: keeps only the walls within that many metres of the provided route.

**`follow_route(route, timeout_s, startup_timeout_s, label, results_dir, set_initial_pose, base_frame, scan_topic)`**

Drives the route through the running Nav2 stack and returns a `RouteResult`.

Assert on `result.succeeded`: Nav2 finished the route and skipped no waypoint

- `timeout_s`: After this many seconds the goal is cancelled.
- `results_dir`: Directory to write each result to, as a YAML file
- `set_initial_pose`: Publish the route's start on `initialpose` until tf shows the robot there. `False` on a robot that is already localised.
- `startup_timeout_s`: How long to wait for the robot to be localised and for Nav2.
- `base_frame`: The robot frame, for localisation and the final pose error.
- `scan_topic`: The LaserScan to measure clearance from (`"scan"`); `None` to skip.

Results of each waypoint are recorded and stored in `result.waypoints`:
- `reached`
- `time_s`: time taken from start of route to the given waypoint
- `closest_approach_m`: nearest the robot came to the waypoint on its way there.
- `recoveries`
- `min_clearance_m`: Distance to closest obstacle while travelling to waypoint.

`result.recoveries` are the totals for the route. `result.min_clearance_m` is the smallest value over the entire route. 

**`result.metrics()`** : A dict of metrics (duration,
waypoints reached, final pose error, recoveries, clearance), that can be then used by the user
to add to metrics.json as required.

### Your own localisation

`follow_route()` does not need amcl. All it asks of a localisation is what
Nav2 asks: the `map -> base_link` transform (the route's frame to
`base_frame`). Before driving it waits, up to `startup_timeout_s`, for that
transform to show the robot at the route's start, publishing the start on
`initialpose` meanwhile when `set_initial_pose=True`. So:

- **amcl**: the default works.
- **A localisation that takes RViz's 2D Pose Estimate** (`initialpose`): the
  default works too.
- **A localisation that localises on its own, or takes its start another
  way** (a service, a parameter, a different topic): do that in your launch
  file or test, then `follow_route(set_initial_pose=False)`. It still waits
  for the transform, and warns in `result.warnings` if the robot is not at
  the route's start.

## Ready-made checks

Import them into your test and call them on the `RouteResult`. Each raises an
AssertionError saying what went wrong.

- **`assert_route_completed(result)`** : Nav2 finished the route and skipped no waypoint.
- **`assert_each_waypoint_reached(result, tolerance_m=None)`** : every waypoint
  was reached, and with `tolerance_m` the robot came within that distance of
  each. A tolerance looser than Nav2's `xy_goal_tolerance` will almost never fail.
- **`assert_final_pose(result, max_pos_error_m=0.5, max_yaw_error_rad=None)`** :
  the robot ended that close to the last waypoint.
- **`assert_min_clearance(result, min_m)`** : no laser return was ever nearer
  than `min_m`. Measured from the lidar, so add the distance from the lidar to
  the robot's edge.
- **`assert_max_recoveries(result, max_recoveries=0)`** : Nav2 ran at most that
  many recovery behaviours (spin, back up, wait, ...).

## Example: the TurtleBot3 Gazebo demo

[examples/nav2_sim.launch.py](examples/nav2_sim.launch.py) is a launch file
that loads a route, extrudes a world from the route's map, and includes Nav2's
TurtleBot3 demo in that world with the robot at the route's start. [examples/test_route_example.py](examples/test_route_example.py)
runs that launch file in a fixture, drives the route and asserts.

```bash
source /opt/ros/<ros-distro>/setup.zsh
source venv/bin/activate #if using
export PYTHONPATH=~/path-to/my-project/venv/lib/python3.12/site-packages:$PYTHONPATH # if using a venv
artefacts-route record --map /opt/ros/jazzy/share/nav2_bringup/maps/tb3_sandbox.yaml
pytest examples/test_route_example.py
```
