#!/usr/bin/env python3 """Generated NavigateToPose 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(navigation_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 navigation_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 navigation_interfaces.action import NavigateToPose as Navigate from action_msgs.msg import GoalStatus assert [getattr(Navigate.Result, name) for name in ('SUCCEEDED', 'CANCELED', 'TIMEOUT', 'BLOCKED', 'NOT_READY', 'FAILED')] == list(range(6)) assert [getattr(Navigate.Feedback, name) for name in ('ACCEPTED', 'CHECKING', 'PLANNING', 'NAVIGATING', 'BLOCKED', 'STOPPING')] == list(range(6)) assert set(Navigate.Goal.get_fields_and_field_types()) == { 'task_id', 'subtask_id', 'target_pose', 'position_tolerance', 'yaw_tolerance', 'timeout'} assert set(Navigate.Result.get_fields_and_field_types()) == { 'status', 'error_code', 'message', 'final_pose_valid', 'final_pose', 'final_position_error', 'final_yaw_error', 'stop_state', 'stopped_at', 'stop_evidence_ref'} assert set(Navigate.Feedback.get_fields_and_field_types()) == { 'stamp', 'sequence', 'phase', 'current_pose_valid', 'current_pose', 'error_valid', 'position_error', 'yaw_error', 'blocked_valid', 'blocked', 'elapsed_time', 'message'} assert Navigate.Feedback.get_fields_and_field_types()['sequence'] == 'uint64' assert (Navigate.Result.STOP_UNKNOWN, Navigate.Result.STOP_CONFIRMED) == (0, 1) 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, 9: 1, 10: 4}[scenario] with lock: counts[(goal.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.task_id and goal.subtask_id == 'navigate' except AssertionError: failures.append('Goal contract mismatch: ' + str(goal)) for phase in range(6): feedback = Navigate.Feedback() feedback.stamp = node.get_clock().now().to_msg() feedback.sequence = (1 << 32) + 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_valid = phase != Navigate.Feedback.STOPPING 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.task_id.startswith('probe-'): # A later malformed sample must not poison the accepted sequence. wrong_frame = deepcopy(feedback) wrong_frame.sequence = (1 << 32) + 100 wrong_frame.current_pose.header.frame_id = 'odom' handle.publish_feedback(wrong_frame) wrong_phase = deepcopy(feedback) wrong_phase.sequence = (1 << 32) + 101 wrong_phase.phase = 255 handle.publish_feedback(wrong_phase) result = Navigate.Result() result.status = status result.error_code = '' if scenario in (7, 8) else 'EXECUTION_BACKEND_NOT_CONFIGURED' if scenario == 10 else 'FIXTURE_' + str(scenario) result.message = 'outcome-' + str(scenario) result.stop_state = 0 if scenario in (6, 10) else 1 result.stopped_at = node.get_clock().now().to_msg() result.stop_evidence_ref = '' if scenario in (6, 9, 10) else 'sim://navigation/stop' result.final_pose_valid = scenario != 10 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 NavigateToPose and production RosDriver over DDS', 'namespace': namespace, 'direct': [], 'native': []} try: assert client.wait_for_server(timeout_sec=10) for scenario in range(11): goal = Navigate.Goal() goal.task_id = 'direct-' + str(scenario) goal.subtask_id = 'navigate' 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, 9): 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, 9: 1, 10: 4}[scenario] assert response.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 == (scenario != 10) 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.stop_state == (0 if scenario in (6, 10) else 1) assert response.result.error_code == ('' if scenario in (7, 8) else 'EXECUTION_BACKEND_NOT_CONFIGURED' if scenario == 10 else 'FIXTURE_' + str(scenario)) assert [f.phase for f in feedback] == list(range(6)), feedback assert [f.sequence for f in feedback] == [(1 << 32) + n for n in range(1, 7)] 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[4].blocked_valid and feedback[4].blocked assert not feedback[5].blocked_valid and not feedback[5].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, 2, 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 in (6, 9, 10) else 1), result assert result['state'] == (3 if scenario in (6, 9, 10) else 4), result assert result['robot_locked'] == (scenario in (6, 9, 10)), result assert result['unknown_stop_blocks_redispatch'] == (scenario in (6, 9, 10)), result assert result['error_code'] == ({7: 'NAV_BLOCKED', 8: 'NAV_NOT_READY', 10: 'EXECUTION_BACKEND_NOT_CONFIGURED'}.get(scenario, 'FIXTURE_' + str(scenario))), result assert result['feedback_sequence'] == (1 << 32) + 6, result assert json.loads(result['feedback'])['phase'] == 5 assert json.loads(result['feedback'])['blocked_valid'] is False, result assert json.loads(result['feedback'])['blocked'] is None, result assert result['mapping_count'] == 1 and result['wire_result_bytes'] > 0, result assert result['wire_request_type'] == 'navigation_interfaces/action/NavigateToPose_Goal', result assert result['wire_result_type'] == 'navigation_interfaces/action/NavigateToPose_Result', result assert result['response_valid'] == (scenario != 10), result report['native'].append(result) assert not failures, failures assert len(counts) == 22 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)