fix: align navigation contract and readiness handling
This commit is contained in:
@@ -0,0 +1,250 @@
|
||||
#!/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)
|
||||
Reference in New Issue
Block a user