#!/usr/bin/env python3 """Check real Nav2 static/inflation costmaps against a small authored map.""" import argparse 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.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy from geometry_msgs.msg import TransformStamped, PolygonStamped 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 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'} WIDTH = HEIGHT = 21 RESOLUTION = 0.1 ORIGIN = (-1.05, -1.05) ROBOT_POSE = (-0.8, -0.8, 0.0) ROBOT_RADIUS = 0.15 SCALING = 3.0 VERTICES = 16 INSCRIBED_RADIUS = ROBOT_RADIUS * math.cos(math.pi / VERTICES) TIMEOUT = 20.0 PLUGIN_LIBRARY = Path('/opt/ros/jazzy/lib/liblayers.so') CASES = (('narrow', 0.3, 77), ('wide', 0.6, 78)) 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(): return [-1 if y >= 18 else 100 if (x, y) == (10, 10) else 0 for y in range(HEIGHT) for x in range(WIDTH)] def read_pgm(): # The supplied P2 image is human-readable, with its top row written first. tokens = [] for line in (HERE / 'map.pgm').read_text().splitlines(): tokens.extend(line.split('#', 1)[0].split()) require(tokens[:4] == ['P2', '21', '21', '255'], 'Unexpected authored PGM header') pixels = [int(value) for value in tokens[4:]] require(len(pixels) == WIDTH * HEIGHT, 'Unexpected PGM pixel count') # Independently check file layout before sending it to map_server. for row in range(HEIGHT): y = HEIGHT - 1 - row for x in range(WIDTH): value = 205 if y >= 18 else 0 if (x, y) == (10, 10) else 254 require(pixels[row * WIDTH + x] == value, 'Authored PGM pixel differs') 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__('costmap_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, '/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. Here they lie outside both radii. 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 run_case(case, radius, domain, work): work.mkdir() config_path = HERE / (case + '.yaml') expected_parameters = flatten(yaml.safe_load(config_path.read_text())['costmap']['ros__parameters']) shutil.copyfile(config_path, work / 'costmap.yaml') rclpy.init(domain_id=domain) fixture = Fixture() processes, handles = [], [] summary = None states = {} environment = dict(os.environ, ROS_DOMAIN_ID=str(domain)) 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'], # In this pinned runtime, keeping the official plugin library loaded # avoids a reproduced CallbackGroup destructor crash at process exit. # --prefix scopes LD_PRELOAD to the costmap executable, not the host, # fixture, map server or ros2 CLI. No mapping code is replaced. ['ros2', 'run', '--prefix', 'env LD_PRELOAD=' + str(PLUGIN_LIBRARY), 'nav2_costmap_2d', 'nav2_costmap_2d', '--ros-args', '--params-file', str(config_path)], ] try: for name, command in zip(('map-server', 'costmap'), 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)) 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, '/costmap', Transition.TRANSITION_CONFIGURE) save_json(work / 'costmap-parameters.json', read_parameters(fixture, processes, '/costmap', expected_parameters)) transition(fixture, processes, '/costmap', Transition.TRANSITION_ACTIVATE) states['costmap'] = active_state(fixture, processes, '/costmap') spin_until(fixture, lambda: fixture.footprints, 'published robot footprint', processes) # Wait for a stable, initialized raw grid; input and robot pose stay fixed. deadline = time.monotonic() + TIMEOUT prior = None while True: require(time.monotonic() < deadline, 'Timed out: initialized stable raw costmap') response = call(fixture, processes, GetCostmap, '/get_costmap', GetCostmap.Request()) raw = grid_json(response.map, raw=True) if (raw['width'], raw['height']) == (WIDTH, HEIGHT) and raw['data'][10 * WIDTH + 10] == 254: if prior == raw['data']: break prior = raw['data'] settle(fixture, 0.25, processes) raw['inflation_radius_m'] = radius save_json(work / 'raw-costmap.json', raw) save_json(work / 'published-footprint.json', footprint_json(fixture.footprints[-1])) check_metadata(raw) footprint_checks = check_footprint(fixture.footprints[-1]) expected, distances, cell_radius = expected_grid(source['data'], radius, raw['resolution_m']) save_json(work / 'expected-costmap.json', {'data': expected, 'nearest_obstacle_distance_m': distances, 'cell_radius': cell_radius, 'inscribed_radius_m': INSCRIBED_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])) summary = {'case': case, 'domain_id': domain, 'inflation_radius_m': radius, 'cell_radius': cell_radius, 'checked_cells': len(expected), 'mismatches': len(mismatches), 'counts': counts(raw['data']), 'active_lifecycle_states': states, 'footprint': footprint_checks, 'row_through_obstacle': raw['data'][10 * WIDTH:11 * WIDTH]} for name in ('/costmap', '/map_server'): transition(fixture, processes, name, Transition.TRANSITION_DEACTIVATE) transition(fixture, processes, name, Transition.TRANSITION_CLEANUP) save_json(work / 'report.json', summary) return source, raw, summary finally: codes = {} for name, process, stream in reversed(list(zip(('map_server', 'costmap'), processes, handles))): codes[name] = stop(process, stream) save_json(work / 'process-exit-codes.json', codes) fixture.destroy_node() rclpy.shutdown() if sys.exc_info()[0] is None: require(all(code == 0 for code in codes.values()), 'Unexpected ROS process exit codes: ' + str(codes)) if summary is not None: summary['process_exit_codes'] = codes save_json(work / 'report.json', summary) 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-costmap-lesson-') work = Path(temporary.name) try: require(PLUGIN_LIBRARY.is_file(), 'Missing pinned costmap plugin library: ' + str(PLUGIN_LIBRARY)) 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) configs = [flatten(yaml.safe_load((HERE / (name + '.yaml')).read_text())['costmap']['ros__parameters']) for name in ('narrow', 'wide')] require(configs[0].pop('inflation_layer.inflation_radius') == 0.3, 'Unexpected narrow radius') require(configs[1].pop('inflation_layer.inflation_radius') == 0.6, 'Unexpected wide radius') require(configs[0] == configs[1], 'The case configurations differ in more than inflation radius') read_pgm() for filename in ('map.pgm', 'map.yaml', 'narrow.yaml', 'wide.yaml'): shutil.copyfile(HERE / filename, work / filename) values = [run_case(case, radius, domain, work / case) for case, radius, domain in CASES] source_narrow, narrow, narrow_checks = values[0] source_wide, wide, wide_checks = values[1] require(source_narrow['data'] == source_wide['data'], 'Loaded source map changed between runs') n, w = narrow['data'], wide['data'] changed = [i for i, (a, b) in enumerate(zip(n, w)) if a != b] require(bool(changed), 'Inflation radius comparison changed no cells') require(all(n[i] == 0 and 1 <= w[i] <= 252 for i in changed), 'Wider radius should only extend the lower-cost gradient in this fixture') require(all(w[i] == n[i] for i in range(len(n)) if n[i] in (253, 254, 255)), 'Inscribed, lethal or unknown cells changed unexpectedly') result = {'source_map': source_narrow, 'narrow': narrow, 'wide': wide, 'parameters': {'narrow_inflation_radius_m': 0.3, 'wide_inflation_radius_m': 0.6, 'cost_scaling_factor': SCALING, 'robot_radius_m': ROBOT_RADIUS, 'footprint_padding_m': 0.0, 'inscribed_radius_m': INSCRIBED_RADIUS, 'robot_pose': {'x_m': ROBOT_POSE[0], 'y_m': ROBOT_POSE[1], 'yaw_rad': ROBOT_POSE[2]}}, 'checks': {'narrow': narrow_checks, 'wide': wide_checks, 'changed_cells': len(changed), 'changed_indices': changed}} save_json(work / 'comparison.json', result) print('PASS map_server: all 441 cells match the authored PGM and YAML') print('PASS narrow: all 441 raw cost bytes match nearest-obstacle geometry and inflation formula') print('PASS wide: all 441 raw cost bytes match nearest-obstacle geometry and inflation formula') print(f'PASS comparison: {len(changed)} cells gain gradient cost; lethal, inscribed and unknown cells stay fixed') print('PASS configuration: both lifecycle nodes active; loaded parameters and published footprint checked') print('PASS cleanup: map server and costmap 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()