#!/usr/bin/env python3 """Feed a finite, synthetic room fixture into the real SLAM Toolbox mapper.""" import argparse import json import math import os from pathlib import Path import signal import subprocess import tempfile import time import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy from rclpy.time import Time from geometry_msgs.msg import TransformStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import ChangeState, GetState from lifecycle_msgs.msg import Transition, State from nav_msgs.msg import OccupancyGrid from rcl_interfaces.srv import GetParameters from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage from tf2_ros import Buffer, TransformListener, TransformBroadcaster, StaticTransformBroadcaster import yaml HERE = Path(__file__).resolve().parent PACKAGE_VERSION = "2.8.5-1noble.20260916.104732" NS = 1_000_000_000 START_NS = 100 * NS STEP_NS = NS // 2 STEPS = 20 RAYS = 361 LASER_X = 0.2 TIMEOUT = 20.0 ROOM = (-4.0, 4.0, -3.0, 3.0) ROOM_ROTATION = 0.1 # Keep long walls away from a grid's exclusive maximum boundary. LOCAL_SEGMENTS = [((-4., -3.), (4., -3.)), ((4., -3.), (4., 3.)), ((4., 3.), (-4., 3.)), ((-4., 3.), (-4., -3.)), ((1., 1.5), (2., 1.5)), ((2., 1.5), (2., 2.5)), ((2., 2.5), (1., 2.5)), ((1., 2.5), (1., 1.5))] def rotate_room(point): x, y = point return (x * math.cos(ROOM_ROTATION) - y * math.sin(ROOM_ROTATION), x * math.sin(ROOM_ROTATION) + y * math.cos(ROOM_ROTATION)) SEGMENTS = [(rotate_room(a), rotate_room(b)) for a, b in LOCAL_SEGMENTS] CASES = ('missing-laser-tf', 'complete-tf') def require(condition, description): if not condition: raise AssertionError(description) def set_stamp(stamp, nanoseconds): stamp.sec, stamp.nanosec = divmod(nanoseconds, NS) def stamp_ns(stamp): return stamp.sec * NS + stamp.nanosec def cross(a, b): return a[0] * b[1] - a[1] * b[0] def ray_distance(origin, angle): direction = (math.cos(angle), math.sin(angle)) hits = [] for a, b in SEGMENTS: edge = (b[0] - a[0], b[1] - a[1]) delta = (a[0] - origin[0], a[1] - origin[1]) divisor = cross(direction, edge) if abs(divisor) < 1e-10: continue distance = cross(delta, edge) / divisor along = cross(delta, direction) / divisor if distance > 0 and -1e-9 <= along <= 1 + 1e-9: hits.append(distance) require(bool(hits), 'Every supplied ray must hit the enclosed room') return min(hits) def fixture_pose(index): return (index * 0.06, 0.0, 0.0) def scan_for(index): x, y, yaw = fixture_pose(index) scan = LaserScan() set_stamp(scan.header.stamp, START_NS + index * STEP_NS) scan.header.frame_id = 'laser' scan.angle_min = -math.pi scan.angle_max = math.pi scan.angle_increment = 2 * math.pi / (RAYS - 1) scan.time_increment = 0.0 # All beams share one idealized acquisition time. scan.scan_time = STEP_NS / NS scan.range_min, scan.range_max = 0.05, 25.0 origin = (x + LASER_X * math.cos(yaw), y + LASER_X * math.sin(yaw)) scan.ranges = [ray_distance(origin, -math.pi + n * 2 * math.pi / (RAYS - 1) + yaw) for n in range(RAYS)] require(all(scan.range_min < r < scan.range_max for r in scan.ranges), 'Supplied range outside declared limits') return scan def save_json(path, value): path.write_text(json.dumps(value, indent=2) + '\n') def spin_until(node, predicate, description, process=None, timeout=TIMEOUT): deadline = time.monotonic() + timeout while not predicate(): require(time.monotonic() < deadline, 'Timed out: ' + description) if process is not None: require(process.poll() is None, 'Mapper exited early') rclpy.spin_once(node, timeout_sec=0.01) def settle(node, seconds, process=None): deadline = time.monotonic() + seconds while time.monotonic() < deadline: if process is not None: require(process.poll() is None, 'Mapper exited early') rclpy.spin_once(node, timeout_sec=0.01) class Fixture(Node): def __init__(self): super().__init__('slam_lesson_fixture') self.current_ns = START_NS self.maps, self.transforms, self.poses = [], [], [] self.scan_pub = self.create_publisher(LaserScan, '/scan', 10) self.clock_pub = self.create_publisher(Clock, '/clock', 10) self.tf_pub = TransformBroadcaster(self) self.static_pub = StaticTransformBroadcaster(self) self.buffer = Buffer() self.listener = TransformListener(self.buffer, self) map_qos = QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL) self.create_subscription(OccupancyGrid, '/map', self.maps.append, map_qos) self.create_subscription(TFMessage, '/tf', self.receive_tf, 100) self.create_subscription(PoseWithCovarianceStamped, '/pose', self.receive_pose, 100) # Wall-time timer and monotonic deadlines let missing ROS data time out. self.create_timer(0.02, self.publish_clock) def publish_clock(self): message = Clock() set_stamp(message.clock, self.current_ns) self.clock_pub.publish(message) def publish_static(self): transform = TransformStamped() transform.header.frame_id, transform.child_frame_id = 'base_link', 'laser' transform.transform.translation.x = LASER_X transform.transform.rotation.w = 1.0 self.static_pub.sendTransform(transform) def publish_odom(self, index): transform = TransformStamped() set_stamp(transform.header.stamp, START_NS + index * STEP_NS) transform.header.frame_id, transform.child_frame_id = 'odom', 'base_link' x, y, yaw = fixture_pose(index) transform.transform.translation.x, transform.transform.translation.y = x, y transform.transform.rotation.z = math.sin(yaw / 2) transform.transform.rotation.w = math.cos(yaw / 2) self.tf_pub.sendTransform(transform) def receive_pose(self, message): p, q = message.pose.pose.position, message.pose.pose.orientation self.poses.append({'stamp_ns': stamp_ns(message.header.stamp), 'frame': message.header.frame_id, 'x_m': p.x, 'y_m': p.y, 'z_m': p.z, 'qx': q.x, 'qy': q.y, 'qz': q.z, 'qw': q.w}) def receive_tf(self, message): for transform in message.transforms: if (transform.header.frame_id, transform.child_frame_id) == ('map', 'odom'): t, q = transform.transform.translation, transform.transform.rotation self.transforms.append({'stamp_ns': stamp_ns(transform.header.stamp), 'frame': 'map', 'child_frame': 'odom', 'x_m': t.x, 'y_m': t.y, 'z_m': t.z, 'qx': q.x, 'qy': q.y, 'qz': q.z, 'qw': q.w}) def call_service(node, process, service_type, name, request): client = node.create_client(service_type, name) try: spin_until(node, client.service_is_ready, name, process) future = client.call_async(request) spin_until(node, future.done, name + ' response', process) require(future.exception() is None, name + ' failed') return future.result() finally: node.destroy_client(client) def activate(node, process): for transition in (Transition.TRANSITION_CONFIGURE, Transition.TRANSITION_ACTIVATE): request = ChangeState.Request() request.transition.id = transition response = call_service(node, process, ChangeState, '/slam_toolbox/change_state', request) require(response.success, 'Lifecycle transition failed') state = call_service(node, process, GetState, '/slam_toolbox/get_state', GetState.Request()) require(state.current_state.id == State.PRIMARY_STATE_ACTIVE, 'Mapper not active') def parameters(node, process, expected): request = GetParameters.Request() request.names = list(expected) response = call_service(node, process, GetParameters, '/slam_toolbox/get_parameters', request) fields = {1: 'bool_value', 2: 'integer_value', 3: 'double_value', 4: 'string_value'} actual = {} require(len(response.values) == len(expected), 'Incomplete parameter response') for name, value in zip(expected, response.values): require(value.type in fields, 'Parameter has unexpected type: ' + name) actual[name] = getattr(value, fields[value.type]) require(actual[name] == expected[name], 'Loaded parameter mismatch: ' + name) return actual def grid_dict(grid): p, q = grid.info.origin.position, grid.info.origin.orientation return {'frame': grid.header.frame_id, 'stamp_ns': stamp_ns(grid.header.stamp), 'resolution_m': grid.info.resolution, 'width': grid.info.width, 'height': grid.info.height, 'origin': {'x': p.x, 'y': p.y, 'z': p.z, 'qx': q.x, 'qy': q.y, 'qz': q.z, 'qw': q.w}, 'data': list(grid.data)} def check_grid(grid): require(grid.header.frame_id == 'map', 'Map frame differs') require(abs(grid.info.resolution - 0.05) < 1e-7, 'Map resolution differs') width, height = grid.info.width, grid.info.height values = list(grid.data) require(width > 0 and height > 0 and len(values) == width * height, 'Malformed map dimensions') require(set(values) <= {-1, 0, 100}, 'Unexpected cell encoding') counts = {'occupied': values.count(100), 'free': values.count(0), 'unknown': values.count(-1)} require(all(counts[k] > 0 for k in counts), 'Expected occupied, free and unknown cells') q = grid.info.origin.orientation require(abs(q.w - 1) < 1e-6 and abs(q.x) + abs(q.y) + abs(q.z) < 1e-6, 'This checker expects an axis-aligned grid') resolution, origin = grid.info.resolution, grid.info.origin.position def near(x, y, radius, target): matches = 0 for row in range(height): cy = origin.y + (row + 0.5) * resolution for column in range(width): cx = origin.x + (column + 0.5) * resolution if (cx - x) ** 2 + (cy - y) ** 2 <= radius ** 2: matches += values[row * width + column] == target return matches # Neighborhood checks allow discretization and small scan-matching differences. probes = {'left_wall': (-4., 0., 100), 'right_wall': (4., 0., 100), 'bottom_wall': (0., -3., 100), 'top_wall': (0., 3., 100), 'obstacle_front': (1.5, 1.5, 100), 'room_interior': (0., -1., 0)} checked = {} for name, (x, y, value) in probes.items(): x, y = rotate_room((x, y)) count = near(x, y, 0.2, value) require(count > 0, 'No expected map cell near reference geometry: ' + name) checked[name] = {'x_m': x, 'y_m': y, 'radius_m': 0.2, 'expected_value': value, 'matching_cells': count} return {'width': width, 'height': height, 'resolution_m': resolution, 'cell_counts': counts, 'geometry_probes': checked} def export_map(grid, work): # This small exporter writes the received grid; it does not call Nav2's saver. values = list(grid.data) width, height = grid.info.width, grid.info.height pixels = {100: 0, 0: 254, -1: 205} with (work / 'map.pgm').open('wb') as stream: stream.write(f'P5\n{width} {height}\n255\n'.encode()) for row in reversed(range(height)): stream.write(bytes(pixels[v] for v in values[row * width:(row + 1) * width])) (work / 'map.yaml').write_text(yaml.safe_dump({ 'image': 'map.pgm', 'mode': 'trinary', 'resolution': grid.info.resolution, 'origin': [grid.info.origin.position.x, grid.info.origin.position.y, 0.0], 'negate': 0, 'occupied_thresh': 0.65, 'free_thresh': 0.196}, sort_keys=False)) save_json(work / 'map.json', grid_dict(grid)) 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() def run_case(case, work, domain): work.mkdir() expected = yaml.safe_load((HERE / 'mapper.yaml').read_text())['slam_toolbox']['ros__parameters'] (work / 'mapper.yaml').write_text((HERE / 'mapper.yaml').read_text()) rclpy.init(domain_id=domain) fixture = Fixture() log = (work / 'mapper.log').open('w') environment = dict(os.environ, ROS_DOMAIN_ID=str(domain)) command = ['ros2', 'run', 'slam_toolbox', 'async_slam_toolbox_node', '--ros-args', '--params-file', str(HERE / 'mapper.yaml')] process = subprocess.Popen(command, stdout=log, stderr=subprocess.STDOUT, env=environment, start_new_session=True) try: activate(fixture, process) save_json(work / 'parameters.json', parameters(fixture, process, expected)) if case == 'complete-tf': fixture.publish_static() spin_until(fixture, lambda: fixture.scan_pub.get_subscription_count() > 0, 'mapper scan subscriber discovery', process) settle(fixture, 0.5, process) lookup_results = [] for index in range(STEPS): fixture.current_ns = START_NS + index * STEP_NS fixture.publish_clock() fixture.publish_odom(index) settle(fixture, 0.05, process) stamp = Time(nanoseconds=fixture.current_ns) available = fixture.buffer.can_transform('odom', 'laser', stamp) lookup_results.append(available) require(available == (case == 'complete-tf'), 'Unexpected fixture TF availability') fixture.scan_pub.publish(scan_for(index)) settle(fixture, 0.15, process) settle(fixture, 1.0, process) result = {'case': case, 'domain_id': domain, 'scans_supplied': STEPS, 'rays_per_scan': RAYS, 'laser_transform_available_per_scan': lookup_results, 'received_maps': len(fixture.maps), 'received_map_to_odom': len(fixture.transforms), 'received_accepted_poses': len(fixture.poses)} if case == 'missing-laser-tf': require(not fixture.maps, 'Broken frame chain unexpectedly produced a map') require(not fixture.transforms, 'Broken frame chain unexpectedly produced map->odom') require(not fixture.poses, 'Broken frame chain unexpectedly produced mapper pose') else: final_ns = START_NS + (STEPS - 1) * STEP_NS transform_ns = final_ns + round(expected['transform_timeout'] * NS) spin_until(fixture, lambda: fixture.maps and fixture.transforms and fixture.poses and stamp_ns(fixture.maps[-1].header.stamp) == final_ns and fixture.transforms[-1]['stamp_ns'] == transform_ns and fixture.poses[-1]['stamp_ns'] == final_ns, 'map, transform and accepted pose through the final scan', process) # A pose is published only after a successfully processed scan. Map # header time alone does not establish scan acceptance in this node. final_pose = fixture.poses[-1] require(final_pose['frame'] == 'map', 'Mapper pose frame differs') require(math.hypot(final_pose['x_m'] - fixture_pose(STEPS - 1)[0], final_pose['y_m']) < 0.2, 'Final accepted pose differs from reference') result['final_accepted_pose'] = final_pose result['map_stamp_ns'] = stamp_ns(fixture.maps[-1].header.stamp) export_map(fixture.maps[-1], work) save_json(work / 'map-to-odom.json', fixture.transforms) save_json(work / 'poses.json', fixture.poses) result['map_checks'] = check_grid(fixture.maps[-1]) result['received_maps'] = len(fixture.maps) result['received_map_to_odom'] = len(fixture.transforms) result['received_accepted_poses'] = len(fixture.poses) last = fixture.transforms[-1] require(all(math.isfinite(last[k]) for k in ('x_m', 'y_m', 'z_m', 'qx', 'qy', 'qz', 'qw')), 'Map->odom contains non-finite values') require(abs(sum(last[k] ** 2 for k in ('qx', 'qy', 'qz', 'qw')) - 1) < 1e-6, 'Map->odom quaternion is not normalized') require(math.hypot(last['x_m'], last['y_m']) < 0.2, 'Map->odom correction too large for ideal odometry') yaw = math.atan2(2 * (last['qw'] * last['qz'] + last['qx'] * last['qy']), 1 - 2 * (last['qy'] ** 2 + last['qz'] ** 2)) require(abs(yaw) < 0.05, 'Map->odom heading correction too large for ideal odometry') require(abs(last['z_m']) < 1e-6 and abs(last['qx']) + abs(last['qy']) < 1e-6, 'Map->odom correction is not planar') require(START_NS <= last['stamp_ns'] <= START_NS + STEPS * STEP_NS, 'Map->odom timestamp outside supplied experiment window') result['last_map_to_odom'] = last save_json(work / 'map-to-odom.json', fixture.transforms) save_json(work / 'poses.json', fixture.poses) save_json(work / 'report.json', result) return result finally: stop(process, log) fixture.destroy_node() rclpy.shutdown() def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--output', type=Path, help='New directory for maps, logs and checks; 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='slam-toolbox-lesson-') work = Path(temporary.name) try: installed = subprocess.check_output( ['dpkg-query', '-W', '-f=${Version}', 'ros-jazzy-slam-toolbox'], text=True, timeout=10).strip() require(installed == PACKAGE_VERSION, 'Unexpected SLAM Toolbox package: ' + installed) save_json(work / 'reference-geometry.json', {'room_local_bounds_m': ROOM, 'room_rotation_rad': ROOM_ROTATION, 'local_segments_m': LOCAL_SEGMENTS, 'segments_m': SEGMENTS, 'base_to_laser_x_m': LASER_X, 'ideal_odometry': True, 'noise': False, 'poses': [{'stamp_ns': START_NS + i * STEP_NS, 'x_m': fixture_pose(i)[0], 'y_m': fixture_pose(i)[1], 'yaw_rad': fixture_pose(i)[2]} for i in range(STEPS)]}) save_json(work / 'scans.json', [{'stamp_ns': stamp_ns(scan_for(i).header.stamp), 'frame': 'laser', 'ranges_m': list(scan_for(i).ranges)} for i in range(STEPS)]) results = [run_case(case, work / case, 75 + index) for index, case in enumerate(CASES)] save_json(work / 'report.json', {'cases': results}) print('PASS missing-laser-tf: supplied scans, no map, no map->odom') print('PASS complete-tf: occupied/free/unknown grid, six geometry probes, final accepted pose and map->odom') print(f'PASS fixture: {STEPS} scans × {RAYS} rays per case; identical room and odometry') print('PASS configuration: loaded values match mapper.yaml in both fresh processes') if args.output: print('ARTIFACTS ' + str(work)) else: print('ARTIFACTS temporary outputs checked and removed') finally: if temporary: temporary.cleanup() if __name__ == '__main__': main()