#!/usr/bin/env python3
"""Run Nav2 map_server and planner_server; verify a route and a disconnected free goal."""

import argparse
from collections import deque
import json
import math
import os
from pathlib import Path
import shutil
import signal
import subprocess
import sys
import tempfile
import time

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import TransformStamped, PolygonStamped, PoseStamped
from lifecycle_msgs.msg import State, Transition
from lifecycle_msgs.srv import ChangeState, GetState
from nav_msgs.msg import OccupancyGrid
from nav2_msgs.srv import GetCostmap
from nav2_msgs.action import ComputePathToPose
from rcl_interfaces.srv import GetParameters
from tf2_ros import StaticTransformBroadcaster
import yaml

HERE = Path(__file__).resolve().parent
PACKAGE_VERSIONS = {
    'ros-jazzy-nav2-map-server': '1.3.13-1noble.20260921.044109',
    'ros-jazzy-nav2-costmap-2d': '1.3.13-1noble.20260921.044133',
    'ros-jazzy-nav2-planner': '1.3.13-1noble.20260921.071108',
    'ros-jazzy-nav2-navfn-planner': '1.3.13-1noble.20260921.071010',
}
WIDTH, HEIGHT = 40, 30
RESOLUTION = 0.1
ORIGIN = (0.0, 0.0)
START = (0.55, 1.55)
REACHABLE = (3.45, 1.35)
DISCONNECTED = (3.35, 2.45)
ROBOT_POSE = (*START, 0.0)
ROBOT_RADIUS = 0.12
INFLATION_RADIUS = 0.35
SCALING = 3.0
VERTICES = 16
INSCRIBED_RADIUS = ROBOT_RADIUS * math.cos(math.pi / VERTICES)
TIMEOUT = 20.0
DOMAIN = 79


def require(condition, description):
    if not condition:
        raise AssertionError(description)


def save_json(path, value):
    path.write_text(json.dumps(value, indent=2) + '\n')


def stamp_ns(stamp):
    return stamp.sec * 1_000_000_000 + stamp.nanosec


def authored_cells():
    result = []
    for y in range(HEIGHT):
        for x in range(WIDTH):
            occupied = (x in (0, WIDTH - 1) or y in (0, HEIGHT - 1)
                        or (x == 20 and y not in range(7, 12))
                        or (x in (28, 38) and 19 <= y <= 29)
                        or (y in (19, 29) and 28 <= x <= 38))
            result.append(100 if occupied else -1 if 3 <= x <= 6 and 23 <= y <= 26 else 0)
    return result


def read_pgm():
    tokens = []
    for line in (HERE / 'map.pgm').read_text().splitlines():
        tokens.extend(line.split('#', 1)[0].split())
    require(tokens[:4] == ['P2', str(WIDTH), str(HEIGHT), '255'], 'Unexpected authored PGM header')
    pixels = [int(value) for value in tokens[4:]]
    expected = authored_cells()
    require(len(pixels) == WIDTH * HEIGHT, 'Unexpected PGM pixel count')
    require(all(pixels[row * WIDTH + x] == {-1: 205, 0: 254, 100: 0}[expected[(HEIGHT - 1 - row) * WIDTH + x]]
                for row in range(HEIGHT) for x in range(WIDTH)), 'Authored PGM cells differ')


def flatten(values, prefix=''):
    result = {}
    for name, value in values.items():
        key = prefix + name
        if isinstance(value, dict):
            result.update(flatten(value, key + '.'))
        else:
            result[key] = value
    return result


def spin_until(node, predicate, description, processes=(), timeout=TIMEOUT):
    deadline = time.monotonic() + timeout
    while not predicate():
        require(time.monotonic() < deadline, 'Timed out: ' + description)
        for process in processes:
            require(process.poll() is None, 'ROS process exited early during ' + description)
        rclpy.spin_once(node, timeout_sec=0.01)


