Metadata-Version: 2.4
Name: artefacts_toolkit_navigation
Version: 0.1.0
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 follow_route, load_route

def test_route_is_completed():
    result = follow_route(load_route(), timeout_s=300)
    assert result.succeeded
    assert result.final_pos_error_m < 0.5
```

`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)`**

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`: Tell amcl the robot is at the route's start before
  driving. `False` on a robot that is already localised.
- `startup_timeout_s`: How long to wait for Nav2
- `base_frame`: The robot frame, for the final pose error.

## 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
```



