#!/usr/bin/env python3 """Generated Navigate messages + real DDS + production C++ RosDriver regression. In a sourced ROS Humble overlay: python3 tests/helpers/native_navigation_contract.py --build-native --output /tmp/nav-contract An external temporary CMake project compiles the existing production driver/core; no ROS package CMake or installed executable is modified. All endpoints use a unique /sim namespace. This is contract evidence, not physical navigation evidence. """ import argparse from collections import Counter from copy import deepcopy import json import os from pathlib import Path import subprocess import tempfile import threading import time def build_native(root, directory): directory.mkdir(parents=True, exist_ok=True) # Paths are CMake quoted so workspace names containing spaces remain valid. q = lambda p: '"' + str(p).replace('\\', '/').replace('"', '\\"') + '"' (directory / 'CMakeLists.txt').write_text('''cmake_minimum_required(VERSION 3.16) project(native_navigation_probe LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(bt_skill_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nlohmann_json REQUIRED) add_executable(native_navigation_probe %s %s %s) target_include_directories(native_navigation_probe PRIVATE %s %s) target_link_libraries(native_navigation_probe nlohmann_json::nlohmann_json) ament_target_dependencies(native_navigation_probe rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs) ''' % tuple(q(root / p) for p in ( 'ros2/bt_executor/tools/native_navigation_probe.cpp', 'ros2/bt_executor/src/ros_driver.cpp', 'core/src/core.cpp', 'ros2/bt_executor/include', 'core/include')), encoding='utf-8') subprocess.run(['cmake', '-S', str(directory), '-B', str(directory / 'build')], check=True) subprocess.run(['cmake', '--build', str(directory / 'build'), '-j2'], check=True) return directory / 'build/native_navigation_probe' def wait(future, timeout=12): until = time.monotonic() + timeout while not future.done() and time.monotonic() < until: time.sleep(.01) assert future.done(), 'ROS future timed out' return future.result() def run(binary, output): import rclpy from rclpy.node import Node from rclpy.action import ActionClient, ActionServer, CancelResponse from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from bt_skill_interfaces.action import Navigate from bt_skill_interfaces.msg import NavigationResult from action_msgs.msg import GoalStatus assert [getattr(NavigationResult, name) for name in ('SUCCEEDED', 'CANCELED', 'TIMEOUT', 'BLOCKED', 'NOT_READY', 'FAILED')] == list(range(6)) assert [getattr(Navigate.Feedback, name) for name in ('ACCEPTED', 'CHECKING', 'NAVIGATING', 'BLOCKED', 'STOPPING')] == list(range(5)) assert set(Navigate.Goal.get_fields_and_field_types()) == { 'trace', 'target_pose', 'position_tolerance', 'yaw_tolerance', 'timeout'} assert set(Navigate.Result.get_fields_and_field_types()) == { 'result', 'final_pose_valid', 'final_pose', 'final_position_error', 'final_yaw_error'} assert set(Navigate.Feedback.get_fields_and_field_types()) == { 'stamp', 'sequence', 'phase', 'current_pose_valid', 'current_pose', 'error_valid', 'position_error', 'yaw_error', 'blocked', 'elapsed_time', 'message'} namespace = '/sim/navigation_contract_' + str(os.getpid()) rclpy.init() node = Node('navigation_contract_fixture', namespace=namespace) counts = Counter() failures = [] lock = threading.Lock() def execute(handle): goal = handle.request scenario = round(goal.target_pose.pose.position.x) status = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4}[scenario] with lock: counts[(goal.trace.task_id, scenario)] += 1 try: assert goal.target_pose.header.frame_id == 'map' assert goal.target_pose.pose.orientation.w == 1. assert goal.position_tolerance == .05 and goal.yaw_tolerance == .1 assert goal.timeout.sec == 5 assert goal.trace.attempt == 1 and goal.trace.task_revision == 1 assert goal.trace.plan_version == 1 and goal.trace.execution_generation == 1 except AssertionError: failures.append('Goal contract mismatch: ' + str(goal)) for phase in range(5): feedback = Navigate.Feedback() feedback.stamp = node.get_clock().now().to_msg() feedback.sequence = phase + 1 feedback.phase = phase feedback.current_pose_valid = True feedback.current_pose = goal.target_pose feedback.error_valid = True feedback.position_error = .02 feedback.yaw_error = -.03 feedback.blocked = phase == Navigate.Feedback.BLOCKED feedback.elapsed_time.nanosec = (phase + 1) * 10000000 feedback.message = 'phase-' + str(phase) handle.publish_feedback(feedback) time.sleep(.03) if phase == 2 and goal.trace.task_id.startswith('probe-'): # A later malformed sample must not poison the accepted sequence. wrong_frame = deepcopy(feedback) wrong_frame.sequence = 100 wrong_frame.current_pose.header.frame_id = 'odom' handle.publish_feedback(wrong_frame) wrong_phase = deepcopy(feedback) wrong_phase.sequence = 101 wrong_phase.phase = 255 handle.publish_feedback(wrong_phase) result = Navigate.Result() result.result.status = status result.result.error_code = '' if scenario in (7, 8) else 'FIXTURE_' + str(scenario) result.result.message = 'outcome-' + str(scenario) result.result.stop_state = 0 if scenario == 6 else 1 result.result.stopped_at = node.get_clock().now().to_msg() result.result.stop_evidence_ref = '' if scenario == 6 else 'sim://navigation/stop' result.final_pose_valid = True result.final_pose = goal.target_pose result.final_position_error = .02 result.final_yaw_error = -.03 if status == 1: deadline = time.monotonic() + 6 while not handle.is_cancel_requested and time.monotonic() < deadline: time.sleep(.01) if not handle.is_cancel_requested: failures.append('Canceled fixture never received cancellation') handle.abort() else: handle.canceled() elif status == 0: handle.succeed() else: handle.abort() return result server = ActionServer(node, Navigate, 'skills/navigate', execute_callback=execute, cancel_callback=lambda _: CancelResponse.ACCEPT, callback_group=ReentrantCallbackGroup()) client = ActionClient(node, Navigate, 'skills/navigate', callback_group=ReentrantCallbackGroup()) executor = MultiThreadedExecutor(num_threads=6) executor.add_node(node) thread = threading.Thread(target=executor.spin, daemon=True) thread.start() report = {'scope': 'simulation-only generated Navigate and production RosDriver over DDS', 'namespace': namespace, 'direct': [], 'native': []} try: assert client.wait_for_server(timeout_sec=10) for scenario in range(9): goal = Navigate.Goal() goal.trace.task_id = 'direct-' + str(scenario) goal.trace.run_id = goal.trace.task_id goal.trace.subtask_id = 'navigate' goal.trace.attempt = goal.trace.task_revision = goal.trace.plan_version = goal.trace.execution_generation = 1 goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = float(scenario) goal.target_pose.pose.orientation.w = 1. goal.position_tolerance = .05 goal.yaw_tolerance = .1 goal.timeout.sec = 5 feedback = [] handle = wait(client.send_goal_async(goal, feedback_callback=lambda value: feedback.append(value.feedback))) assert handle.accepted if scenario in (1, 6): assert wait(handle.cancel_goal_async()).return_code == 0 response = wait(handle.get_result_async()) expected = scenario if scenario < 6 else {6: 1, 7: 3, 8: 4}[scenario] assert response.result.result.status == expected assert response.status == (GoalStatus.STATUS_SUCCEEDED if expected == 0 else GoalStatus.STATUS_CANCELED if expected == 1 else GoalStatus.STATUS_ABORTED) assert response.result.final_pose_valid assert response.result.final_pose.pose.position.x == float(scenario) assert response.result.final_position_error == .02 and response.result.final_yaw_error == -.03 assert response.result.result.stop_state == (0 if scenario == 6 else 1) assert response.result.result.error_code == ('' if scenario in (7, 8) else 'FIXTURE_' + str(scenario)) assert [f.phase for f in feedback] == list(range(5)), feedback assert [f.sequence for f in feedback] == list(range(1, 6)) assert all(f.current_pose_valid and f.error_valid and f.position_error == .02 and f.yaw_error == -.03 and f.stamp.sec > 0 for f in feedback) assert feedback[3].blocked and not feedback[4].blocked report['direct'].append({'scenario': scenario, 'status': expected, 'feedback_phases': [f.phase for f in feedback]}) for scenario, expected_code in enumerate((0, 2, 3, 1, 4, 1, 2, 1, 4)): state = output / ('state-' + str(scenario)) state.mkdir(exist_ok=True) completed = subprocess.run([str(binary), str(scenario), str(state), namespace], text=True, capture_output=True, timeout=25) (output / ('native-' + str(scenario) + '.log')).write_text(completed.stdout + completed.stderr) if completed.returncode != 0: report['passed'] = False report['failure'] = {'scenario': scenario, 'returncode': completed.returncode, 'stdout': completed.stdout, 'stderr': completed.stderr, 'log': str(output / ('native-' + str(scenario) + '.log'))} (output / 'navigation-contract-report.json').write_text(json.dumps(report, indent=2) + '\n') raise AssertionError('Native probe failed: ' + json.dumps(report['failure'])) result = json.loads(next(line for line in reversed(completed.stdout.splitlines()) if line.startswith('{'))) assert result['code'] == expected_code, result assert result['stop'] == (0 if scenario == 6 else 1), result assert result['state'] == (3 if scenario == 6 else 4), result assert result['robot_locked'] == (scenario == 6), result assert result['unknown_stop_blocks_redispatch'] == (scenario == 6), result assert result['error_code'] == ({7: 'NAV_BLOCKED', 8: 'NAV_NOT_READY'}.get(scenario, 'FIXTURE_' + str(scenario))), result assert result['feedback_sequence'] == 5, result assert json.loads(result['feedback'])['phase'] == 4, result assert result['mapping_count'] == 1 and result['wire_result_bytes'] > 0, result assert 'Navigate' in result['wire_request_type'] and 'Navigate' in result['wire_result_type'], result assert result['response_valid'], result report['native'].append(result) assert not failures, failures assert len(counts) == 18 and all(count == 1 for count in counts.values()), dict(counts) report['exactly_once_goal_count'] = sum(counts.values()) report['passed'] = True (output / 'navigation-contract-report.json').write_text(json.dumps(report, indent=2) + '\n') print(json.dumps(report, indent=2)) finally: executor.shutdown() thread.join(timeout=3) client.destroy() server.destroy() node.destroy_node() rclpy.shutdown() if __name__ == '__main__': parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--root', type=Path, default=Path(__file__).resolve().parents[2]) parser.add_argument('--output', type=Path, required=True) parser.add_argument('--native-probe', type=Path) parser.add_argument('--build-native', action='store_true') args = parser.parse_args() args.output = args.output.resolve() args.output.mkdir(parents=True, exist_ok=True) with tempfile.TemporaryDirectory(prefix='native-navigation-build-') as temporary: binary = build_native(args.root.resolve(), Path(temporary)) if args.build_native else args.native_probe if binary is None: parser.error('Supply --build-native or --native-probe') run(binary.resolve(), args.output)