def settle(node, seconds, processes):
    deadline = time.monotonic() + seconds
    while time.monotonic() < deadline:
        for process in processes:
            require(process.poll() is None, 'ROS process exited early')
        rclpy.spin_once(node, timeout_sec=0.01)


def call(node, processes, service, name, request):
    client = node.create_client(service, name)
    try:
        spin_until(node, client.service_is_ready, name, processes)
        future = client.call_async(request)
        spin_until(node, future.done, name + ' response', processes)
        require(future.exception() is None, name + ' returned an exception')
        return future.result()
    finally:
        node.destroy_client(client)


def transition(node, processes, name, transition_id):
    request = ChangeState.Request()
    request.transition.id = transition_id
    result = call(node, processes, ChangeState, name + '/change_state', request)
    require(result.success, 'Lifecycle transition failed: ' + name)


def active_state(node, processes, name):
    result = call(node, processes, GetState, name + '/get_state', GetState.Request())
    require(result.current_state.id == State.PRIMARY_STATE_ACTIVE, name + ' is not active')
    return {'id': result.current_state.id, 'label': result.current_state.label}


def read_parameters(node, processes, name, expected):
    request = GetParameters.Request()
    request.names = list(expected)
    response = call(node, processes, GetParameters, name + '/get_parameters', request)
    require(len(response.values) == len(expected), 'Incomplete parameter response')
    fields = {1: 'bool_value', 2: 'integer_value', 3: 'double_value',
              4: 'string_value', 9: 'string_array_value'}
    actual = {}
    for key, value in zip(expected, response.values):
        require(value.type in fields, 'Unset/unsupported parameter: ' + key)
        result = getattr(value, fields[value.type])
        actual[key] = list(result) if value.type == 9 else result
        require(actual[key] == expected[key], 'Loaded parameter differs: ' + key)
    return actual


class Fixture(Node):
    def __init__(self):
        super().__init__('planner_lesson_fixture')
        self.maps, self.footprints = [], []
        qos = QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE,
                         durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self.create_subscription(OccupancyGrid, '/map', self.maps.append, qos)
        self.create_subscription(PolygonStamped, '/global_costmap/published_footprint', self.footprints.append, 10)
        self.broadcaster = StaticTransformBroadcaster(self)
        transform = TransformStamped()
        transform.header.frame_id, transform.child_frame_id = 'map', 'base_link'
        transform.transform.translation.x = ROBOT_POSE[0]
        transform.transform.translation.y = ROBOT_POSE[1]
        transform.transform.rotation.w = 1.0
        self.broadcaster.sendTransform(transform)


def grid_json(message, raw=False):
    metadata = message.metadata if raw else message.info
    p, q = metadata.origin.position, metadata.origin.orientation
    return {'frame': message.header.frame_id, 'stamp_ns': stamp_ns(message.header.stamp),
            'width': metadata.size_x if raw else metadata.width,
            'height': metadata.size_y if raw else metadata.height,
            'resolution_m': metadata.resolution,
            'origin': {'x': p.x, 'y': p.y, 'z': p.z, 'qx': q.x, 'qy': q.y, 'qz': q.z, 'qw': q.w},
            'data': list(message.data)}


def check_metadata(grid):
    require(grid['frame'] == 'map', 'Grid frame differs')
    require((grid['width'], grid['height']) == (WIDTH, HEIGHT), 'Grid dimensions differ')
    require(len(grid['data']) == WIDTH * HEIGHT, 'Grid data length differs')
    require(abs(grid['resolution_m'] - RESOLUTION) < 1e-7, 'Grid resolution differs')
    o = grid['origin']
    require(abs(o['x'] - ORIGIN[0]) < 1e-7 and abs(o['y'] - ORIGIN[1]) < 1e-7,
            'Grid origin differs')
    require(abs(o['z']) + abs(o['qx']) + abs(o['qy']) + abs(o['qz']) < 1e-7
            and abs(o['qw'] - 1) < 1e-7, 'Grid origin orientation differs')


