Navigation Helpers in the Artefacts Toolkit

from artefacts_toolkit.navigation import <function>

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

load_route(
    route=None
)

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(
    route,
    x=None,
    y=None,
    yaw=None,
    names=("x_pose", "y_pose", "yaw"),
    map_arg="map"
)

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(
    route,
    x=None,
    y=None,
    yaw=None
)

check_map_match(
    route,
    map_yaml,
    ignore_mismatch=False
)

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

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(
    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"
)

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)

assert_route_completed(
    result
)

assert_each_waypoint_reached(
    result,
    tolerance_m=None
)

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

assert_min_clearance(
    result,
    min_m
)

assert_max_recoveries(
    result,
    max_recoveries=0
)

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)