493 lines
26 KiB
Python
493 lines
26 KiB
Python
|
"""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))
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__": unittest.main()
|