def footprint_json(message):
    return {'frame': message.header.frame_id, 'stamp_ns': stamp_ns(message.header.stamp),
            'points': [{'x': p.x, 'y': p.y, 'z': p.z} for p in message.polygon.points]}


def check_footprint(message):
    require(message.header.frame_id == 'map', 'Footprint frame differs')
    points = [(p.x - ROBOT_POSE[0], p.y - ROBOT_POSE[1]) for p in message.polygon.points]
    require(len(points) == VERTICES, 'Expected the configured radius footprint to have 16 vertices')
    require(all(abs(math.hypot(x, y) - ROBOT_RADIUS) < 1e-6 for x, y in points),
            'Published footprint differs from unpadded circular radius')
    # Distance from origin to each polygon edge, computed from captured vertices.
    distances = []
    for a, b in zip(points, points[1:] + points[:1]):
        distances.append(abs(a[0] * b[1] - a[1] * b[0]) / math.hypot(b[0] - a[0], b[1] - a[1]))
    require(abs(min(distances) - INSCRIBED_RADIUS) < 1e-6, 'Footprint inscribed radius differs')
    return {'vertices': len(points), 'measured_inscribed_radius_m': min(distances),
            'formula_inscribed_radius_m': INSCRIBED_RADIUS}


def expected_grid(source, radius, resolution):
    obstacles = [(i % WIDTH, i // WIDTH) for i, value in enumerate(source) if value == 100]
    cell_radius = math.ceil(radius / resolution)
    output = []
    distances = []
    for i, value in enumerate(source):
        x, y = i % WIDTH, i // WIDTH
        distance_cells = min(math.hypot(x - ox, y - oy) for ox, oy in obstacles)
        distance_m = distance_cells * resolution
        distances.append(distance_m)
        if distance_cells == 0:
            cost = 254
        elif distance_cells > cell_radius:
            cost = 0
        elif distance_m <= INSCRIBED_RADIUS:
            cost = 253
        else:
            cost = int(252 * math.exp(-SCALING * (distance_m - INSCRIBED_RADIUS)))
        # With inflate_unknown=false, lower costs do not overwrite unknown.
        # Unknown cells do not seed this distance calculation because
        # inflate_around_unknown=false.
        output.append(255 if value == -1 and cost < 253 else cost)
    return output, distances, cell_radius


def counts(data):
    return {'free': data.count(0), 'gradient': sum(1 <= c <= 252 for c in data),
            'inscribed': data.count(253), 'lethal': data.count(254), 'unknown': data.count(255)}


def stop(process, stream):
    if process.poll() is None:
        try:
            os.killpg(process.pid, signal.SIGINT)
        except ProcessLookupError:
            pass
        try:
            process.wait(timeout=10)
        except subprocess.TimeoutExpired:
            os.killpg(process.pid, signal.SIGKILL)
            process.wait(timeout=5)
    stream.close()
    return process.returncode


def pose_message(node, xy):
    message = PoseStamped()
    message.header.frame_id = 'map'
    message.header.stamp = node.get_clock().now().to_msg()
    message.pose.position.x, message.pose.position.y = xy
    message.pose.orientation.w = 1.0
    return message


def pose_json(message):
    p, q = message.pose.position, message.pose.orientation
    return {'frame': message.header.frame_id, 'stamp_ns': stamp_ns(message.header.stamp),
            'x': p.x, 'y': p.y, 'z': p.z, 'qx': q.x, 'qy': q.y, 'qz': q.z, 'qw': q.w}


def request_path(node, processes, xy):
    client = ActionClient(node, ComputePathToPose, '/compute_path_to_pose')
    try:
        spin_until(node, client.server_is_ready, 'planner action server', processes)
        request = ComputePathToPose.Goal()
        request.start, request.goal = pose_message(node, START), pose_message(node, xy)
        request.planner_id, request.use_start = 'GridBased', True
        future = client.send_goal_async(request)
        spin_until(node, future.done, 'planning request acceptance', processes)
        handle = future.result()
        require(handle is not None and handle.accepted, 'Planner rejected the action request')
        result_future = handle.get_result_async()
        spin_until(node, result_future.done, 'planning result', processes)
        wrapped = result_future.result()
        result = wrapped.result
        status_labels = {GoalStatus.STATUS_SUCCEEDED: 'SUCCEEDED', GoalStatus.STATUS_ABORTED: 'ABORTED'}
        return {'start': pose_json(request.start), 'goal': pose_json(request.goal),
                'planner_id': request.planner_id, 'use_start': request.use_start,
                'accepted': handle.accepted, 'status': wrapped.status,
                'status_label': status_labels.get(wrapped.status, 'UNEXPECTED'),
                'error_code': result.error_code, 'error_msg': result.error_msg,
                'planning_time_s': result.planning_time.sec + result.planning_time.nanosec / 1e9,
                'path': {'frame': result.path.header.frame_id,
                         'stamp_ns': stamp_ns(result.path.header.stamp),
                         'poses': [pose_json(pose) for pose in result.path.poses]}}
    finally:
        client.destroy()


def xy_indices(xy, grid, rounding=False):
    values = [(xy[0] - grid['origin']['x']) / grid['resolution_m'],
              (xy[1] - grid['origin']['y']) / grid['resolution_m']]
    # Inputs are positive; floor(v + 0.5) matches std::round, unlike Python round.
    return tuple(math.floor(v + 0.5) if rounding else math.floor(v) for v in values)


def check_request_cells(source, raw):
    # Raw zero is a deliberately strict fixture choice. NavFn can also traverse
    # gradient cells below 253; nonzero cost alone does not mean occupied.
    result = {}
    for name, xy in (('start', START), ('reachable', REACHABLE), ('disconnected', DISCONNECTED)):
        result[name] = {}
        for convention in ('floor', 'navfn_round'):
            x, y = xy_indices(xy, raw, convention == 'navfn_round')
            require(0 <= x < WIDTH and 0 <= y < HEIGHT, 'Request outside map')
            index = y * WIDTH + x
            require(source['data'][index] == 0 and raw['data'][index] == 0,
                    f'{name} {convention} cell is not free before planning')
            result[name][convention] = {'x': x, 'y': y, 'source': source['data'][index], 'raw': raw['data'][index]}
    return result


def reachable_cells(source, raw):
    start = xy_indices(START, raw, True)
    visited, pending = {start}, deque([start])
    # 8-connected reachability is more permissive than 4-connected. Even this
    # cannot reach the enclosed goal; this is independent of NavFn's search.
    while pending:
        x, y = pending.popleft()
        for dx in (-1, 0, 1):
            for dy in (-1, 0, 1):
                q = (x + dx, y + dy)
                if 0 <= q[0] < WIDTH and 0 <= q[1] < HEIGHT and q not in visited:
                    index = q[1] * WIDTH + q[0]
                    if source['data'][index] == 0 and raw['data'][index] < 253:
                        visited.add(q)
                        pending.append(q)
    require(xy_indices(REACHABLE, raw, True) in visited, 'Authored reachable goal is disconnected')
    require(xy_indices(DISCONNECTED, raw, True) not in visited, 'Authored no-path goal is connected')
    return len(visited)


def point_segment_distance(point, a, b):
    dx, dy = b[0] - a[0], b[1] - a[1]
    length2 = dx * dx + dy * dy
    t = max(0.0, min(1.0, ((point[0] - a[0]) * dx + (point[1] - a[1]) * dy) / length2)) if length2 else 0.0
    return math.hypot(point[0] - a[0] - t * dx, point[1] - a[1] - t * dy)


def segment_rectangle_distance(a, b, rectangle):
    xmin, ymin, xmax, ymax = rectangle
    # Slab intersection test includes endpoints and every interior segment point.
    lower, upper = 0.0, 1.0
    for av, bv, lo, hi in ((a[0], b[0], xmin, xmax), (a[1], b[1], ymin, ymax)):
        delta = bv - av
        if abs(delta) < 1e-15:
            if not lo <= av <= hi:
                lower, upper = 1.0, 0.0
                break
        else:
            t1, t2 = (lo - av) / delta, (hi - av) / delta
            lower, upper = max(lower, min(t1, t2)), min(upper, max(t1, t2))
    if lower <= upper:
        return 0.0
    corners = [(xmin, ymin), (xmax, ymin), (xmax, ymax), (xmin, ymax)]
    def point_rectangle(p):
        return math.hypot(max(xmin - p[0], 0.0, p[0] - xmax), max(ymin - p[1], 0.0, p[1] - ymax))
    return min(point_rectangle(a), point_rectangle(b),
               *(point_segment_distance(corner, a, b) for corner in corners))


def check_path(requests, source, raw):
    success, failure = requests['reachable'], requests['disconnected']
    require(success['accepted'] and success['status'] == GoalStatus.STATUS_SUCCEEDED
            and success['error_code'] == 0, 'Reachable request did not succeed')
    require(failure['accepted'] and failure['status'] == GoalStatus.STATUS_ABORTED
            and failure['error_code'] == 208 and not failure['path']['poses'],
            'Disconnected free goal did not return ABORTED / NO_VALID_PATH / empty path')
    path = success['path']
    require(path['frame'] == 'map' and len(path['poses']) >= 2, 'Invalid path frame or pose count')
    poses = path['poses']
    for pose in poses:
        require(pose['frame'] == 'map', 'Path pose frame differs')
        require(all(math.isfinite(pose[k]) for k in ('x', 'y', 'z', 'qx', 'qy', 'qz', 'qw')), 'Nonfinite path pose')
        require(abs(sum(pose[k] ** 2 for k in ('qx', 'qy', 'qz', 'qw')) - 1) < 1e-6, 'Nonunit path quaternion')
        require(abs(pose['z']) + abs(pose['qx']) + abs(pose['qy']) < 1e-7, 'Path is not planar')
    resolution, origin = raw['resolution_m'], raw['origin']
    snapped = xy_indices(START, raw, True)
    expected_start = (origin['x'] + snapped[0] * resolution, origin['y'] + snapped[1] * resolution)
    first, last = (poses[0]['x'], poses[0]['y']), (poses[-1]['x'], poses[-1]['y'])
    require(math.dist(first, expected_start) < 1e-6, 'First pose differs from NavFn rounded start')
    start_offset = math.dist(first, START)
    require(start_offset <= math.sqrt(2) * resolution / 2 + 1e-7, 'Unexpected initial pose offset')
    require(math.dist(last, REACHABLE) < 1e-7, 'Last pose differs from exact requested goal')
    require(abs(poses[-1]['qz']) < 1e-7 and abs(poses[-1]['qw'] - 1) < 1e-7, 'Final orientation differs')
    # Include the supplied-start connector: checking only returned waypoints
    # would miss collisions between points or at the initial lattice snap.
    points = [START] + [(p['x'], p['y']) for p in poses]
    require(all(origin['x'] <= x <= origin['x'] + WIDTH * resolution
                and origin['y'] <= y <= origin['y'] + HEIGHT * resolution for x, y in points), 'Path outside map')
    require(all(origin['x'] + ROBOT_RADIUS <= x <= origin['x'] + WIDTH * resolution - ROBOT_RADIUS
                and origin['y'] + ROBOT_RADIUS <= y <= origin['y'] + HEIGHT * resolution - ROBOT_RADIUS
                for x, y in points), 'Swept circle exceeds map bounds')
    rectangles = {'occupied': [], 'unknown': []}
    for i, value in enumerate(source['data']):
        if value in (100, -1):
            x, y = i % WIDTH, i // WIDTH
            rectangles['occupied' if value == 100 else 'unknown'].append(
                (origin['x'] + x * resolution, origin['y'] + y * resolution,
                 origin['x'] + (x + 1) * resolution, origin['y'] + (y + 1) * resolution))
    clearances = {kind: min(segment_rectangle_distance(a, b, rectangle)
                           for a, b in zip(points, points[1:]) for rectangle in cells)
                  for kind, cells in rectangles.items()}
    require(clearances['occupied'] >= ROBOT_RADIUS - 1e-7, 'Path segment lacks nominal circle clearance')
    require(clearances['unknown'] >= ROBOT_RADIUS - 1e-7, 'Path segment lacks clearance from unknown space')
    # The crossing of the divider's center plane must occur in its declared gap.
    crossings = []
    plane = origin['x'] + 20.5 * resolution
    for a, b in zip(points, points[1:]):
        if (a[0] - plane) * (b[0] - plane) <= 0 and a[0] != b[0]:
            t = (plane - a[0]) / (b[0] - a[0])
            crossings.append(a[1] + t * (b[1] - a[1]))
    require(bool(crossings) and all(7 * resolution <= y <= 12 * resolution for y in crossings),
            'Path did not cross divider through its gap')
    return {'pose_count': len(poses), 'segment_count_including_start_connector': len(points) - 1,
            'path_length_m': sum(math.dist(a, b) for a, b in zip(points[1:], points[2:])),
            'start_connector_length_m': start_offset, 'expected_first_xy': list(expected_start),
            'minimum_occupied_rectangle_clearance_m': clearances['occupied'],
            'minimum_unknown_rectangle_clearance_m': clearances['unknown'],
            'divider_crossing_y_m': crossings}


def get_raw(node, processes):
    return grid_json(call(node, processes, GetCostmap, '/global_costmap/get_costmap', GetCostmap.Request()).map, raw=True)


def run(work):
    rclpy.init(domain_id=DOMAIN)
    fixture = Fixture()
    processes, handles = [], []
    result = None
    environment = dict(os.environ, ROS_DOMAIN_ID=str(DOMAIN))
    config_path = HERE / 'planner.yaml'
    config = yaml.safe_load(config_path.read_text())
    planner_parameters = flatten(config['planner_server']['ros__parameters'])
    costmap_parameters = flatten(config['global_costmap']['global_costmap']['ros__parameters'])
    map_parameters = {'yaml_filename': str(HERE / 'map.yaml'), 'use_sim_time': False,
                      'frame_id': 'map', 'topic_name': 'map'}
    commands = [
        ['ros2', 'run', 'nav2_map_server', 'map_server', '--ros-args',
         '-p', 'yaml_filename:=' + str(HERE / 'map.yaml'), '-p', 'use_sim_time:=false',
         '-p', 'frame_id:=map', '-p', 'topic_name:=map'],
        ['ros2', 'run', 'nav2_planner', 'planner_server', '--ros-args', '--params-file', str(config_path)],
    ]
    try:
        for name, command in zip(('map-server', 'planner-server'), commands):
            stream = (work / (name + '.log')).open('w')
            handles.append(stream)
            processes.append(subprocess.Popen(command, stdout=stream, stderr=subprocess.STDOUT,
                                               env=environment, start_new_session=True))
        states = {}
        transition(fixture, processes, '/map_server', Transition.TRANSITION_CONFIGURE)
        transition(fixture, processes, '/map_server', Transition.TRANSITION_ACTIVATE)
        states['map_server'] = active_state(fixture, processes, '/map_server')
        spin_until(fixture, lambda: fixture.maps, 'map_server occupancy grid', processes)
        source = grid_json(fixture.maps[-1])
        save_json(work / 'source-map.json', source)
        check_metadata(source)
        require(source['data'] == authored_cells(), 'Map server cells differ from authored PGM/YAML')
        save_json(work / 'map-server-parameters.json', read_parameters(fixture, processes, '/map_server', map_parameters))
        transition(fixture, processes, '/planner_server', Transition.TRANSITION_CONFIGURE)
        save_json(work / 'planner-parameters.json', read_parameters(fixture, processes, '/planner_server', planner_parameters))
        save_json(work / 'costmap-parameters.json', read_parameters(fixture, processes, '/global_costmap/global_costmap', costmap_parameters))
        transition(fixture, processes, '/planner_server', Transition.TRANSITION_ACTIVATE)
        for name in ('/planner_server', '/global_costmap/global_costmap'):
            states[name.strip('/')] = active_state(fixture, processes, name)
        spin_until(fixture, lambda: fixture.footprints, 'published robot footprint', processes)
        deadline, prior = time.monotonic() + TIMEOUT, None
        while True:
            require(time.monotonic() < deadline, 'Timed out: stable raw costmap')
            raw = get_raw(fixture, processes)
            if (raw['width'], raw['height']) == (WIDTH, HEIGHT) and raw['data'][0] == 254:
                if prior == raw['data']:
                    break
                prior = raw['data']
            settle(fixture, 0.25, processes)
        save_json(work / 'raw-costmap-before.json', raw)
        save_json(work / 'published-footprint.json', footprint_json(fixture.footprints[-1]))
        check_metadata(raw)
        footprint = check_footprint(fixture.footprints[-1])
        expected, distances, cell_radius = expected_grid(source['data'], INFLATION_RADIUS, raw['resolution_m'])
        save_json(work / 'expected-costmap.json', {'data': expected, 'nearest_obstacle_distance_m': distances, 'cell_radius': cell_radius})
        mismatches = [{'x': i % WIDTH, 'y': i // WIDTH, 'expected': e, 'actual': a}
                      for i, (e, a) in enumerate(zip(expected, raw['data'])) if e != a]
        require(not mismatches, 'Raw costmap formula mismatches: ' + str(mismatches[:10]))
        request_cells = check_request_cells(source, raw)
        connected_count = reachable_cells(source, raw)
        requests = {}
        for name, goal in (('reachable', REACHABLE), ('disconnected', DISCONNECTED)):
            requests[name] = request_path(fixture, processes, goal)
            save_json(work / (name + '-request.json'), requests[name])
            after = get_raw(fixture, processes)
            save_json(work / ('raw-costmap-after-' + name + '.json'), after)
            require(after['data'] == raw['data'], 'Planning changed the raw costmap')
            require(grid_json(fixture.maps[-1])['data'] == source['data'], 'Source map changed')
        save_json(work / 'source-map-after.json', grid_json(fixture.maps[-1]))
        path_checks = check_path(requests, source, raw)
        result = {'source_map': source, 'raw_costmap': raw, 'requests': requests,
                  'parameters': {'planner_id': 'GridBased', 'use_astar': True, 'tolerance_m': 0.0,
                    'allow_unknown': False, 'use_start': True, 'robot_radius_m': ROBOT_RADIUS,
                    'footprint_padding_m': 0.0, 'inscribed_radius_m': INSCRIBED_RADIUS,
                    'inflation_radius_m': INFLATION_RADIUS, 'cost_scaling_factor': SCALING},
                  'checks': {'checked_cells': len(expected), 'raw_costmap_mismatches': 0,
                    'costmap_unchanged_after_both_requests': True, 'source_map_unchanged': True,
                    'counts': counts(raw['data']), 'request_cells': request_cells,
                    'reachable_cell_count': connected_count, 'footprint': footprint,
                    'active_lifecycle_states': states, 'path': path_checks}}
        for name in ('/planner_server', '/map_server'):
            transition(fixture, processes, name, Transition.TRANSITION_DEACTIVATE)
            transition(fixture, processes, name, Transition.TRANSITION_CLEANUP)
        return result
    finally:
        codes = {}
        for name, process, stream in reversed(list(zip(('map_server', 'planner_server'), processes, handles))):
            codes[name] = stop(process, stream)
        save_json(work / 'process-exit-codes.json', codes)
        fixture.destroy_node()
        rclpy.shutdown()
        if result is not None:
            result['checks']['process_exit_codes'] = codes
            save_json(work / 'planning.json', result)
        if sys.exc_info()[0] is None:
            require(all(code == 0 for code in codes.values()), 'Unexpected ROS process exit codes: ' + str(codes))


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--output', type=Path, help='New output directory; existing paths are refused.')
    args = parser.parse_args()
    temporary = None
    if args.output:
        work = args.output.resolve()
        work.mkdir(parents=True, exist_ok=False)
    else:
        temporary = tempfile.TemporaryDirectory(prefix='nav2-planner-lesson-')
        work = Path(temporary.name)
    try:
        for package, expected in PACKAGE_VERSIONS.items():
            actual = subprocess.check_output(['dpkg-query', '-W', '-f=${Version}', package], text=True, timeout=10).strip()
            require(actual == expected, 'Unexpected package version: ' + package)
        config = yaml.safe_load((HERE / 'planner.yaml').read_text())
        planner = flatten(config['planner_server']['ros__parameters'])
        costmap = flatten(config['global_costmap']['global_costmap']['ros__parameters'])
        required_planner = {'use_sim_time': False, 'planner_plugins': ['GridBased'],
            'GridBased.plugin': 'nav2_navfn_planner::NavfnPlanner', 'GridBased.tolerance': 0.0,
            'GridBased.use_astar': True, 'GridBased.allow_unknown': False,
            'GridBased.use_final_approach_orientation': False}
        required_costmap = {'global_frame': 'map', 'robot_base_frame': 'base_link',
            'rolling_window': False, 'robot_radius': ROBOT_RADIUS, 'footprint': '[]',
            'footprint_padding': 0.0, 'track_unknown_space': True,
            'plugins': ['static_layer', 'inflation_layer'],
            'static_layer.footprint_clearing_enabled': False,
            'inflation_layer.inflation_radius': INFLATION_RADIUS,
            'inflation_layer.cost_scaling_factor': SCALING,
            'inflation_layer.inflate_unknown': False, 'inflation_layer.inflate_around_unknown': False}
        for values, required in ((planner, required_planner), (costmap, required_costmap)):
            require(all(values.get(key) == value for key, value in required.items()),
                    'Configuration differs from the fixture contract; revise its assertions too')
        read_pgm()
        for filename in ('map.pgm', 'map.yaml', 'planner.yaml'):
            shutil.copyfile(HERE / filename, work / filename)
        result = run(work)
        checks = result['checks']
        print('PASS map_server: all 1200 cells match the authored PGM and YAML')
        print('PASS global costmap: all 1200 raw cost bytes match independent geometry and inflation formula')
        print('PASS requests: supplied start and both goals are free under floor and NavFn rounded indexing')
        print('PASS reachable: accepted, SUCCEEDED, error 0; ' + str(checks['path']['pose_count']) + ' path poses')
        print('PASS geometry: every segment and the start connector clear occupied cells by at least 0.12 m and keep that disk clear of unknown cells')
        print('PASS disconnected: accepted, ABORTED, error 208 (NO_VALID_PATH), empty path')
        print('PASS configuration: active lifecycle states, loaded parameters, footprint and unchanged grids checked')
        print('PASS cleanup: map server and planner server exit normally after lifecycle cleanup')
        print('ARTIFACTS ' + (str(work) if args.output else 'temporary outputs checked and removed'))
    finally:
        if temporary:
            temporary.cleanup()


if __name__ == '__main__':
    main()