"""Safety contract tests; no ROS installation or robot required.""" import copy import json import sys import tempfile import threading import unittest import urllib.error import urllib.request from pathlib import Path PROJECT_ROOT = Path(__file__).resolve().parents[1] if str(PROJECT_ROOT) not in sys.path: sys.path.insert(0, str(PROJECT_ROOT)) try: from navigation_gateway.gateway import Gateway, GatewayError, SafetyConfig, validate_goal from navigation_gateway.backends import MockBackend from navigation_gateway.server import make_server IMPORT_ERROR = None except ImportError as exc: IMPORT_ERROR = str(exc) class Clock: def __init__(self): self.now = 100.0 def __call__(self): return self.now def advance(self, seconds): self.now += seconds def request(goal_id="11111111-1111-4111-8111-111111111111"): return {"goal_id": goal_id, "trace": {"task_id": "task-1", "subtask_id": "navigate", "attempt": 1}, "map_id": "sim-map", "target_pose": {"frame_id": "map", "position": {"x": 1.0, "y": 2.0, "z": 0.0}, "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}, "position_tolerance": 0.05, "yaw_tolerance": 0.05, "timeout_sec": 10.0} class GatewayTests(unittest.TestCase): def setUp(self): self.assertIsNone(IMPORT_ERROR, "navigation gateway implementation missing: " + str(IMPORT_ERROR)) self.tmp = tempfile.TemporaryDirectory() self.clock = Clock() self.config = SafetyConfig(odom_max_age_sec=0.2, stationary_window_sec=0.3, linear_stopped_mps=0.005, angular_stopped_radps=0.005, pose_max_age_sec=0.2, readiness_max_age_sec=0.5, stop_wait_timeout_sec=1.0) self.backend = MockBackend(self.clock, map_id="sim-map", source_clock=lambda: 1700000000.0 + self.clock()) self.path = str(Path(self.tmp.name) / "journal.sqlite3") self.gateway = Gateway(self.path, self.backend, self.config, self.clock) self.addCleanup(self.tmp.cleanup) self.addCleanup(lambda: self.gateway.close()) def sample(self, state="ACTIVE", linear=0.0, pose=None, fresh=True): self.backend.health_sample(True) self.backend.set_snapshot(request()["goal_id"], state, linear=linear, angular=0.0, pose=pose or request()["target_pose"], source_fresh=fresh) return self.gateway.poll(request()["goal_id"]) def stop_window(self, state="SUCCEEDED", pose=None): out = self.sample(state, pose=pose) for _ in range(4): self.clock.advance(0.1) out = self.sample(state, pose=pose) return out def test_goal_is_sent_once_and_replay_is_immutable(self): first = self.gateway.submit(request()) self.assertEqual(first["goal_id"], self.gateway.submit(copy.deepcopy(request()))["goal_id"]) self.assertEqual(self.backend.send_count, 1) changed = request(); changed["timeout_sec"] = 8 with self.assertRaises(GatewayError) as cm: self.gateway.submit(changed) self.assertEqual(cm.exception.http_status, 409) self.assertEqual(self.backend.send_count, 1) def test_goal_validation_rejects_nonfinite_bad_quaternion_and_duration(self): for path, value in [(('position_tolerance',), float('nan')), (('yaw_tolerance',), -1), (('timeout_sec',), float('inf')), (('timeout_sec',), 0), (('target_pose', 'position', 'x'), float('inf')), (('target_pose', 'orientation', 'w'), 0), (('target_pose', 'frame_id'), 'odom')]: body = request(); node = body for key in path[:-1]: node = node[key] node[path[-1]] = value with self.subTest(path=path, value=value), self.assertRaises(GatewayError): validate_goal(body) def test_readiness_cannot_be_claimed_by_http_caller(self): self.backend.health_sample(False) with self.assertRaises(GatewayError): self.gateway.submit(request()) self.assertEqual(self.backend.send_count, 0) body = request(); body["ready"] = True with self.assertRaises(GatewayError): self.gateway.submit(body) def test_stale_health_and_wrong_map_reject_before_sending(self): self.clock.advance(1) with self.assertRaises(GatewayError): self.gateway.submit(request()) self.backend.health_sample(True) body = request(); body["map_id"] = "another-map" with self.assertRaises(GatewayError): self.gateway.submit(body) self.assertEqual(self.backend.send_count, 0) def test_cancel_ack_is_not_stop_confirmation_and_cancel_is_idempotent(self): self.gateway.submit(request()) out = self.gateway.cancel(request()["goal_id"]) self.assertEqual(out["stop_state"], "UNKNOWN") self.gateway.cancel(request()["goal_id"]) self.assertEqual(self.backend.cancel_count, 1) self.assertEqual(self.stop_window("ACTIVE")["stop_state"], "UNKNOWN") stopped = self.stop_window("PREEMPTED") self.assertEqual((stopped["outcome"], stopped["stop_state"]), ("CANCELED", "CONFIRMED")) def test_success_requires_terminal_pose_and_continuous_fresh_odom(self): self.gateway.submit(request()) out = self.sample("SUCCEEDED") self.assertEqual(out["stop_state"], "UNKNOWN") self.clock.advance(0.5) self.assertEqual(self.gateway.poll(request()["goal_id"])["stop_state"], "UNKNOWN") out = self.stop_window() self.assertEqual((out["outcome"], out["stop_state"]), ("COMPLETED", "CONFIRMED")) def test_stale_source_odom_never_confirms_stop(self): self.gateway.submit(request()) for _ in range(10): self.clock.advance(0.1) out = self.sample("SUCCEEDED", fresh=False) self.assertEqual(out["stop_state"], "UNKNOWN") def test_moving_sample_resets_stationary_window(self): self.gateway.submit(request()); self.sample("SUCCEEDED") self.clock.advance(0.15); self.sample("SUCCEEDED", linear=0.1) self.clock.advance(0.15); self.sample("SUCCEEDED") self.clock.advance(0.15) self.assertEqual(self.sample("SUCCEEDED")["stop_state"], "UNKNOWN") def test_odom_samples_between_polls_cannot_hide_motion(self): self.gateway.submit(request()); self.sample("SUCCEEDED") for _ in range(2): self.clock.advance(0.1) self.sample("SUCCEEDED") self.clock.advance(0.05) self.backend.set_snapshot(request()["goal_id"], "SUCCEEDED", linear=0.1, pose=request()["target_pose"]) self.clock.advance(0.1) self.backend.set_snapshot(request()["goal_id"], "SUCCEEDED", linear=0.0, pose=request()["target_pose"]) out = self.gateway.poll(request()["goal_id"]) self.assertEqual(out["stop_state"], "UNKNOWN") def test_readiness_loss_during_motion_requests_stop(self): self.gateway.submit(request()) self.backend.health_sample(False) out = self.gateway.poll(request()["goal_id"]) self.assertEqual(self.backend.cancel_count, 1) self.assertEqual(out["stop_state"], "UNKNOWN") out = self.stop_window("PREEMPTED") self.assertEqual(out["outcome"], "FAILED") def test_success_with_health_lost_does_not_complete(self): self.gateway.submit(request()); self.sample("SUCCEEDED") self.backend.health_sample(False) self.gateway.poll(request()["goal_id"]) out = self.stop_window("SUCCEEDED") self.assertEqual(out["outcome"], "FAILED") def test_success_outside_tolerance_becomes_failed_not_completed(self): self.gateway.submit(request()) pose = copy.deepcopy(request()["target_pose"]); pose["position"]["x"] = 2.0 out = self.stop_window(pose=pose) self.assertEqual((out["outcome"], out["stop_state"]), ("FAILED", "CONFIRMED")) self.assertIn("tolerance", out["message"]) def test_execution_timeout_cancels_once_and_waits_for_actual_stop(self): body = request(); body["timeout_sec"] = 0.2 self.gateway.submit(body); self.clock.advance(0.3) out = self.gateway.poll(body["goal_id"]) self.assertEqual(out["stop_state"], "UNKNOWN") self.assertEqual(self.backend.cancel_count, 1) out = self.stop_window("PREEMPTED") self.assertEqual(out["outcome"], "TIMED_OUT") def test_ambiguous_send_is_quarantined_and_never_retried(self): self.backend.send_mode = "UNKNOWN" out = self.gateway.submit(request()) self.assertEqual(out["status"], "STOP_UNKNOWN") self.gateway.submit(request()) with self.assertRaises(GatewayError): self.gateway.submit(request("22222222-2222-4222-8222-222222222222")) self.assertEqual(self.backend.send_count, 1) self.assertEqual(self.stop_window()["stop_state"], "UNKNOWN") self.assertEqual(self.backend.cancel_count, 1) def test_controller_state_loss_requests_cancel_and_keeps_lock(self): self.gateway.submit(request()) self.backend.set_snapshot(request()["goal_id"], "LOST", pose=request()["target_pose"]) out = self.gateway.poll(request()["goal_id"]) self.assertEqual(self.backend.cancel_count, 1) self.assertEqual(out["stop_state"], "UNKNOWN") with self.assertRaises(GatewayError): self.gateway.submit(request("22222222-2222-4222-8222-222222222222")) def test_second_process_cannot_open_the_same_robot_journal(self): with self.assertRaises(RuntimeError): Gateway(self.path, self.backend, self.config, self.clock) def test_completed_record_survives_restart_without_resending(self): self.gateway.submit(request()); expected = self.stop_window() self.gateway.close() self.clock.advance(100) self.gateway = Gateway(self.path, self.backend, self.config, self.clock) actual = self.gateway.submit(request()) self.assertEqual(actual, expected) self.assertEqual(self.backend.send_count, 1) def test_stop_proof_keeps_original_source_timestamp_across_queries_and_restart(self): self.gateway.submit(request()) out = self.stop_window() self.assertEqual(out["stopped_at"], 1700000000.0 + self.clock()) original_stamp = out["stopped_at"] self.clock.advance(50) self.assertEqual(self.gateway.poll(request()["goal_id"])["stopped_at"], original_stamp) self.gateway.close() self.gateway = Gateway(self.path, self.backend, self.config, self.clock) self.assertEqual(self.gateway.get(request()["goal_id"])["stopped_at"], original_stamp) def test_rejected_goal_requires_stop_evidence(self): self.backend.send_mode = "REJECTED" out = self.gateway.submit(request()) self.assertEqual(out["stop_state"], "UNKNOWN") out = self.stop_window("REJECTED") self.assertEqual((out["outcome"], out["stop_state"]), ("REJECTED", "CONFIRMED")) def test_restart_locks_unfinished_goal_and_does_not_resend(self): self.gateway.submit(request()); self.gateway.close() self.gateway = Gateway(self.path, self.backend, self.config, self.clock) old = self.gateway.get(request()["goal_id"]) self.assertEqual(old["status"], "STOP_UNKNOWN") self.assertTrue(old["quarantined"]) self.gateway.submit(request()) with self.assertRaises(GatewayError): self.gateway.submit(request("22222222-2222-4222-8222-222222222222")) self.assertEqual(self.backend.send_count, 1) def test_http_all_routes_require_bearer_and_goal_query_cancel_work(self): server = make_server(self.gateway, "test-secret-token", host="127.0.0.1", port=0) thread = threading.Thread(target=server.serve_forever, daemon=True); thread.start() self.addCleanup(server.server_close); self.addCleanup(server.shutdown) base = f"http://127.0.0.1:{server.server_address[1]}" with self.assertRaises(urllib.error.HTTPError) as cm: urllib.request.urlopen(base + "/healthz") self.assertEqual(cm.exception.code, 401) def call(path, body=None): data = None if body is None else json.dumps(body).encode() req = urllib.request.Request(base + path, data=data, headers={"Authorization": "Bearer test-secret-token", "Content-Type": "application/json"}) with urllib.request.urlopen(req, timeout=2) as response: return json.load(response) self.assertTrue(call("/healthz")["ready"]) self.assertEqual(call("/v1/goals", request())["goal_id"], request()["goal_id"]) self.assertEqual(call("/v1/goals/" + request()["goal_id"])["status"], "ACTIVE") self.assertEqual(call("/v1/goals/" + request()["goal_id"] + "/cancel", {})["stop_state"], "UNKNOWN") class RosSourceTimeTests(unittest.TestCase): """Exercise actual Noetic callbacks and gateway checks with injected clocks.""" def setUp(self): from types import SimpleNamespace as NS from unittest.mock import patch from navigation_gateway.backends import Ros1MoveBaseBackend self.NS = NS self.ros_time, self.monotonic_time = [100.0], [10.0] self.backend = Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend) self.backend.rospy = NS(Time=NS(now=lambda: NS(to_sec=lambda: self.ros_time[0]))) self.backend.max_age = 1.0 self.backend.map_id = "sim-map" self.backend.lock = threading.RLock() self.backend.sequence = 0 self.backend.previous_odom_stamp = None self.backend.odom_queue = [] self.gateway = Gateway.__new__(Gateway) self.gateway.backend = self.backend self.gateway.clock = lambda: self.monotonic_time[0] self.patch = patch("navigation_gateway.backends.time.monotonic", lambda: self.monotonic_time[0]) self.patch.start() self.addCleanup(self.patch.stop) def receive_all(self, stamp): NS = self.NS header = NS(stamp=NS(to_sec=lambda: stamp), frame_id="map") self.backend._odom(NS(header=header, twist=NS(twist=NS(linear=NS(x=0., y=0., z=0.), angular=NS(x=0., y=0., z=0.))))) self.backend._pose(NS(header=header, pose=NS(position=NS(x=1., y=2., z=0.), orientation=NS(x=0., y=0., z=0., w=1.)))) self.backend._health(NS(data=json.dumps({"ready": True, "map_id": "sim-map", "stamp": stamp}))) return (self.backend.odom, self.backend.pose, self.backend.health_value) def test_source_age_is_rechecked_even_when_receipt_is_fresh(self): samples = self.receive_all(99.1) self.assertTrue(all(self.gateway._fresh(sample, 1.0) for sample in samples)) self.ros_time[0], self.monotonic_time[0] = 100.5, 10.5 for sample in samples: with self.subTest(sample=sample): self.assertFalse(self.gateway._fresh(sample, 1.0)) def test_backward_ros_jump_invalidates_old_epoch_even_when_age_is_in_range(self): samples = self.receive_all(99.8) self.ros_time[0], self.monotonic_time[0] = 99.9, 10.05 for sample in samples: with self.subTest(sample=sample): self.assertFalse(self.gateway._fresh(sample, 1.0)) self.ros_time[0] = 100.1 self.assertTrue(all(not self.gateway._fresh(sample, 1.0) for sample in samples)) def test_future_source_time_is_never_admitted_later_as_a_cached_sample(self): samples = self.receive_all(100.1) self.assertTrue(all(not self.gateway._fresh(sample, 1.0) for sample in samples)) self.ros_time[0], self.monotonic_time[0] = 100.2, 10.1 self.assertTrue(all(not self.gateway._fresh(sample, 1.0) for sample in samples)) # HTTP proxy contract tests use real loopback sockets; ROS imports remain lazy. from navigation_gateway import ros2_proxy as module import time from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer ID = '11111111-1111-4111-8111-111111111111' BODY = {'goal_id': ID, 'trace': {'task_id': 't', 'subtask_id': 's', 'attempt': 1}, 'map_id': 'map-a', 'target_pose': {'frame_id': 'map', 'position': {'x': 1., 'y': 2., 'z': 0.}, 'orientation': {'x': 0., 'y': 0., 'z': 0., 'w': 1.}}, 'position_tolerance': .1, 'yaw_tolerance': .2, 'timeout_sec': 2.} def snapshot(**updates): value = {'goal_id': ID, 'status': 'ACTIVE', 'outcome': None, 'stop_state': 'UNKNOWN', 'controller_state': 'ACTIVE', 'message': '', 'position_error': None, 'yaw_error': None, 'sequence': 1, 'elapsed': .1, 'stopped_at': 1234.5} value.update(updates) return value class ProxyTests(unittest.TestCase): def setUp(self): self.assertIsNotNone(module, 'ROS2 proxy behavior is not implemented') self.requests = [] self.reply = snapshot() self.delay = 0 self.dribble = False self.cancel_reply = None owner = self class Handler(BaseHTTPRequestHandler): def do_GET(self): self.respond() def do_POST(self): self.respond() def log_message(self, *args): pass def respond(self): raw = self.rfile.read(int(self.headers.get('Content-Length', '0'))) owner.requests.append((self.command, self.path, self.headers.get('Authorization'), json.loads(raw) if raw else None)) if owner.delay: time.sleep(owner.delay) value = {'ready': True, 'reason': '', 'map_id': 'map-a'} if self.path == '/healthz' else copy.deepcopy(owner.reply) if self.path.endswith('/cancel') and owner.cancel_reply is not None: value = owner.cancel_reply data = json.dumps(value).encode() try: self.send_response(200) self.send_header('Content-Length', str(len(data))) self.end_headers() if owner.dribble: for byte in data: self.wfile.write(bytes([byte])) self.wfile.flush() time.sleep(.005) else: self.wfile.write(data) except (BrokenPipeError, ConnectionResetError): pass self.server = ThreadingHTTPServer(('127.0.0.1', 0), Handler) self.server.daemon_threads = True self.thread = threading.Thread(target=self.server.serve_forever, daemon=True) self.thread.start() self.config = module.ProxyConfig(endpoint='http://127.0.0.1:%d' % self.server.server_port, token='test-secret-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) self.client = module.GatewayClient(self.config) def tearDown(self): if hasattr(self, 'server'): self.server.shutdown() self.server.server_close() def wait_for(self, predicate): deadline = time.monotonic() + 2 while time.monotonic() < deadline: if predicate(): return time.sleep(.005) self.fail('condition not reached') def test_body_is_frozen_and_all_http_routes_are_authenticated(self): body = copy.deepcopy(BODY) session = module.GoalSession(self.client, body) body['trace']['task_id'] = 'changed' self.client.health() session.start() self.wait_for(lambda: len(self.requests) >= 3) self.assertEqual(self.requests[1][3]['trace']['task_id'], 't') session.request_cancel() self.wait_for(lambda: any(r[1].endswith('/cancel') for r in self.requests)) self.assertTrue(all(r[2] == 'Bearer test-secret-token' for r in self.requests)) self.assertEqual([r[3]['goal_id'] for r in self.requests if r[1] == '/v1/goals'], [ID]) self.reply = snapshot(status='TERMINAL', outcome='CANCELED', stop_state='CONFIRMED', controller_state='PREEMPTED', sequence=9) events = [] self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.terminal for e in events)) self.assertTrue(any(e.terminal and e.snapshot.outcome == 'CANCELED' for e in events)) def test_cancel_ack_and_unconfirmed_terminal_do_not_finish(self): session = module.GoalSession(self.client, BODY) session.start() self.wait_for(lambda: len(self.requests) >= 2) self.cancel_reply = snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', stop_state='CONFIRMED', sequence=8) session.request_cancel() self.wait_for(lambda: any(r[1].endswith('/cancel') for r in self.requests)) self.reply = snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', sequence=2) time.sleep(.06) self.assertFalse(any(e.terminal for e in session.drain_events())) self.reply = snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', stop_state='CONFIRMED', sequence=3) events = [] self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.terminal for e in events)) self.assertEqual([e.snapshot.stop_state for e in events if e.terminal], ['CONFIRMED']) def test_transport_timeout_produces_unknown_error_and_bounded_wait(self): self.delay = .7 session = module.GoalSession(self.client, BODY) started = time.monotonic() session.start() self.assertLess(time.monotonic() - started, .05) events = [] self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error for e in events)) self.assertLess(time.monotonic() - started, .6) self.assertFalse(any(e.terminal for e in events)) self.assertTrue(any(e.error for e in events)) def test_completion_outside_tolerance_never_releases(self): self.reply = snapshot(status='TERMINAL', outcome='COMPLETED', controller_state='SUCCEEDED', stop_state='CONFIRMED', position_error=.9, yaw_error=.01) session = module.GoalSession(self.client, BODY) session.start() events = [] self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error or e.terminal for e in events)) self.assertFalse(any(e.terminal for e in events)) self.assertTrue(any(e.error for e in events)) def test_native_arrived_with_stop_and_tolerances_completes(self): self.reply = snapshot(status='TERMINAL', outcome='COMPLETED', controller_state='ARRIVED', stop_state='CONFIRMED', position_error=.02, yaw_error=.01) session = module.GoalSession(self.client, BODY) session.start() events = [] self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error or e.terminal for e in events)) self.assertTrue(any(e.terminal for e in events)) self.assertFalse(any(e.error for e in events)) def test_short_token_is_rejected_before_network_io(self): from dataclasses import replace with self.assertRaises(ValueError): replace(self.config, token='short') def test_actual_pose_reaches_ros_pose_stamped_without_invented_stamp(self): from types import SimpleNamespace as NS pose = {'frame_id': 'map', 'position': {'x': 1.0, 'y': 2.0, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}} value = module.GatewaySnapshot.parse(snapshot(pose_valid=True, current_pose=pose, final_pose=pose), ID) self.assertTrue(hasattr(value, 'pose_valid'), 'actual pose evidence is missing from gateway snapshot') self.assertTrue(value.pose_valid) target = NS(header=NS(frame_id='', stamp=NS(sec=0, nanosec=0)), pose=NS(position=NS(x=0., y=0., z=0.), orientation=NS(x=0., y=0., z=0., w=0.))) module.assign_ros_pose(target, value.final_pose) self.assertEqual((target.header.frame_id, target.pose.position.x, target.pose.position.y, target.pose.orientation.w), ('map', 1.0, 2.0, 1.0)) self.assertEqual((target.header.stamp.sec, target.header.stamp.nanosec), (0, 0)) def test_invalid_observed_pose_is_rejected(self): for changes in ({'frame_id': 'odom'}, {'orientation': {'x': 0., 'y': 0., 'z': 0., 'w': 0.}}, {'position': {'x': float('nan'), 'y': 0., 'z': 0.}}): pose = {'frame_id': 'map', 'position': {'x': 1., 'y': 2., 'z': 0.}, 'orientation': {'x': 0., 'y': 0., 'z': 0., 'w': 1.}} pose.update(changes) with self.assertRaises(module.GatewayError): module.GatewaySnapshot.parse(snapshot(pose_valid=True, final_pose=pose), ID) def test_confirmed_terminal_rejects_missing_or_invalid_stop_stamp(self): for stamp in (None, 0.0, -1.0, True, float('nan'), float('inf'), 2147483648.0): with self.subTest(stamp=stamp), self.assertRaises(module.GatewayError): module.GatewaySnapshot.parse(snapshot(status='TERMINAL', outcome='CANCELED', controller_state='PREEMPTED', stop_state='CONFIRMED', stopped_at=stamp), ID) def test_ros_stop_time_normalizes_nanosecond_rounding_and_range(self): from types import SimpleNamespace as NS self.assertTrue(hasattr(module, 'assign_ros_time'), 'source stop time mapping is missing') for source, expected in ((1234.5, (1234, 500000000)), (1.9999999996, (2, 0)), (2147483647.0, (2147483647, 0))): result = NS(sec=0, nanosec=0) module.assign_ros_time(result, source) self.assertEqual((result.sec, result.nanosec), expected) for invalid in (-1.0, 2147483648.0, float('inf'), True): with self.assertRaises(module.GatewayError): module.assign_ros_time(NS(sec=0, nanosec=0), invalid) def test_repeated_query_preserves_original_stop_evidence_stamp(self): self.reply = snapshot(status='TERMINAL', outcome='COMPLETED', controller_state='SUCCEEDED', stop_state='CONFIRMED', position_error=.02, yaw_error=.01, stopped_at=1234.5) first = self.client.query(ID) second = self.client.query(ID) self.assertTrue(hasattr(first, 'stopped_at'), 'original stop evidence timestamp was dropped') self.assertEqual((first.stopped_at, second.stopped_at), (1234.5, 1234.5)) def test_request_deadline_bounds_slow_trickle_response(self): self.dribble = True started = time.monotonic() with self.assertRaises(module.GatewayError): self.client.query(ID) self.assertLess(time.monotonic() - started, .4) def test_mismatched_uuid_never_completes_current_goal(self): self.reply = snapshot(goal_id='22222222-2222-4222-8222-222222222222', status='TERMINAL', outcome='COMPLETED', controller_state='SUCCEEDED', stop_state='CONFIRMED') session = module.GoalSession(self.client, BODY) session.start() events=[] self.wait_for(lambda: bool(events.extend(session.drain_events())) or any(e.error for e in events)) self.assertFalse(any(e.terminal for e in events)) 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()