fix: align navigation contract and readiness handling
This commit is contained in:
@@ -32,7 +32,7 @@ def goal_for(name):
|
||||
if name == "navigate":
|
||||
goal.target_pose.header.frame_id = "map"
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
goal.position_tolerance = goal.yaw_tolerance = 0.1
|
||||
elif name == "navigate_semantic":
|
||||
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
|
||||
@@ -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)
|
||||
@@ -33,7 +33,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
if name == "navigate":
|
||||
goal.target_pose.header.frame_id = "map"
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
goal.position_tolerance = goal.yaw_tolerance = 0.1
|
||||
elif name == "navigate_semantic":
|
||||
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
@@ -89,7 +89,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
handle, result = self.execute(name, {"kind": positive_kind, "duration_seconds": 0})
|
||||
self.assertEqual(handle.native, "succeeded")
|
||||
if hasattr(result, "result"):
|
||||
self.assertEqual(result.result.status, result.result.COMPLETED)
|
||||
self.assertEqual(result.result.status, result.result.SUCCEEDED if name == "navigate" else result.result.COMPLETED)
|
||||
self.assertEqual(result.result.stop_state, result.result.CONFIRMED)
|
||||
elif hasattr(result, "status"):
|
||||
self.assertNotEqual(result.status, result.FAILED)
|
||||
@@ -118,7 +118,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
|
||||
def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self):
|
||||
cases = {
|
||||
"navigate": [0, 1, 2, 5],
|
||||
"navigate": [0, 1, 2],
|
||||
"execute_manipulation": [0, 1, 2, 3, 4],
|
||||
"execute_posture": [0, 1, 2],
|
||||
}
|
||||
@@ -131,7 +131,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertTrue(all(item.stamp.sec > 0 and item.message for item in handle.feedback))
|
||||
self.assertNotIn(getattr(self.module.ACTION_TYPES[name].Feedback, "STOPPING"), expected)
|
||||
nav = self.execute("navigate", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1]
|
||||
self.assertTrue(nav.pose_valid and nav.errors_valid and nav.blocked_valid)
|
||||
self.assertTrue(nav.current_pose_valid and nav.error_valid)
|
||||
self.assertFalse(nav.blocked)
|
||||
self.assertGreaterEqual(nav.elapsed_time.nanosec, 0)
|
||||
manipulation = self.execute("execute_manipulation", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1]
|
||||
@@ -139,7 +139,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
|
||||
def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self):
|
||||
expected = {
|
||||
"navigate": [0, 1, 2, 5], "execute_manipulation": [0, 1, 2, 3, 4],
|
||||
"navigate": [0, 1, 2], "execute_manipulation": [0, 1, 2, 3, 4],
|
||||
"plan_task": [0, 1, 2], "locate_shelf_column": [0, 1],
|
||||
"localize_target_3d": [0, 1], "check_free_space": [0, 1],
|
||||
"assess_grasp": [0], "execute_posture": [0, 1, 2], "verify_state": [0],
|
||||
@@ -153,7 +153,8 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertEqual([item.phase for item in handle.feedback], phases)
|
||||
self.node.counts["navigate"] = 0
|
||||
recovery, _ = self.execute("navigate", {"kind": "obstacle_recovery", "duration_seconds": 0.7})
|
||||
self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 4, 5])
|
||||
self.assertEqual([item.phase for item in recovery.feedback], [0, 1, 2, 3, 2])
|
||||
self.assertEqual([item.blocked for item in recovery.feedback], [False, False, False, True, False])
|
||||
self.node.counts["execute_task"] = 0
|
||||
task, _ = self.execute("execute_task", {"kind": "normal", "duration_seconds": 0.05})
|
||||
self.assertTrue(task.feedback[0].stage)
|
||||
@@ -181,7 +182,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
|
||||
timeout_handle = GoalHandle(goal); self.node._accepted("navigate", timeout_handle)
|
||||
timeout_result = self.node._execute("navigate", timeout_handle)
|
||||
self.assertEqual((timeout_handle.native, timeout_result.result.status), ("aborted", timeout_result.result.TIMED_OUT))
|
||||
self.assertEqual((timeout_handle.native, timeout_result.result.status), ("aborted", timeout_result.result.TIMEOUT))
|
||||
self.assertEqual(timeout_handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING)
|
||||
self.node.motion_reserved = False; self.node.counts["navigate"] = 0
|
||||
unknown_handle, unknown = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
||||
@@ -205,6 +206,63 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.assertEqual(timed_out.result.stop_state, timed_out.result.UNKNOWN)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
|
||||
def test_navigation_status_mapping_and_readiness_reasons(self):
|
||||
cases = [("normal", 0, "succeeded", ""), ("canceled", 1, "canceled", "MOCK_TERMINATED"),
|
||||
("timeout", 2, "aborted", "MOCK_TERMINATED"), ("blocked", 3, "aborted", "BLOCKED"),
|
||||
("failed", 5, "aborted", "MOCK_TERMINATED")]
|
||||
cases += [("not_ready", 4, "aborted", code) for code in
|
||||
("INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED",
|
||||
"ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED")]
|
||||
for kind, status, native, code in cases:
|
||||
self.node.counts["navigate"] = 0
|
||||
fixture = {"kind": "normal" if kind == "canceled" else kind, "duration_seconds": 0}
|
||||
if kind == "not_ready": fixture["error_code"] = code
|
||||
self.module.parse_scenarios(json.dumps({"navigate": fixture}))
|
||||
handle, result = self.execute("navigate", fixture, cancel=kind == "canceled")
|
||||
with self.subTest(kind=kind, code=code):
|
||||
self.assertEqual((result.result.status, handle.native), (status, native))
|
||||
self.assertEqual(result.result.error_code, code)
|
||||
self.assertEqual(result.final_pose_valid, status == 0)
|
||||
if result.final_pose_valid:
|
||||
self.assertEqual((result.final_position_error, result.final_yaw_error), (0.0, 0.0))
|
||||
self.assertFalse(hasattr(result, "errors_valid"))
|
||||
self.node.counts["navigate"] = 0
|
||||
_, result = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
||||
self.assertEqual((result.result.status, result.result.stop_state), (5, 0))
|
||||
self.assertEqual((result.result.stopped_at.sec, result.result.stopped_at.nanosec,
|
||||
result.result.stop_evidence_ref), (0, 0, ""))
|
||||
|
||||
def test_navigation_goal_requires_map_and_finite_nonzero_quaternion(self):
|
||||
mutations = (
|
||||
lambda g: setattr(g.target_pose.header, "frame_id", "odom"),
|
||||
lambda g: setattr(g.target_pose.pose.orientation, "w", 0.0),
|
||||
lambda g: setattr(g.target_pose.pose.orientation, "w", float("nan")),
|
||||
lambda g: setattr(g.target_pose.pose.position, "x", float("nan")),
|
||||
lambda g: setattr(g, "yaw_tolerance", float("nan")),
|
||||
)
|
||||
for mutate in mutations:
|
||||
self.node.inflight, self.node.motion_reserved = 0, False
|
||||
goal = self.goal("navigate"); mutate(goal)
|
||||
with self.subTest(mutate=mutate):
|
||||
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.REJECT)
|
||||
|
||||
def test_navigation_accepts_large_yaw_tolerance_and_normalizes_quaternion(self):
|
||||
for magnitude in (2.0, 1e308, 1e-308):
|
||||
goal = self.goal("navigate")
|
||||
goal.yaw_tolerance = 4.0
|
||||
goal.target_pose.pose.orientation.w = magnitude
|
||||
self.node.scenarios["navigate"] = [{"kind": "normal", "duration_seconds": 0.1}]
|
||||
with self.subTest(magnitude=magnitude):
|
||||
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
|
||||
handle = GoalHandle(goal); self.node._accepted("navigate", handle)
|
||||
result = self.node._execute("navigate", handle)
|
||||
self.assertEqual(result.result.status, 0)
|
||||
self.assertTrue(result.final_pose_valid)
|
||||
self.assertEqual(result.final_pose.pose.orientation.w, 1.0)
|
||||
self.assertTrue(handle.feedback)
|
||||
self.assertTrue(all(item.current_pose.pose.orientation.w == 1.0 for item in handle.feedback))
|
||||
self.assertEqual(goal.target_pose.pose.orientation.w, magnitude)
|
||||
|
||||
def test_action_specific_malformed_goals_are_rejected(self):
|
||||
mutations = {
|
||||
"navigate": lambda g: setattr(g.target_pose.header, "frame_id", ""),
|
||||
@@ -387,7 +445,7 @@ class MockRuntimeTests(unittest.TestCase):
|
||||
self.node.counts["navigate"] = 0
|
||||
handle, result = self.execute("navigate", {"kind": "native_mismatch", "duration_seconds": 0})
|
||||
self.assertEqual(handle.native, "aborted")
|
||||
self.assertEqual(result.result.status, result.result.COMPLETED)
|
||||
self.assertEqual(result.result.status, result.result.SUCCEEDED)
|
||||
|
||||
def test_topic_and_result_fixture_values_are_finite_and_bounded(self):
|
||||
for raw in (
|
||||
|
||||
@@ -8,3 +8,35 @@ class CatalogTests(unittest.TestCase):
|
||||
self.assertEqual(c.resolve('CELL','',3,shelf='s',side='FRONT',column='1',tier='2')['location_id'],'stop')
|
||||
for args in [('OBJECT','unknown',3),('OBJECT','water',2),('LOCATION','water',3)]:
|
||||
with self.assertRaises(ValueError):c.resolve(*args)
|
||||
|
||||
class SemanticTranslationTests(unittest.TestCase):
|
||||
def test_navigation_status_is_translated_not_copied(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from navigation_gateway import semantic_proxy as m
|
||||
nav = NS(SUCCEEDED=0, CANCELED=1, TIMEOUT=2, BLOCKED=3, NOT_READY=4, FAILED=5, UNKNOWN=0, CONFIRMED=1)
|
||||
old = NS(COMPLETED=0, FAILED=1, CANCELED=2, TIMED_OUT=3, REJECTED=4, UNKNOWN=0, CONFIRMED=1)
|
||||
self.assertTrue(hasattr(m, 'translate_navigation_result'))
|
||||
for status, expected in ((0,0),(1,2),(2,3),(3,1),(4,4),(5,1)):
|
||||
source = NS(result=NS(status=status, stop_state=1, error_code='INPUTS_UNHEALTHY', message='source', stopped_at=NS(sec=123,nanosec=9), stop_evidence_ref='proof'), final_pose_valid=False, final_pose=object(), final_position_error=float('nan'), final_yaw_error=float('nan'))
|
||||
target = NS(result=NS())
|
||||
m.translate_navigation_result(source, target, nav, old)
|
||||
self.assertEqual(target.result.status, expected)
|
||||
self.assertEqual((target.result.error_code, target.result.stop_state, target.result.stopped_at.sec, target.result.stop_evidence_ref), ('INPUTS_UNHEALTHY',1,123,'proof'))
|
||||
self.assertFalse(target.errors_valid)
|
||||
|
||||
def test_invalid_pose_does_not_make_default_zero_errors_valid(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from navigation_gateway.semantic_proxy import translate_navigation_result
|
||||
nav=NS(SUCCEEDED=0,CANCELED=1,TIMEOUT=2,BLOCKED=3,NOT_READY=4,FAILED=5,UNKNOWN=0,CONFIRMED=1)
|
||||
old=NS(COMPLETED=0,FAILED=1,CANCELED=2,TIMED_OUT=3,REJECTED=4,UNKNOWN=0,CONFIRMED=1)
|
||||
for status,code in [(3,'NAV_BLOCKED'),(4,'NAV_NOT_READY')]:
|
||||
source=NS(result=NS(status=status,stop_state=0,error_code='',message='',stopped_at=None,stop_evidence_ref=''),final_pose_valid=False,final_pose=None,final_position_error=0.,final_yaw_error=0.)
|
||||
target=NS(result=NS())
|
||||
translate_navigation_result(source,target,nav,old)
|
||||
self.assertFalse(target.errors_valid)
|
||||
self.assertEqual(target.result.error_code,code)
|
||||
source.final_pose_valid=True
|
||||
for p,y in [(-1.,0.),(0.,4.)]:
|
||||
source.final_position_error=p;source.final_yaw_error=y
|
||||
translate_navigation_result(source,target,nav,old)
|
||||
self.assertFalse(target.errors_valid)
|
||||
|
||||
@@ -489,4 +489,158 @@ class ProxyTests(unittest.TestCase):
|
||||
self.assertTrue(any(e.error for e in events))
|
||||
|
||||
|
||||
|
||||
|
||||
class NavigationWireTests(unittest.TestCase):
|
||||
def test_blocked_unknown_is_not_reported_as_clear(self):
|
||||
s = module.GatewaySnapshot.parse(snapshot(), ID)
|
||||
self.assertIsNone(getattr(s, 'blocked', 'missing'))
|
||||
with self.assertRaises(module.GatewayError):
|
||||
module.assign_navigation_feedback(object(), s)
|
||||
|
||||
def test_explicit_blocked_and_error_code_survive_parse(self):
|
||||
s = module.GatewaySnapshot.parse(snapshot(blocked=True, error_code='INPUTS_UNHEALTHY'), ID)
|
||||
self.assertEqual((getattr(s, 'blocked', None), getattr(s, 'error_code', None)), (True, 'INPUTS_UNHEALTHY'))
|
||||
for invalid in (0, 'false'):
|
||||
with self.assertRaises(module.GatewayError):
|
||||
module.GatewaySnapshot.parse(snapshot(blocked=invalid), ID)
|
||||
|
||||
def test_new_feedback_maps_yaw_and_blocked_phase(self):
|
||||
from types import SimpleNamespace as NS
|
||||
f = NS(STOPPING=4, CHECKING=1, NAVIGATING=2, BLOCKED=3, elapsed_time=NS(sec=0, nanosec=0))
|
||||
s = module.GatewaySnapshot.parse(snapshot(blocked=True, position_error=.5, yaw_error=-.2), ID)
|
||||
self.assertTrue(hasattr(module, 'assign_navigation_feedback'))
|
||||
module.assign_navigation_feedback(f, s)
|
||||
self.assertEqual((f.phase, f.blocked, f.error_valid, f.yaw_error), (3, True, True, -.2))
|
||||
self.assertFalse(f.current_pose_valid)
|
||||
|
||||
def test_goal_uses_yaw_tolerance(self):
|
||||
from types import SimpleNamespace as NS
|
||||
goal = NS(trace=NS(**{k: 't' if k in ('task_id', 'subtask_id', 'run_id') else 1 for k in module._TRACE_FIELDS}), timeout=NS(sec=2, nanosec=0), position_tolerance=.1, yaw_tolerance=.23,
|
||||
target_pose=NS(header=NS(frame_id='map'), pose=NS(position=NS(x=1., y=2., z=0.), orientation=NS(x=0., y=0., z=0., w=1.))))
|
||||
self.assertEqual(module.build_goal_body(goal, ID, 'map-a')['yaw_tolerance'], .23)
|
||||
|
||||
class GatewayTelemetryTests(unittest.TestCase):
|
||||
setUp = GatewayTests.setUp
|
||||
def test_only_fresh_explicit_blocked_sample_is_published(self):
|
||||
self.gateway.submit(request())
|
||||
self.backend.set_snapshot(request()['goal_id'], 'ACTIVE', pose=request()['target_pose'])
|
||||
sample = self.backend.samples[request()['goal_id']]
|
||||
sample['blocked'] = dict(self.backend.health_value, value=True)
|
||||
self.assertIs(self.gateway.poll(request()['goal_id']).get('blocked'), True)
|
||||
self.clock.advance(1)
|
||||
self.assertIsNone(self.gateway.poll(request()['goal_id']).get('blocked'))
|
||||
|
||||
def test_readiness_code_requires_fresh_explicit_backend_diagnostic(self):
|
||||
self.gateway.submit(request())
|
||||
self.backend.health_sample(False)
|
||||
self.backend.health_value['error_code'] = 'INPUTS_UNHEALTHY'
|
||||
out = self.gateway.poll(request()['goal_id'])
|
||||
self.assertEqual(out.get('error_code'), 'INPUTS_UNHEALTHY')
|
||||
self.assertEqual(out['stop_state'], 'UNKNOWN')
|
||||
|
||||
class NavigationOutcomeTests(unittest.TestCase):
|
||||
def test_terminal_status_mapping_distinguishes_timeout_blocked_and_not_ready(self):
|
||||
from types import SimpleNamespace as NS
|
||||
enum = NS(SUCCEEDED=0, CANCELED=1, TIMEOUT=2, BLOCKED=3, NOT_READY=4, FAILED=5)
|
||||
for outcome, expected in [('COMPLETED',0),('CANCELED',1),('TIMED_OUT',2),('BLOCKED',3),('NOT_READY',4),('REJECTED',5),('FAILED',5)]:
|
||||
value = module.GatewaySnapshot.parse(snapshot(outcome=outcome), ID)
|
||||
self.assertEqual(module.navigation_status(value, enum), expected)
|
||||
value = module.GatewaySnapshot.parse(snapshot(outcome='FAILED', error_code='ROBOT_STATE_UNAVAILABLE'), ID)
|
||||
self.assertEqual(module.navigation_status(value, enum), 4)
|
||||
|
||||
class GoalValidationTests(unittest.TestCase):
|
||||
def test_trace_ranges_quaternion_and_yaw_are_checked_before_network(self):
|
||||
from types import SimpleNamespace as NS
|
||||
goal = NS(trace=NS(task_id='t', subtask_id='s', run_id='r', attempt=1, task_revision=1, plan_version=1, execution_generation=1), timeout=NS(sec=2,nanosec=0), position_tolerance=.1, yaw_tolerance=.2, target_pose=NS(header=NS(frame_id='map'), pose=NS(position=NS(x=0.,y=0.,z=0.),orientation=NS(x=0.,y=0.,z=0.,w=1.))))
|
||||
for field,value in [('run_id',''),('attempt',True),('attempt',2**32),('task_revision',0),('plan_version',True),('execution_generation',2**64)]:
|
||||
bad=copy.deepcopy(goal);setattr(bad.trace,field,value)
|
||||
with self.subTest(field=field), self.assertRaises(ValueError):module.build_goal_body(bad,ID,'map-a')
|
||||
goal.yaw_tolerance=4.
|
||||
goal.target_pose.pose.orientation.w=2.
|
||||
body=module.build_goal_body(goal,ID,'map-a')
|
||||
self.assertEqual(body['yaw_tolerance'],4.)
|
||||
self.assertEqual(body['target_pose']['orientation']['w'],1.)
|
||||
self.assertEqual(validate_goal(body)['yaw_tolerance'],4.)
|
||||
goal.target_pose.pose.orientation.w=0.
|
||||
with self.assertRaises(ValueError):module.build_goal_body(goal,ID,'map-a')
|
||||
|
||||
class BackendDiagnosticTests(unittest.TestCase):
|
||||
def test_health_callback_keeps_explicit_machine_readable_cause(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from navigation_gateway.backends import Ros1MoveBaseBackend
|
||||
backend = Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend)
|
||||
backend.map_id='map-a'; backend.lock=threading.RLock()
|
||||
backend._source_metadata=lambda stamp: {'source_fresh':True,'source_stamp':stamp}
|
||||
backend._health(NS(data=json.dumps({'ready':False,'map_id':'map-a','stamp':12.,'error_code':'INPUTS_UNHEALTHY'})))
|
||||
self.assertEqual(backend.health_value.get('error_code'), 'INPUTS_UNHEALTHY')
|
||||
backend._health(NS(data=json.dumps({'ready':False,'map_id':'map-a','stamp':12.,'reason':'some prose'})))
|
||||
self.assertEqual(backend.health_value.get('error_code', ''), '')
|
||||
|
||||
class LocalNotReadyTests(unittest.IsolatedAsyncioTestCase):
|
||||
async def test_valid_unready_goal_returns_not_ready_without_http_submit(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from unittest.mock import patch, Mock
|
||||
class Node:
|
||||
def __init__(self,*args):pass
|
||||
def create_timer(self,*args):pass
|
||||
nav=NS(SUCCEEDED=0,CANCELED=1,TIMEOUT=2,BLOCKED=3,NOT_READY=4,FAILED=5,UNKNOWN=0,CONFIRMED=1)
|
||||
imports={'rclpy':NS(), 'rclpy.action':NS(ActionServer=lambda *a,**k:NS(),CancelResponse=NS(ACCEPT=1),GoalResponse=NS(ACCEPT=1,REJECT=2)),
|
||||
'rclpy.node':NS(Node=Node),'rclpy.task':NS(Future=lambda:object()),'rclpy.callback_groups':NS(ReentrantCallbackGroup=lambda:object()),
|
||||
'bt_skill_interfaces':NS(), 'bt_skill_interfaces.action':NS(Navigate=NS(Result=lambda:NS(result=NS()))),'bt_skill_interfaces.msg':NS(NavigationResult=nav)}
|
||||
goal=NS(trace=NS(task_id='t',subtask_id='s',run_id='r',attempt=1,task_revision=1,plan_version=1,execution_generation=1),timeout=NS(sec=2,nanosec=0),position_tolerance=.1,yaw_tolerance=.2,target_pose=NS(header=NS(frame_id='map'),pose=NS(position=NS(x=0.,y=0.,z=0.),orientation=NS(x=0.,y=0.,z=0.,w=1.))))
|
||||
config=module.ProxyConfig(token='a-long-test-token',connect_timeout_sec=.1,read_timeout_sec=.1,request_timeout_sec=.2,poll_interval_sec=.01,readiness_max_age_sec=.5,feedback_silence_timeout_sec=.5)
|
||||
client=Mock()
|
||||
with patch.dict(sys.modules,imports), patch.object(module,'GatewayClient',return_value=client), patch.object(module.threading,'Thread'):
|
||||
node=module.create_ros_node(config,map_id='map-a',action_name='skills/navigate')
|
||||
node._health_at=time.monotonic();node._health_error_code='INPUTS_UNHEALTHY'
|
||||
self.assertEqual(node._accept(goal),1)
|
||||
handle=NS(request=goal,goal_id=NS(uuid=list(__import__('uuid').UUID(ID).bytes)),abort=Mock())
|
||||
result=await node._execute(handle)
|
||||
self.assertEqual((result.result.status,result.result.error_code,result.result.stop_state),(4,'INPUTS_UNHEALTHY',0))
|
||||
self.assertFalse(node._reserved)
|
||||
client.submit.assert_not_called()
|
||||
node._health_at=0
|
||||
self.assertEqual(node._accept(goal),1)
|
||||
result=await node._execute(handle)
|
||||
self.assertEqual(result.result.error_code,'NAV_NOT_READY')
|
||||
client.submit.assert_not_called()
|
||||
|
||||
class BlockedAdmissionTests(unittest.TestCase):
|
||||
setUp = GatewayTests.setUp
|
||||
def test_missing_or_stale_blocked_rejects_before_send(self):
|
||||
for blocked in (None, {'value':False,'received_at':0.,'source_fresh':False}):
|
||||
self.backend.health_sample(True)
|
||||
self.backend.health_value['blocked']=blocked
|
||||
self.assertEqual(self.gateway.health()['error_code'],'INPUTS_UNHEALTHY')
|
||||
with self.assertRaises(GatewayError):self.gateway.submit(request())
|
||||
self.assertEqual(self.backend.send_count,0)
|
||||
|
||||
def test_explicit_simulation_blocked_can_progress_then_loss_cancels(self):
|
||||
self.assertTrue(self.gateway.health()['ready'])
|
||||
self.gateway.submit(request())
|
||||
for blocked in (False,True):
|
||||
self.backend.set_snapshot(request()['goal_id'],'ACTIVE',pose=request()['target_pose'],blocked=blocked)
|
||||
out=self.gateway.poll(request()['goal_id'])
|
||||
self.assertIs(out['blocked'],blocked)
|
||||
self.assertEqual(self.backend.cancel_count,0)
|
||||
self.backend.set_snapshot(request()['goal_id'],'ACTIVE',pose=request()['target_pose'],blocked=None)
|
||||
out=self.gateway.poll(request()['goal_id'])
|
||||
self.assertEqual((self.backend.cancel_count,out['error_code'],out['stop_state']),(1,'INPUTS_UNHEALTHY','UNKNOWN'))
|
||||
|
||||
def test_ros_health_carries_only_explicit_blocked_with_original_source_time(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from navigation_gateway.backends import Ros1MoveBaseBackend
|
||||
backend=Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend)
|
||||
backend.map_id='sim-map';backend.lock=threading.RLock();backend.connected=True
|
||||
backend._source_metadata=lambda stamp:{'source_stamp':stamp,'source_fresh':True}
|
||||
backend.source_is_fresh=lambda sample:sample.get('source_fresh') is True
|
||||
for blocked in (False,True,None,0):
|
||||
payload={'ready':True,'map_id':'sim-map','stamp':100.,'blocked':blocked}
|
||||
backend._health(NS(data=json.dumps(payload)))
|
||||
sample=backend.health().get('blocked')
|
||||
if type(blocked) is bool:
|
||||
self.assertEqual((sample['value'],sample['source_stamp']),(blocked,100.))
|
||||
else:self.assertIsNone(sample)
|
||||
|
||||
if __name__ == "__main__": unittest.main()
|
||||
|
||||
@@ -79,21 +79,29 @@ class RosContractTests(unittest.TestCase):
|
||||
goal, result, feedback = sections("Navigate")
|
||||
self.assertEqual(goal, "\n".join([
|
||||
"bt_skill_interfaces/TaskTrace trace", "geometry_msgs/PoseStamped target_pose",
|
||||
"float64 position_tolerance", "float64 orientation_tolerance", "builtin_interfaces/Duration timeout",
|
||||
"float64 position_tolerance", "float64 yaw_tolerance", "builtin_interfaces/Duration timeout",
|
||||
]))
|
||||
self.assertEqual(result, "\n".join([
|
||||
"bt_skill_interfaces/ExecutionResult result", "bool pose_valid", "geometry_msgs/PoseStamped final_pose",
|
||||
"bool errors_valid", "float64 final_position_error", "float64 final_orientation_error",
|
||||
"bt_skill_interfaces/NavigationResult result", "bool final_pose_valid", "geometry_msgs/PoseStamped final_pose",
|
||||
"float64 final_position_error", "float64 final_yaw_error",
|
||||
]))
|
||||
self.assertEqual(feedback, "\n".join([
|
||||
"uint8 CHECKING=0", "uint8 PLANNING=1", "uint8 NAVIGATING=2", "uint8 WAITING_OBSTACLE=3",
|
||||
"uint8 RECOVERING=4", "uint8 ARRIVING=5", "uint8 STOPPING=6", "builtin_interfaces/Time stamp",
|
||||
"uint32 sequence", "uint8 phase", "bool pose_valid", "geometry_msgs/PoseStamped current_pose",
|
||||
"bool errors_valid", "float64 position_error", "float64 orientation_error", "bool blocked_valid",
|
||||
"uint8 ACCEPTED=0", "uint8 CHECKING=1", "uint8 NAVIGATING=2", "uint8 BLOCKED=3",
|
||||
"uint8 STOPPING=4", "builtin_interfaces/Time stamp",
|
||||
"uint32 sequence", "uint8 phase", "bool current_pose_valid", "geometry_msgs/PoseStamped current_pose",
|
||||
"bool error_valid", "float64 position_error", "float64 yaw_error",
|
||||
"bool blocked", "builtin_interfaces/Duration elapsed_time", "string message",
|
||||
]))
|
||||
self.assertFalse((INTERFACES / "action" / "ExecuteNavigation.action").exists())
|
||||
|
||||
def test_navigation_result_is_separate_from_other_skill_results(self):
|
||||
self.assertEqual(fields(INTERFACES / "msg" / "NavigationResult.msg"), [
|
||||
"uint8 SUCCEEDED=0", "uint8 CANCELED=1", "uint8 TIMEOUT=2", "uint8 BLOCKED=3",
|
||||
"uint8 NOT_READY=4", "uint8 FAILED=5", "uint8 UNKNOWN=0", "uint8 CONFIRMED=1",
|
||||
"uint8 status", "string error_code", "string message", "uint8 stop_state",
|
||||
"builtin_interfaces/Time stopped_at", "string stop_evidence_ref",
|
||||
])
|
||||
|
||||
def test_manipulation_exact_source_outer_contract(self):
|
||||
goal, result, feedback = sections("ExecuteManipulation")
|
||||
self.assertEqual(goal, "\n".join([
|
||||
@@ -156,7 +164,7 @@ class RosContractTests(unittest.TestCase):
|
||||
def test_all_idl_files_are_registered_in_build(self):
|
||||
cmake = (INTERFACES / "CMakeLists.txt").read_text()
|
||||
declarations = re.findall(r'"((?:msg|srv|action)/[^"\n]+)"', cmake)
|
||||
actual = sorted(str(p.relative_to(INTERFACES)) for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action"))
|
||||
actual = sorted(p.relative_to(INTERFACES).as_posix() for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action"))
|
||||
self.assertEqual(sorted(declarations), actual)
|
||||
self.assertEqual(len(declarations), len(set(declarations)))
|
||||
manifest = ET.parse(INTERFACES / "package.xml").getroot()
|
||||
|
||||
Reference in New Issue
Block a user