实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Test support for executing ROS adapters without a ROS installation."""
|
||||
@@ -0,0 +1,155 @@
|
||||
#!/usr/bin/env python3
|
||||
"""ROS Humble smoke test for bt_mock_servers; run only in a built ROS overlay."""
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
|
||||
from bt_mock_servers.mock_skills import ACTION_ENDPOINTS, ACTION_TYPES, MockSkills
|
||||
from bt_skill_interfaces.msg import VerificationEvidence
|
||||
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal
|
||||
|
||||
|
||||
def trace(value):
|
||||
value.task_id, value.subtask_id = "smoke-task", "smoke-step"
|
||||
value.attempt = value.task_revision = value.plan_version = 1
|
||||
value.run_id, value.execution_generation = "smoke-run", 1
|
||||
|
||||
|
||||
def goal_for(name):
|
||||
goal = ACTION_TYPES[name].Goal()
|
||||
goal.timeout.sec = 2
|
||||
if hasattr(goal, "trace"):
|
||||
trace(goal.trace)
|
||||
if hasattr(goal, "task_id"):
|
||||
goal.task_id = "smoke-task"
|
||||
if hasattr(goal, "subtask_id"):
|
||||
goal.subtask_id = "smoke-step"
|
||||
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
|
||||
elif name == "navigate_semantic":
|
||||
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
elif name == "execute_manipulation":
|
||||
goal.skill, goal.instruction = "pick", "pick the smoke target"
|
||||
goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
|
||||
elif name == "execute_posture":
|
||||
goal.posture_id, goal.expected_geometry_epoch = "home", 1
|
||||
elif name == "plan_task":
|
||||
goal.task_id, goal.task_revision, goal.planning_generation = "smoke-task", 1, 1
|
||||
goal.instruction = "fetch smoke target"
|
||||
goal.known_info_json = goal.context_snapshot_json = goal.constraints_json = "{}"
|
||||
elif name == "verify_state":
|
||||
goal.check, goal.expected_geometry_epoch = goal.PRECHECK, 1
|
||||
goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
|
||||
elif name == "locate_shelf_column":
|
||||
goal.target_ref, goal.target_description = "smoke-target", "smoke target"
|
||||
goal.source_region_ref, goal.observation_station_id = "shelf_zone", "station_A"
|
||||
goal.station_registry_version = 1
|
||||
elif name == "localize_target_3d":
|
||||
goal.target_ref, goal.target_description, goal.expected_geometry_epoch = "smoke-target", "smoke target", 1
|
||||
goal.shelf_id, goal.column_id, goal.station_binding_ref = "shelf_A", "1", "station_A"
|
||||
elif name == "check_free_space":
|
||||
goal.destination_ref, goal.destination_description = "bin_A", "bin A"
|
||||
goal.object_ref, goal.object_description, goal.placement_constraints_json = "smoke-target", "smoke target", "{}"
|
||||
elif name == "assess_grasp":
|
||||
goal.allowed_posture_ids = ["pregrasp"]
|
||||
goal.target_binding.target.object_ref, goal.target_binding.target.description = "smoke-target", "smoke target"
|
||||
goal.target_binding.context.schema_version = goal.target_binding.context.geometry_epoch = 1
|
||||
goal.robot_state.robot_id = "robot_01"
|
||||
goal.robot_state.valid_until.sec = 2_000_000_000
|
||||
elif name == "evaluate_progress":
|
||||
goal.task_description, goal.sequence = "fetch smoke target", 1
|
||||
goal.window_json = '[{"stamp":1,"views":{"front":"/tmp/smoke.png"}}]'
|
||||
elif name == "execute_task":
|
||||
goal.approved_plan_json, goal.context_json = '{"subtasks":[]}', "{}"
|
||||
return goal
|
||||
|
||||
|
||||
def await_future(future, seconds=5.0):
|
||||
deadline = time.monotonic() + seconds
|
||||
while not future.done() and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
if not future.done():
|
||||
raise TimeoutError("ROS smoke future timed out")
|
||||
return future.result()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
server, client_node = MockSkills(), Node("bt_mock_live_smoke", namespace="/sim/robot_01")
|
||||
executor = MultiThreadedExecutor(num_threads=8)
|
||||
executor.add_node(server); executor.add_node(client_node)
|
||||
thread = threading.Thread(target=executor.spin, daemon=True); thread.start()
|
||||
try:
|
||||
for name, action_type in ACTION_TYPES.items():
|
||||
feedback = []
|
||||
client = ActionClient(client_node, action_type, ACTION_ENDPOINTS[name])
|
||||
if not client.wait_for_server(timeout_sec=3.0):
|
||||
raise RuntimeError("unreachable action: " + name)
|
||||
handle = await_future(client.send_goal_async(
|
||||
goal_for(name), feedback_callback=lambda item, out=feedback: out.append(item.feedback)))
|
||||
if not handle.accepted:
|
||||
raise RuntimeError("valid smoke goal rejected: " + name)
|
||||
wrapped = await_future(handle.get_result_async())
|
||||
if wrapped.result is None:
|
||||
raise RuntimeError("missing result: " + name)
|
||||
if not feedback:
|
||||
raise RuntimeError("missing feedback: " + name)
|
||||
client.destroy()
|
||||
|
||||
server.scenarios["navigate"] = [{"kind": "cancel_stop_unknown", "duration_seconds": 1.0}]
|
||||
cancel_client = ActionClient(client_node, ACTION_TYPES["navigate"], ACTION_ENDPOINTS["navigate"])
|
||||
handle = await_future(cancel_client.send_goal_async(goal_for("navigate")))
|
||||
cancel = await_future(handle.cancel_goal_async())
|
||||
if not cancel.goals_canceling:
|
||||
raise RuntimeError("cancel was not acknowledged")
|
||||
canceled = await_future(handle.get_result_async())
|
||||
if (canceled.result.result.status != canceled.result.result.CANCELED or
|
||||
canceled.result.result.stop_state != canceled.result.result.UNKNOWN):
|
||||
raise RuntimeError("cancel terminal did not preserve unknown stop")
|
||||
|
||||
state_client = client_node.create_client(GetRobotState, "get_robot_state")
|
||||
if not state_client.wait_for_service(timeout_sec=3.0):
|
||||
raise RuntimeError("GetRobotState unavailable")
|
||||
state = await_future(state_client.call_async(GetRobotState.Request(robot_id="robot_01")))
|
||||
if not state.available:
|
||||
raise RuntimeError("GetRobotState did not return simulator state")
|
||||
|
||||
reconcile_client = client_node.create_client(ReconcileGoal, "reconcile_goal")
|
||||
if not reconcile_client.wait_for_service(timeout_sec=3.0):
|
||||
raise RuntimeError("ReconcileGoal unavailable")
|
||||
request = ReconcileGoal.Request()
|
||||
trace(request.trace)
|
||||
request.goal_id = bytes(handle.goal_id.uuid).hex()
|
||||
request.operator_id, request.reason = "smoke", "live smoke"
|
||||
evidence = request.evidence
|
||||
evidence.status = VerificationEvidence.PASSED
|
||||
evidence.context.trace = request.trace
|
||||
evidence.context.source_goal_id = request.goal_id
|
||||
evidence.context.writer = "independent_smoke_observer"
|
||||
evidence.context.observed_at = client_node.get_clock().now().to_msg()
|
||||
evidence.context.valid_until.sec = evidence.context.observed_at.sec + 5
|
||||
evidence.context.valid_until.nanosec = evidence.context.observed_at.nanosec
|
||||
evidence.stopped_valid = evidence.stopped = True
|
||||
evidence.source = "SIMULATOR_INDEPENDENT_FIXTURE"
|
||||
evidence.evidence_ref = "sim://smoke/stopped"
|
||||
reconciled = await_future(reconcile_client.call_async(request))
|
||||
if not reconciled.accepted:
|
||||
raise RuntimeError("bound reconciliation rejected: " + reconciled.error_code)
|
||||
print(json.dumps({"actions": len(ACTION_TYPES), "services": 2, "status": "passed"}))
|
||||
finally:
|
||||
executor.shutdown(timeout_sec=2.0)
|
||||
server.destroy_node(); client_node.destroy_node()
|
||||
if rclpy.ok(): rclpy.shutdown()
|
||||
thread.join(timeout=2.0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,234 @@
|
||||
"""Minimal ROS runtime and message classes generated from repository IDL."""
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import pathlib
|
||||
import sys
|
||||
import types
|
||||
|
||||
ROOT = pathlib.Path(__file__).resolve().parents[2]
|
||||
IDL = ROOT / "ros2" / "bt_skill_interfaces"
|
||||
|
||||
|
||||
def _module(name):
|
||||
module = types.ModuleType(name)
|
||||
sys.modules[name] = module
|
||||
return module
|
||||
|
||||
|
||||
def _fields(path, section=0):
|
||||
parts = [[]]
|
||||
for raw in path.read_text().splitlines():
|
||||
line = raw.split("#", 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
if line == "---":
|
||||
parts.append([])
|
||||
else:
|
||||
parts[-1].append(line)
|
||||
return parts[section]
|
||||
|
||||
|
||||
class RosClock:
|
||||
_nanoseconds = 10_000_000_000
|
||||
|
||||
def now(self):
|
||||
self._nanoseconds += 1_000_000
|
||||
return types.SimpleNamespace(
|
||||
nanoseconds=self._nanoseconds,
|
||||
to_msg=lambda: _type("builtin_interfaces/Time")(
|
||||
sec=self._nanoseconds // 1_000_000_000,
|
||||
nanosec=self._nanoseconds % 1_000_000_000,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
_registry = {}
|
||||
|
||||
|
||||
def _type(type_name):
|
||||
if type_name.endswith("[]"):
|
||||
return list
|
||||
return _registry[type_name]
|
||||
|
||||
|
||||
def _default(type_name):
|
||||
if type_name.endswith("[]"):
|
||||
return []
|
||||
if type_name in ("string",):
|
||||
return ""
|
||||
if type_name in ("bool",):
|
||||
return False
|
||||
if type_name.startswith(("uint", "int", "float")):
|
||||
return 0
|
||||
return _type(type_name)()
|
||||
|
||||
|
||||
def _make_class(name, lines):
|
||||
constants, fields = {}, []
|
||||
for line in lines:
|
||||
type_name, declaration = line.split(None, 1)
|
||||
if "=" in declaration:
|
||||
key, value = declaration.split("=", 1)
|
||||
constants[key] = int(value)
|
||||
else:
|
||||
fields.append((type_name, declaration))
|
||||
slots = tuple(field for _, field in fields)
|
||||
|
||||
def init(self, **kwargs):
|
||||
for type_name, field in fields:
|
||||
setattr(self, field, kwargs.pop(field, _default(type_name)))
|
||||
if kwargs:
|
||||
raise TypeError("unexpected fields: " + ", ".join(kwargs))
|
||||
|
||||
attrs = {"__slots__": slots, "__init__": init, **constants}
|
||||
return type(name, (), attrs)
|
||||
|
||||
|
||||
def install():
|
||||
"""Install deterministic rclpy and generated interface modules."""
|
||||
for name in list(sys.modules):
|
||||
if name == "rclpy" or name.startswith("rclpy.") or name.startswith("bt_skill_interfaces"):
|
||||
del sys.modules[name]
|
||||
|
||||
builtin = _module("builtin_interfaces")
|
||||
builtin_msg = _module("builtin_interfaces.msg")
|
||||
builtin.msg = builtin_msg
|
||||
for name, fields in {
|
||||
"Time": ["int32 sec", "uint32 nanosec"],
|
||||
"Duration": ["int32 sec", "uint32 nanosec"],
|
||||
}.items():
|
||||
cls = _make_class(name, fields)
|
||||
setattr(builtin_msg, name, cls)
|
||||
_registry[f"builtin_interfaces/{name}"] = cls
|
||||
|
||||
std = _module("std_msgs")
|
||||
std_msg = _module("std_msgs.msg")
|
||||
std.msg = std_msg
|
||||
header = _make_class("Header", ["builtin_interfaces/Time stamp", "string frame_id"])
|
||||
std_msg.Header = header
|
||||
_registry["std_msgs/Header"] = header
|
||||
string = _make_class("String", ["string data"])
|
||||
std_msg.String = string
|
||||
_registry["std_msgs/String"] = string
|
||||
|
||||
geometry = _module("geometry_msgs")
|
||||
geometry_msg = _module("geometry_msgs.msg")
|
||||
geometry.msg = geometry_msg
|
||||
definitions = {
|
||||
"Point": ["float64 x", "float64 y", "float64 z"],
|
||||
"Quaternion": ["float64 x", "float64 y", "float64 z", "float64 w"],
|
||||
"Pose": ["geometry_msgs/Point position", "geometry_msgs/Quaternion orientation"],
|
||||
"PoseStamped": ["std_msgs/Header header", "geometry_msgs/Pose pose"],
|
||||
"PointStamped": ["std_msgs/Header header", "geometry_msgs/Point point"],
|
||||
}
|
||||
for name, fields in definitions.items():
|
||||
cls = _make_class(name, fields)
|
||||
setattr(geometry_msg, name, cls)
|
||||
_registry[f"geometry_msgs/{name}"] = cls
|
||||
|
||||
package = _module("bt_skill_interfaces")
|
||||
msg_module = _module("bt_skill_interfaces.msg")
|
||||
package.msg = msg_module
|
||||
pending = {p.stem: _fields(p) for p in (IDL / "msg").glob("*.msg")}
|
||||
while pending:
|
||||
progress = False
|
||||
for name, lines in list(pending.items()):
|
||||
deps = [line.split()[0].removesuffix("[]") for line in lines if "=" not in line]
|
||||
if all(dep in _registry or dep in ("string", "bool") or dep.startswith(("uint", "int", "float")) for dep in deps):
|
||||
cls = _make_class(name, lines)
|
||||
setattr(msg_module, name, cls)
|
||||
_registry[f"bt_skill_interfaces/{name}"] = cls
|
||||
del pending[name]
|
||||
progress = True
|
||||
if not progress:
|
||||
raise RuntimeError("unresolved IDL: " + repr(pending))
|
||||
|
||||
action_module = _module("bt_skill_interfaces.action")
|
||||
package.action = action_module
|
||||
for path in (IDL / "action").glob("*.action"):
|
||||
action = type(path.stem, (), {})
|
||||
action.Goal = _make_class("Goal", _fields(path, 0))
|
||||
action.Result = _make_class("Result", _fields(path, 1))
|
||||
action.Feedback = _make_class("Feedback", _fields(path, 2))
|
||||
setattr(action_module, path.stem, action)
|
||||
|
||||
srv_module = _module("bt_skill_interfaces.srv")
|
||||
package.srv = srv_module
|
||||
for path in (IDL / "srv").glob("*.srv"):
|
||||
srv = type(path.stem, (), {})
|
||||
srv.Request = _make_class("Request", _fields(path, 0))
|
||||
srv.Response = _make_class("Response", _fields(path, 1))
|
||||
setattr(srv_module, path.stem, srv)
|
||||
|
||||
rclpy = _module("rclpy")
|
||||
rclpy.ok = lambda: True
|
||||
rclpy.init = lambda **_: None
|
||||
rclpy.shutdown = lambda: None
|
||||
action = _module("rclpy.action")
|
||||
action.GoalResponse = types.SimpleNamespace(ACCEPT=1, REJECT=2)
|
||||
action.CancelResponse = types.SimpleNamespace(ACCEPT=1)
|
||||
action.ActionServer = lambda *args, **kwargs: types.SimpleNamespace(destroy=lambda: None)
|
||||
callbacks = _module("rclpy.callback_groups")
|
||||
callbacks.ReentrantCallbackGroup = object
|
||||
executors = _module("rclpy.executors")
|
||||
executors.MultiThreadedExecutor = lambda **_: types.SimpleNamespace(
|
||||
add_node=lambda node: None, spin=lambda: None, shutdown=lambda **_: None)
|
||||
node_module = _module("rclpy.node")
|
||||
|
||||
class Node:
|
||||
parameters = {}
|
||||
|
||||
def __init__(self, *_args, namespace="/sim/robot_01", **_kwargs):
|
||||
self._namespace = namespace
|
||||
self._clock = RosClock()
|
||||
|
||||
def get_namespace(self): return self._namespace
|
||||
def declare_parameter(self, name, default): self.parameters.setdefault(name, default)
|
||||
def get_parameter(self, name): return types.SimpleNamespace(value=self.parameters[name])
|
||||
def resolve_topic_name(self, name): return self._namespace + "/" + name
|
||||
def create_publisher(self, _type, name, *_a, **_k):
|
||||
publisher = types.SimpleNamespace(name=name, values=[])
|
||||
publisher.publish = publisher.values.append
|
||||
return publisher
|
||||
def create_service(self, _type, name, callback, **_k): return types.SimpleNamespace(name=name, callback=callback)
|
||||
def create_timer(self, *_a, **_k): return object()
|
||||
def get_logger(self): return types.SimpleNamespace(warning=lambda *_: None, error=lambda *_: None)
|
||||
def get_clock(self): return self._clock
|
||||
def destroy_node(self): pass
|
||||
|
||||
node_module.Node = Node
|
||||
rclpy.action, rclpy.callback_groups, rclpy.executors, rclpy.node = action, callbacks, executors, node_module
|
||||
|
||||
|
||||
def load_mock_module(parameters=None):
|
||||
install()
|
||||
sys.modules["rclpy.node"].Node.parameters = dict(parameters or {})
|
||||
package = _module("bt_mock_servers")
|
||||
package.__path__ = [str(IDL.parent / "bt_mock_servers" / "bt_mock_servers")]
|
||||
source = IDL.parent / "bt_mock_servers" / "bt_mock_servers" / "mock_skills.py"
|
||||
spec = importlib.util.spec_from_file_location("bt_mock_servers.mock_skills", source)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
sys.modules[spec.name] = module
|
||||
spec.loader.exec_module(module)
|
||||
return module
|
||||
|
||||
|
||||
class GoalHandle:
|
||||
def __init__(self, request, cancel=False, uuid=bytes(range(16))):
|
||||
self.request, self.is_cancel_requested = request, cancel
|
||||
self.goal_id = types.SimpleNamespace(uuid=uuid)
|
||||
self.feedback, self.native = [], None
|
||||
self.is_active = True
|
||||
self.cancel_acknowledged = False
|
||||
|
||||
def execute(self): pass
|
||||
def publish_feedback(self, value): self.feedback.append(value)
|
||||
def succeed(self): self.native, self.is_active = "succeeded", False
|
||||
def abort(self): self.native, self.is_active = "aborted", False
|
||||
def canceled(self): self.native, self.is_active = "canceled", False
|
||||
|
||||
def request_cancel(self):
|
||||
self.is_cancel_requested = True
|
||||
self.cancel_acknowledged = True
|
||||
return True
|
||||
@@ -0,0 +1,141 @@
|
||||
import json
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
import sys
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'coordinator'))
|
||||
from robot_bt_coordinator.service import Coordinator
|
||||
from robot_bt_coordinator.backends import ManualBackend, demo_plan
|
||||
from robot_bt_coordinator.errors import ApiError
|
||||
from robot_bt_coordinator.plan import validate_plan
|
||||
|
||||
REQ = dict(client_request_id='r1', robot_id='robot_01', instruction='把水放入箱A', known_info=dict(target_name='water', quantity=1, source_location='shelf_A', destination='tote_A'))
|
||||
|
||||
class CoordinatorTest(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.tmp = tempfile.TemporaryDirectory()
|
||||
self.db = str(Path(self.tmp.name)/'state.db')
|
||||
self.backend = ManualBackend()
|
||||
self.c = Coordinator(self.db, self.backend, {'robot_01'})
|
||||
def tearDown(self):
|
||||
self.c.close(); self.tmp.cleanup()
|
||||
def plan(self, tid):
|
||||
self.c.tick()
|
||||
t=self.c.get(tid)
|
||||
self.backend.emit(dict(type='plan',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=demo_plan(REQ['known_info'])))
|
||||
self.c.tick()
|
||||
def result(self,tid,**changes):
|
||||
t=self.c.get(tid)
|
||||
e=dict(type='execution_result',task_id=tid,run_id=t['run_id'],status='SUCCEEDED',stop_confirmed=True,completed_quantity=1,evidence=dict(evidence_id='ev1',target_ref='water',destination_ref='tote_A',passed=True,empty_hand=True,in_destination=True,valid=True,safe_to_release=True))
|
||||
e.update(changes); self.backend.emit(e);self.c.tick()
|
||||
def test_same_request_is_idempotent_and_conflict_rejected(self):
|
||||
a=self.c.submit(REQ); b=self.c.submit(REQ)
|
||||
self.assertEqual(a['task_id'],b['task_id']);self.assertTrue(b['deduplicated'])
|
||||
with self.assertRaises(ApiError) as e:self.c.submit(dict(REQ,instruction='different'))
|
||||
self.assertEqual(e.exception.status,409)
|
||||
def test_fifo_and_queue_cancel(self):
|
||||
a=self.c.submit(REQ)['task_id'];b=self.c.submit(dict(REQ,client_request_id='r2'))['task_id']
|
||||
self.plan(a);self.assertEqual(self.c.get(b)['status'],'QUEUED')
|
||||
self.c.control(b,'cancel');self.assertEqual(self.c.get(b)['status'],'CANCELED')
|
||||
def test_cancel_during_planning_ignores_late_plan(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.c.tick(); t=self.c.get(tid)
|
||||
self.c.control(tid,'cancel')
|
||||
self.backend.emit(dict(type='plan',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=demo_plan(REQ['known_info'])))
|
||||
self.c.tick();self.assertEqual(self.c.get(tid)['status'],'CANCELED')
|
||||
self.assertEqual(len(self.backend.executions),0)
|
||||
def test_cancel_ack_does_not_release_execution(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid);self.c.control(tid,'cancel')
|
||||
self.backend.emit(dict(type='cancel_ack',task_id=tid,run_id=self.c.get(tid)['run_id']))
|
||||
self.c.tick();self.assertEqual(self.c.get(tid)['status'],'CANCELING')
|
||||
def test_unknown_stop_quarantines_and_blocks_next_task(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid)
|
||||
other=self.c.submit(dict(REQ,client_request_id='r2'))['task_id']
|
||||
self.result(tid,stop_confirmed=False)
|
||||
self.assertEqual(self.c.get(tid)['status'],'INTERVENTION_REQUIRED')
|
||||
self.c.tick();self.assertEqual(self.c.get(other)['status'],'QUEUED')
|
||||
def test_wrong_container_not_counted(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid)
|
||||
self.result(tid,evidence=dict(evidence_id='ev',target_ref='water',destination_ref='tote_B',passed=True,empty_hand=True,in_destination=True,valid=True,safe_to_release=True))
|
||||
self.assertEqual(self.c.get(tid)['completed_quantity'],0)
|
||||
self.assertEqual(self.c.get(tid)['status'],'INTERVENTION_REQUIRED')
|
||||
def test_delivery_transaction_is_idempotent_after_duplicate_and_restart(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid);self.result(tid);self.result(tid)
|
||||
self.assertEqual(self.c.get(tid)['completed_quantity'],1)
|
||||
self.c.close();self.c=Coordinator(self.db,ManualBackend(),{'robot_01'})
|
||||
self.assertEqual(self.c.get(tid)['completed_quantity'],1)
|
||||
self.assertEqual(self.c.get(tid)['status'],'SUCCEEDED')
|
||||
def test_restart_quarantines_unfinished_motion(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid);self.c.close()
|
||||
self.c=Coordinator(self.db,ManualBackend(),{'robot_01'})
|
||||
self.assertEqual(self.c.get(tid)['status'],'INTERVENTION_REQUIRED')
|
||||
self.c.tick();self.assertEqual(len(self.c.backend.executions),0)
|
||||
def test_stale_clarification_rejected(self):
|
||||
tid=self.c.submit(dict(REQ,known_info={}))['task_id'];self.c.tick();t=self.c.get(tid)
|
||||
self.backend.emit(dict(type='plan',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='NEEDS_CLARIFICATION',questions=['destination']))
|
||||
self.c.tick();t=self.c.get(tid)
|
||||
with self.assertRaises(ApiError): self.c.clarify(tid,dict(question_id='old',task_revision=t['task_revision'],known_info={'destination':'tote_A'}))
|
||||
def test_input_unknown_robot_and_quantity_rejected(self):
|
||||
for req in [dict(REQ,robot_id='other'),dict(REQ,instruction=' '*5),dict(REQ,known_info=dict(REQ['known_info'],quantity=2))]:
|
||||
with self.assertRaises(ApiError):self.c.submit(req)
|
||||
def test_cancel_with_unknown_hand_does_not_admit_next_task(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid);self.c.control(tid,'cancel')
|
||||
self.result(tid,status='CANCELED',completed_quantity=0,evidence={})
|
||||
self.assertEqual(self.c.get(tid)['status'],'INTERVENTION_REQUIRED')
|
||||
def test_single_process_owns_scheduler_database(self):
|
||||
with self.assertRaises(RuntimeError):Coordinator(self.db,ManualBackend(),{'robot_01'})
|
||||
def test_planning_budget_expires_without_reply(self):
|
||||
elapsed=[0.]
|
||||
self.c.steady=lambda:elapsed[0]
|
||||
tid=self.c.submit(REQ)['task_id'];self.c.tick()
|
||||
elapsed[0]+=100
|
||||
self.c.tick()
|
||||
self.assertEqual(self.c.get(tid)['planning_attempts'],2)
|
||||
elapsed[0]+=100
|
||||
self.c.tick();self.assertEqual(self.c.get(tid)['status'],'FAILED')
|
||||
def test_planner_cannot_change_user_known_slots(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.c.tick();t=self.c.get(tid)
|
||||
changed=dict(REQ['known_info'],target_name='other')
|
||||
self.backend.emit(dict(type='plan',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=demo_plan(changed)))
|
||||
self.c.tick();self.assertEqual(len(self.backend.executions),0)
|
||||
def test_ask_user_plan_enters_clarification_without_execution(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.c.tick();t=self.c.get(tid)
|
||||
p=demo_plan(REQ['known_info']);p['subtasks']=[dict(id='Q1',skill='ASK_USER',arguments={'question':'请确认目标箱编号'},depends_on=[])]
|
||||
self.backend.emit(dict(type='plan',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=p))
|
||||
self.c.tick();self.assertEqual(self.c.get(tid)['status'],'NEEDS_CLARIFICATION')
|
||||
self.assertEqual(len(self.backend.executions),0)
|
||||
def test_late_plan_after_budget_cannot_dispatch(self):
|
||||
elapsed=[0.];self.c.steady=lambda:elapsed[0]
|
||||
tid=self.c.submit(REQ)['task_id'];self.c.tick();t=self.c.get(tid)
|
||||
elapsed[0]=31.
|
||||
self.backend.emit(dict(type='plan',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=demo_plan(REQ['known_info'])))
|
||||
self.c.tick();self.assertEqual(len(self.backend.executions),0)
|
||||
def test_delivered_item_retained_if_cleanup_requires_intervention(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid)
|
||||
ev=dict(evidence_id='ev1',target_ref='water',destination_ref='tote_A',passed=True,empty_hand=True,in_destination=True,valid=True,safe_to_release=False)
|
||||
self.result(tid,status='INTERVENTION_REQUIRED',evidence=ev)
|
||||
self.assertEqual(self.c.get(tid)['completed_quantity'],1)
|
||||
self.assertEqual(self.c.get(tid)['status'],'INTERVENTION_REQUIRED')
|
||||
other=self.c.submit(dict(REQ,client_request_id='cleanup-next'))['task_id'];self.c.tick()
|
||||
self.assertEqual(self.c.get(other)['status'],'QUEUED')
|
||||
def test_event_cursor_monotonic(self):
|
||||
tid=self.c.submit(REQ)['task_id'];self.plan(tid);es=self.c.events(tid)
|
||||
self.assertTrue(len(es)>2); self.assertEqual(self.c.events(tid,es[-1]['event_id']),[])
|
||||
|
||||
class PlanTest(unittest.TestCase):
|
||||
def test_valid_fixed_plan(self):
|
||||
self.assertEqual(validate_plan(demo_plan(REQ['known_info']))['task_type'],'pick_transport_place')
|
||||
def test_unknown_skill_cycle_missing_id_xml_injection_rejected(self):
|
||||
base=demo_plan(REQ['known_info'])
|
||||
plans=[]
|
||||
a=json.loads(json.dumps(base));a['subtasks'][2]['skill']='SHELL';plans.append(a)
|
||||
a=json.loads(json.dumps(base));a['subtasks'][0]['depends_on']=['S6'];plans.append(a)
|
||||
a=json.loads(json.dumps(base));a['subtasks'][1]['depends_on']=['NO'];plans.append(a)
|
||||
a=json.loads(json.dumps(base));a['xml']='<Script/>';plans.append(a)
|
||||
for p in plans:
|
||||
with self.subTest(plan=p),self.assertRaises(ApiError):validate_plan(p)
|
||||
def test_nonfinite_and_bool_quantity_rejected(self):
|
||||
for q in [True,float('nan'),2]:
|
||||
p=demo_plan(REQ['known_info']);p['slots']['quantity']=q
|
||||
with self.assertRaises(ApiError):validate_plan(p)
|
||||
|
||||
if __name__=='__main__':unittest.main()
|
||||
@@ -0,0 +1,65 @@
|
||||
"""DR semantic regression tests independent of ROS transport."""
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
sys.path[:0] = [str(ROOT), str(ROOT / 'coordinator'), str(ROOT / 'robobrain')]
|
||||
|
||||
|
||||
class DrSemanticsTests(unittest.TestCase):
|
||||
def test_localization_adapter_preserves_all_dr_context_fields(self):
|
||||
sys.path.insert(0,str(ROOT/'ros2/robobrain_services'))
|
||||
from robobrain_services.nodes import perception_goal
|
||||
from types import SimpleNamespace as S
|
||||
goal=S(task_id='t',subtask_id='s',target_ref='water',target_description='bottle',
|
||||
shelf_id='shelf_A',column_id='2',tier_id='3',station_binding_ref='binding-9',
|
||||
expected_geometry_epoch=7,capture_after=S(sec=1,nanosec=5),timeout=S(sec=2,nanosec=0))
|
||||
result=perception_goal(goal,'localize')
|
||||
for key in ('shelf_id','column_id','tier_id','station_binding_ref','expected_geometry_epoch'):
|
||||
self.assertEqual(result[key],getattr(goal,key))
|
||||
|
||||
def test_semantic_navigation_accepts_positive_subsecond_budget(self):
|
||||
from navigation_gateway.semantic_proxy import duration_seconds
|
||||
from types import SimpleNamespace
|
||||
self.assertAlmostEqual(duration_seconds(SimpleNamespace(sec=0,nanosec=500000000)),.5)
|
||||
for sec,nanosec in [(0,0),(-1,1),(3600,1),(1,1000000000),(1,-1)]:
|
||||
with self.assertRaises(ValueError):duration_seconds(SimpleNamespace(sec=sec,nanosec=nanosec))
|
||||
|
||||
def test_shelf_tier_can_be_empty_without_guessing(self):
|
||||
# BT p15 and RB02 allow an unknown tier; execution must decide whether
|
||||
# its chosen route can use the observation without that calibration.
|
||||
from robot_robobrain.service import BrainService
|
||||
from robot_robobrain.backends import FixtureBackend
|
||||
from robot_robobrain.observations import Observation
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
image = Path(folder) / 'frame.jpg'; image.write_bytes(b'fixture')
|
||||
raw = dict(status='SUCCEEDED', shelf_id='shelf_A', side_id='FRONT',
|
||||
column_id='1', tier_id='', confidence=.95)
|
||||
service = BrainService(FixtureBackend(json.dumps(raw)), folder)
|
||||
observation = Observation('obs', 100, 'camera', str(image), 'observe_A', 1, 'shelf_A')
|
||||
goal = dict(task_id='t', subtask_id='s', target_ref='water', source_region_ref='shelf_A',
|
||||
observation_station_id='observe_A', station_registry_version=1, capture_after=90, timeout=1)
|
||||
result = service.shelf(goal, observation, 110, max_age_ns=50)
|
||||
self.assertEqual(result['status'], 'SUCCEEDED')
|
||||
self.assertEqual(result['tier_id'], '')
|
||||
|
||||
def test_navigation_reports_signed_shortest_yaw_error(self):
|
||||
from navigation_gateway.gateway import pose_errors
|
||||
from test_navigation_gateway import request
|
||||
target = request()['target_pose']
|
||||
current = copy.deepcopy(target)
|
||||
current['orientation'].update(z=math.sin(.2/2), w=math.cos(.2/2))
|
||||
distance, yaw = pose_errors(target, current)
|
||||
self.assertEqual(distance, 0)
|
||||
self.assertAlmostEqual(yaw, -.2)
|
||||
current['orientation'].update(z=math.sin(-.2/2), w=math.cos(-.2/2))
|
||||
self.assertAlmostEqual(pose_errors(target, current)[1], .2)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,38 @@
|
||||
import http.client
|
||||
import json
|
||||
import sys
|
||||
import tempfile
|
||||
import threading
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'coordinator'))
|
||||
from robot_bt_coordinator.service import Coordinator
|
||||
from robot_bt_coordinator.backends import ManualBackend
|
||||
from robot_bt_coordinator.http_api import make_server
|
||||
|
||||
class HttpTest(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.tmp=tempfile.TemporaryDirectory();self.c=Coordinator(str(Path(self.tmp.name)/'db'),ManualBackend(),{'robot_01'})
|
||||
self.server=make_server(self.c,'127.0.0.1',0,'test-secret','operator-secret')
|
||||
self.thread=threading.Thread(target=self.server.serve_forever,daemon=True);self.thread.start()
|
||||
def tearDown(self):
|
||||
self.server.shutdown();self.server.server_close();self.thread.join();self.c.close();self.tmp.cleanup()
|
||||
def request(self,method,path,body=None,token='test-secret'):
|
||||
conn=http.client.HTTPConnection('127.0.0.1',self.server.server_port,timeout=3)
|
||||
headers={'Authorization':'Bearer '+token,'Content-Type':'application/json'}
|
||||
conn.request(method,path,body,headers);r=conn.getresponse();result=(r.status,json.loads(r.read()));conn.close();return result
|
||||
def test_auth_required_and_health_allowed(self):
|
||||
self.assertEqual(self.request('GET','/v1/capabilities',token='wrong')[0],401)
|
||||
self.assertEqual(self.request('GET','/healthz',token='wrong')[0],200)
|
||||
def test_submit_get_cancel_and_event_cursor(self):
|
||||
req=dict(client_request_id='http1',robot_id='robot_01',instruction='move water',known_info={})
|
||||
code,t=self.request('POST','/v1/tasks',json.dumps(req));self.assertEqual(code,202)
|
||||
tid=t['task_id'];self.assertEqual(self.request('GET','/v1/tasks/'+tid)[1]['status'],'QUEUED')
|
||||
self.assertEqual(self.request('POST',f'/v1/tasks/{tid}/cancel','{}')[1]['status'],'CANCELED')
|
||||
self.assertEqual(self.request('GET',f'/v1/tasks/{tid}/events?after=0')[0],200)
|
||||
def test_duplicate_json_keys_nonfinite_and_unknown_endpoint_rejected(self):
|
||||
for payload in ['{"robot_id":"a","robot_id":"b"}','{"quantity":NaN}']:
|
||||
self.assertEqual(self.request('POST','/v1/tasks',payload)[0],400)
|
||||
self.assertEqual(self.request('GET','/not-found')[0],404)
|
||||
|
||||
if __name__=='__main__':unittest.main()
|
||||
@@ -0,0 +1,439 @@
|
||||
import json
|
||||
import pathlib
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
import unittest
|
||||
|
||||
sys.path.insert(0, str(pathlib.Path(__file__).resolve().parent.parent))
|
||||
from tests.helpers.ros_shim import GoalHandle, load_mock_module
|
||||
|
||||
|
||||
class MockRuntimeTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.module = load_mock_module({
|
||||
"scenarios_json": "{}", "max_goal_seconds": 1.0,
|
||||
"allowed_postures": ["pregrasp", "transport", "home"],
|
||||
"initial_holding_state": "UNKNOWN",
|
||||
})
|
||||
self.node = self.module.MockSkills()
|
||||
|
||||
def trace(self):
|
||||
T = sys.modules["bt_skill_interfaces.msg"].TaskTrace
|
||||
return T(task_id="t", subtask_id="s", attempt=1, task_revision=1,
|
||||
plan_version=1, run_id="r", execution_generation=1)
|
||||
|
||||
def goal(self, name):
|
||||
action = self.module.ACTION_TYPES[name]
|
||||
goal = action.Goal()
|
||||
goal.timeout.sec = 1
|
||||
if hasattr(goal, "trace"): goal.trace = self.trace()
|
||||
if hasattr(goal, "task_id"): goal.task_id = "t"
|
||||
if hasattr(goal, "subtask_id"): goal.subtask_id = "s"
|
||||
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
|
||||
elif name == "navigate_semantic":
|
||||
goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
|
||||
goal.position_tolerance = goal.orientation_tolerance = 0.1
|
||||
elif name == "execute_manipulation":
|
||||
goal.skill, goal.instruction = "pick", "pick bottle"
|
||||
goal.target.object_ref, goal.target.description = "bottle", "bottle"
|
||||
elif name == "execute_posture": goal.posture_id, goal.expected_geometry_epoch = "home", 1
|
||||
elif name == "plan_task":
|
||||
goal.instruction, goal.task_revision, goal.planning_generation = "fetch", 1, 1
|
||||
goal.known_info_json = goal.context_snapshot_json = goal.constraints_json = "{}"
|
||||
elif name == "verify_state":
|
||||
goal.check, goal.expected_geometry_epoch = goal.PRECHECK, 1
|
||||
goal.target.object_ref, goal.target.description = "bottle", "bottle"
|
||||
elif name == "locate_shelf_column":
|
||||
goal.target_ref, goal.target_description = "bottle", "bottle"
|
||||
goal.source_region_ref, goal.observation_station_id, goal.station_registry_version = "shelf_zone", "station_A", 1
|
||||
elif name == "localize_target_3d":
|
||||
goal.expected_geometry_epoch, goal.target_ref, goal.target_description = 1, "bottle", "bottle"
|
||||
goal.shelf_id, goal.column_id, goal.station_binding_ref = "shelf_A", "1", "station_A"
|
||||
elif name == "check_free_space":
|
||||
goal.destination_ref, goal.destination_description = "bin_A", "bin A"
|
||||
goal.object_ref, goal.object_description, goal.placement_constraints_json = "bottle", "bottle", "{}"
|
||||
elif name == "assess_grasp":
|
||||
goal.allowed_posture_ids = ["pregrasp"]
|
||||
goal.target_binding.target.object_ref, goal.target_binding.target.description = "bottle", "bottle"
|
||||
goal.target_binding.context.schema_version = goal.target_binding.context.geometry_epoch = 1
|
||||
goal.robot_state.robot_id = "robot_01"
|
||||
goal.robot_state.valid_until.sec = 20
|
||||
elif name == "evaluate_progress":
|
||||
goal.task_description, goal.sequence = "fetch bottle", 1
|
||||
goal.window_json = '[{"stamp":1,"views":{"front":"/tmp/frame.png"}}]'
|
||||
elif name == "execute_task":
|
||||
goal.approved_plan_json = '{"subtasks":[]}'
|
||||
goal.context_json = "{}"
|
||||
return goal
|
||||
|
||||
def execute(self, name, fixture, cancel=False):
|
||||
self.node.scenarios[name] = [fixture]
|
||||
goal = self.goal(name)
|
||||
self.assertEqual(self.node._goal(name, goal), self.module.GoalResponse.ACCEPT)
|
||||
handle = GoalHandle(goal, cancel=cancel)
|
||||
self.node._accepted(name, handle)
|
||||
return handle, self.node._execute(name, handle)
|
||||
|
||||
def test_all_twelve_actions_execute_success_and_failure(self):
|
||||
self.assertEqual(len(self.module.ACTION_TYPES), 12)
|
||||
self.assertEqual(self.module.ACTION_ENDPOINTS["plan_task"], "tasks/plan")
|
||||
self.assertEqual(self.module.ACTION_ENDPOINTS["execute_task"], "tasks/execute")
|
||||
self.assertEqual(self.module.ACTION_ENDPOINTS["evaluate_progress"], "monitor/evaluate_progress")
|
||||
for name in self.module.ACTION_TYPES:
|
||||
with self.subTest(action=name, kind="normal"):
|
||||
positive_kind = "passed" if name == "verify_state" else "normal"
|
||||
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.stop_state, result.result.CONFIRMED)
|
||||
elif hasattr(result, "status"):
|
||||
self.assertNotEqual(result.status, result.FAILED)
|
||||
elif hasattr(result, "decision"):
|
||||
self.assertEqual(result.decision, result.DIRECT)
|
||||
elif hasattr(result, "evidence"):
|
||||
self.assertEqual(result.evidence.status, result.evidence.PASSED)
|
||||
elif hasattr(result, "feedback_state"):
|
||||
self.assertTrue(result.feedback_state.progress_valid)
|
||||
self.assertEqual(result.error_code, "")
|
||||
self.node.counts[name] = 0
|
||||
with self.subTest(action=name, kind="failed"):
|
||||
handle, result = self.execute(name, {"kind": "failed", "duration_seconds": 0})
|
||||
self.assertEqual(handle.native, "aborted")
|
||||
if hasattr(result, "result"):
|
||||
self.assertEqual(result.result.status, result.result.FAILED)
|
||||
elif hasattr(result, "status"):
|
||||
self.assertEqual(result.status, result.FAILED)
|
||||
elif hasattr(result, "decision"):
|
||||
self.assertEqual(result.decision, result.UNKNOWN)
|
||||
elif hasattr(result, "evidence"):
|
||||
self.assertEqual(result.evidence.status, result.evidence.UNKNOWN)
|
||||
elif hasattr(result, "feedback_state"):
|
||||
self.assertFalse(result.feedback_state.progress_valid)
|
||||
self.assertEqual(result.error_code, "MOCK_TERMINATED")
|
||||
|
||||
def test_success_feedback_follows_normal_lifecycle_and_populates_fields(self):
|
||||
cases = {
|
||||
"navigate": [0, 1, 2, 5],
|
||||
"execute_manipulation": [0, 1, 2, 3, 4],
|
||||
"execute_posture": [0, 1, 2],
|
||||
}
|
||||
for name, expected in cases.items():
|
||||
self.node.counts[name] = 0
|
||||
handle, _ = self.execute(name, {"kind": "normal", "duration_seconds": 0.55})
|
||||
with self.subTest(action=name):
|
||||
self.assertEqual([item.phase for item in handle.feedback], expected)
|
||||
self.assertEqual([item.sequence for item in handle.feedback], list(range(1, len(expected) + 1)))
|
||||
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.assertFalse(nav.blocked)
|
||||
self.assertGreaterEqual(nav.elapsed_time.nanosec, 0)
|
||||
manipulation = self.execute("execute_manipulation", {"kind": "normal", "duration_seconds": 0.05})[0].feedback[-1]
|
||||
self.assertFalse(manipulation.progress_valid)
|
||||
|
||||
def test_each_action_feedback_lifecycle_and_navigation_recovery_execute(self):
|
||||
expected = {
|
||||
"navigate": [0, 1, 2, 5], "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],
|
||||
"navigate_semantic": [0], "evaluate_progress": [0],
|
||||
}
|
||||
for name, phases in expected.items():
|
||||
self.node.counts[name] = 0
|
||||
handle, _ = self.execute(name, {"kind": "passed" if name == "verify_state" else "normal",
|
||||
"duration_seconds": 0.55})
|
||||
with self.subTest(action=name):
|
||||
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.node.counts["execute_task"] = 0
|
||||
task, _ = self.execute("execute_task", {"kind": "normal", "duration_seconds": 0.05})
|
||||
self.assertTrue(task.feedback[0].stage)
|
||||
self.assertEqual(json.loads(task.feedback[0].status_json)["sequence"], 1)
|
||||
|
||||
def test_cancel_timeout_and_stop_unknown_have_consistent_terminals(self):
|
||||
self.node.scenarios["navigate"] = [{"kind": "normal", "duration_seconds": 1, "stop_delay_seconds": 0.05}]
|
||||
goal = self.goal("navigate")
|
||||
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
|
||||
handle = GoalHandle(goal); self.node._accepted("navigate", handle)
|
||||
box = {}
|
||||
thread = threading.Thread(target=lambda: box.setdefault("result", self.node._execute("navigate", handle)))
|
||||
thread.start(); time.sleep(0.02)
|
||||
self.assertTrue(handle.request_cancel())
|
||||
self.assertTrue(handle.cancel_acknowledged)
|
||||
self.assertTrue(thread.is_alive(), "cancel ACK must precede delayed stop termination")
|
||||
thread.join(1)
|
||||
result = box["result"]
|
||||
self.assertEqual((handle.native, result.result.status, result.result.stop_state),
|
||||
("canceled", result.result.CANCELED, result.result.CONFIRMED))
|
||||
self.assertEqual(handle.feedback[-1].phase, self.module.Navigate.Feedback.STOPPING)
|
||||
self.node.motion_reserved = False; self.node.counts["navigate"] = 0
|
||||
goal = self.goal("navigate"); goal.timeout.sec = 0; goal.timeout.nanosec = 1_000_000
|
||||
self.node.scenarios["navigate"] = [{"kind": "timeout"}]
|
||||
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.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})
|
||||
self.assertEqual(unknown.result.stop_state, unknown.result.UNKNOWN)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
self.assertEqual(unknown.result.stop_evidence_ref, "")
|
||||
|
||||
def test_cancel_and_timeout_can_acknowledge_without_confirming_stop(self):
|
||||
handle, canceled = self.execute(
|
||||
"execute_manipulation", {"kind": "cancel_stop_unknown", "duration_seconds": 1}, cancel=True)
|
||||
self.assertEqual((handle.native, canceled.result.status), ("canceled", canceled.result.CANCELED))
|
||||
self.assertEqual(canceled.result.stop_state, canceled.result.UNKNOWN)
|
||||
self.assertEqual(handle.feedback[-1].phase, self.module.ExecuteManipulation.Feedback.STOPPING)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
self.node.motion_reserved = False; self.node.unresolved_motion.clear(); self.node.counts["navigate"] = 0
|
||||
goal = self.goal("navigate"); goal.timeout.sec = 0; goal.timeout.nanosec = 1_000_000
|
||||
self.node.scenarios["navigate"] = [{"kind": "timeout_stop_unknown"}]
|
||||
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
|
||||
timeout_handle = GoalHandle(goal); self.node._accepted("navigate", timeout_handle)
|
||||
timed_out = self.node._execute("navigate", timeout_handle)
|
||||
self.assertEqual(timed_out.result.stop_state, timed_out.result.UNKNOWN)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
|
||||
def test_action_specific_malformed_goals_are_rejected(self):
|
||||
mutations = {
|
||||
"navigate": lambda g: setattr(g.target_pose.header, "frame_id", ""),
|
||||
"navigate_semantic": lambda g: setattr(g, "registry_version", 0),
|
||||
"execute_manipulation": lambda g: setattr(g.target, "object_ref", ""),
|
||||
"execute_posture": lambda g: setattr(g, "expected_geometry_epoch", 0),
|
||||
"plan_task": lambda g: setattr(g, "known_info_json", "[]"),
|
||||
"verify_state": lambda g: setattr(g.target, "object_ref", ""),
|
||||
"locate_shelf_column": lambda g: setattr(g, "observation_station_id", ""),
|
||||
"localize_target_3d": lambda g: setattr(g, "station_binding_ref", ""),
|
||||
"check_free_space": lambda g: setattr(g, "placement_constraints_json", "[]"),
|
||||
"assess_grasp": lambda g: setattr(g.robot_state, "robot_id", ""),
|
||||
"evaluate_progress": lambda g: setattr(g, "window_json", "[]"),
|
||||
"execute_task": lambda g: setattr(g, "approved_plan_json", "{}"),
|
||||
}
|
||||
for name, mutate in mutations.items():
|
||||
goal = self.goal(name); mutate(goal)
|
||||
with self.subTest(action=name):
|
||||
self.assertEqual(self.node._goal(name, goal), self.module.GoalResponse.REJECT)
|
||||
|
||||
def test_semantic_navigation_accepts_production_kind_matrix_only(self):
|
||||
accepted = (
|
||||
("LOCATION", "destination_A", {}),
|
||||
("OBJECT", "bottle", {}),
|
||||
("CELL", "bottle", {"shelf_id": "shelf_A", "side_id": "FRONT",
|
||||
"column_id": "1", "tier_id": "2"}),
|
||||
)
|
||||
for kind, reference, fields in accepted:
|
||||
goal = self.goal("navigate_semantic")
|
||||
goal.kind, goal.reference = kind, reference
|
||||
for field, value in fields.items(): setattr(goal, field, value)
|
||||
with self.subTest(kind=kind):
|
||||
self.assertEqual(self.node._goal("navigate_semantic", goal), self.module.GoalResponse.ACCEPT)
|
||||
self.node.inflight, self.node.motion_reserved = 0, False
|
||||
cell = self.goal("navigate_semantic"); cell.kind = "CELL"
|
||||
self.assertEqual(self.node._goal("navigate_semantic", cell), self.module.GoalResponse.REJECT)
|
||||
for invented in ("region", "shelf", "station"):
|
||||
goal = self.goal("navigate_semantic"); goal.kind = invented
|
||||
with self.subTest(invented=invented):
|
||||
self.assertEqual(self.node._goal("navigate_semantic", goal), self.module.GoalResponse.REJECT)
|
||||
|
||||
def test_semantic_navigation_pose_is_explicit_and_validated(self):
|
||||
pose = {"frame_id": "map", "x": 1.25, "y": -2.5, "z": 0.0,
|
||||
"qx": 0.0, "qy": 0.0, "qz": 0.0, "qw": 1.0}
|
||||
_, explicit = self.execute("navigate_semantic", {"kind": "normal", "duration_seconds": 0,
|
||||
"final_pose": pose})
|
||||
self.assertTrue(explicit.pose_valid and explicit.errors_valid)
|
||||
self.assertEqual(explicit.final_pose.header.frame_id, "map")
|
||||
self.assertEqual((explicit.final_pose.pose.position.x, explicit.final_pose.pose.position.y), (1.25, -2.5))
|
||||
self.node.counts["navigate_semantic"] = 0
|
||||
_, unspecified = self.execute("navigate_semantic", {"kind": "normal", "duration_seconds": 0})
|
||||
self.assertFalse(unspecified.pose_valid)
|
||||
self.assertFalse(unspecified.errors_valid)
|
||||
for raw in (
|
||||
'{"navigate_semantic":{"final_pose":{"frame_id":"map","x":NaN,"y":0,"z":0,"qx":0,"qy":0,"qz":0,"qw":1}}}',
|
||||
'{"navigate_semantic":{"final_pose":{"frame_id":"map","x":1,"y":2,"z":0,"qx":0,"qy":0,"qz":0,"qw":0}}}',
|
||||
):
|
||||
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
||||
self.module.parse_scenarios(raw)
|
||||
|
||||
def test_enabled_interfaces_can_exclude_executor_owned_endpoints(self):
|
||||
module = load_mock_module({
|
||||
"scenarios_json": "{}", "max_goal_seconds": 1.0,
|
||||
"allowed_postures": ["pregrasp", "transport", "home"],
|
||||
"initial_holding_state": "UNKNOWN",
|
||||
"enabled_actions": ["plan_task", "navigate_semantic"],
|
||||
"enabled_topics": ["robot_state"],
|
||||
})
|
||||
node = module.MockSkills()
|
||||
self.assertEqual(node.enabled_actions, ("plan_task", "navigate_semantic"))
|
||||
self.assertEqual(len(node.servers), 2)
|
||||
self.assertTrue(hasattr(node, "state_pub"))
|
||||
self.assertFalse(hasattr(node, "registry_pub"))
|
||||
self.assertFalse(hasattr(node, "progress_pub"))
|
||||
|
||||
def test_verification_default_is_unknown(self):
|
||||
_, result = self.execute("verify_state", {"kind": "unknown", "duration_seconds": 0})
|
||||
self.assertEqual(result.evidence.status, result.evidence.UNKNOWN)
|
||||
self.assertFalse(result.evidence.stopped_valid)
|
||||
|
||||
def test_get_state_and_reconcile_require_bound_evidence(self):
|
||||
srv = sys.modules["bt_skill_interfaces.srv"]
|
||||
response = self.node._get_state(srv.GetRobotState.Request(robot_id="robot_01"), srv.GetRobotState.Response())
|
||||
self.assertTrue(response.available)
|
||||
missing = self.node._get_state(srv.GetRobotState.Request(robot_id="other"), srv.GetRobotState.Response())
|
||||
self.assertFalse(missing.available)
|
||||
self.assertEqual(missing.error_code, "UNKNOWN_ROBOT")
|
||||
handle, unresolved = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
||||
goal_id = bytes(handle.goal_id.uuid).hex()
|
||||
self.assertEqual(unresolved.result.stop_state, unresolved.result.UNKNOWN)
|
||||
req = srv.ReconcileGoal.Request(trace=self.trace(), goal_id="never-accepted", operator_id="op", reason="review")
|
||||
req.evidence.status = req.evidence.PASSED
|
||||
req.evidence.context.source_goal_id = "never-accepted"
|
||||
req.evidence.context.trace = self.trace()
|
||||
req.evidence.context.writer = "independent_sim_observer"
|
||||
req.evidence.context.observed_at.sec = 10
|
||||
req.evidence.context.valid_until.sec = 20
|
||||
req.evidence.stopped_valid = req.evidence.stopped = True
|
||||
req.evidence.source = "SIMULATOR_INDEPENDENT_FIXTURE"
|
||||
req.evidence.evidence_ref = "sim://independent/1"
|
||||
rejected = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
||||
self.assertFalse(rejected.accepted)
|
||||
self.assertTrue(self.node.motion_reserved)
|
||||
req.goal_id = req.evidence.context.source_goal_id = goal_id
|
||||
accepted = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
||||
self.assertTrue(accepted.accepted)
|
||||
self.assertFalse(self.node.motion_reserved)
|
||||
replay = self.node._reconcile(req, srv.ReconcileGoal.Response())
|
||||
self.assertFalse(replay.accepted)
|
||||
|
||||
def test_reconcile_rejects_expired_or_unidentified_evidence(self):
|
||||
srv = sys.modules["bt_skill_interfaces.srv"]
|
||||
handle, _ = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
|
||||
goal_id = bytes(handle.goal_id.uuid).hex()
|
||||
request = srv.ReconcileGoal.Request(trace=self.trace(), goal_id=goal_id, operator_id="op", reason="review")
|
||||
evidence = request.evidence
|
||||
evidence.status, evidence.stopped_valid, evidence.stopped = evidence.PASSED, True, True
|
||||
evidence.context.source_goal_id, evidence.context.trace = goal_id, self.trace()
|
||||
evidence.context.observed_at.sec, evidence.context.valid_until.sec = 1, 2
|
||||
evidence.context.writer, evidence.source, evidence.evidence_ref = "observer", "SIMULATOR_INDEPENDENT_FIXTURE", "sim://e/1"
|
||||
response = self.node._reconcile(request, srv.ReconcileGoal.Response())
|
||||
self.assertFalse(response.accepted)
|
||||
|
||||
def test_precheck_passed_is_fresh_empty_hand_evidence_for_requested_target(self):
|
||||
goal = self.goal("verify_state")
|
||||
goal.check, goal.source_goal_id = goal.PRECHECK, "goal-preflight"
|
||||
goal.target.object_ref = "bottle"
|
||||
self.node.scenarios["verify_state"] = [{"kind": "passed", "duration_seconds": 0}]
|
||||
self.assertEqual(self.node._goal("verify_state", goal), self.module.GoalResponse.ACCEPT)
|
||||
handle = GoalHandle(goal); self.node._accepted("verify_state", handle)
|
||||
evidence = self.node._execute("verify_state", handle).evidence
|
||||
self.assertEqual(evidence.holding_state, evidence.EMPTY)
|
||||
self.assertTrue(evidence.hand_empty_valid and evidence.hand_empty)
|
||||
self.assertEqual(evidence.target_ref, "bottle")
|
||||
observed = evidence.context.observed_at.sec * 1_000_000_000 + evidence.context.observed_at.nanosec
|
||||
valid_until = evidence.context.valid_until.sec * 1_000_000_000 + evidence.context.valid_until.nanosec
|
||||
self.assertGreater(valid_until, observed)
|
||||
|
||||
def test_business_result_enums_execute_through_handlers(self):
|
||||
cases = (
|
||||
("locate_shelf_column", "not_found", "status", "NOT_FOUND"),
|
||||
("locate_shelf_column", "ambiguous", "status", "AMBIGUOUS"),
|
||||
("localize_target_3d", "not_found", "status", "NOT_FOUND"),
|
||||
("localize_target_3d", "ambiguous", "status", "AMBIGUOUS"),
|
||||
("check_free_space", "no_free_space", "status", "NO_FREE_SPACE"),
|
||||
("check_free_space", "ambiguous", "status", "AMBIGUOUS"),
|
||||
("assess_grasp", "adjust_posture", "decision", "ADJUST_POSTURE"),
|
||||
("assess_grasp", "not_reachable", "decision", "NOT_REACHABLE"),
|
||||
("assess_grasp", "unknown", "decision", "UNKNOWN"),
|
||||
)
|
||||
for name, kind, field, constant in cases:
|
||||
self.node.counts[name] = 0
|
||||
_, result = self.execute(name, {"kind": kind, "duration_seconds": 0})
|
||||
with self.subTest(action=name, kind=kind):
|
||||
self.assertEqual(getattr(result, field), getattr(result, constant))
|
||||
self.assertTrue(result.error_code)
|
||||
self.assertTrue(result.message)
|
||||
for kind in ("wrong_object", "wrong_destination"):
|
||||
self.node.counts["verify_state"] = 0
|
||||
_, result = self.execute("verify_state", {"kind": kind, "duration_seconds": 0})
|
||||
self.assertEqual(result.evidence.status, result.evidence.FAILED)
|
||||
for kind, expected in (("model_estimate", "MODEL_ESTIMATE"), ("fused", "FUSED")):
|
||||
self.node.counts["localize_target_3d"] = 0
|
||||
_, result = self.execute("localize_target_3d", {"kind": kind, "duration_seconds": 0})
|
||||
self.assertEqual(result.measurement_source, getattr(result, expected))
|
||||
self.node.counts["plan_task"] = 0
|
||||
ready_plan = {"schema_version": 1, "plan_version": 1, "task_type": "pick_transport_place",
|
||||
"goal": "fetch", "slots": {}, "missing_information": [], "subtasks": []}
|
||||
_, ready = self.execute("plan_task", {"kind": "normal", "duration_seconds": 0, "plan": ready_plan})
|
||||
self.assertEqual(ready.status, ready.PLAN_READY)
|
||||
self.node.counts["plan_task"] = 0
|
||||
_, clarification = self.execute("plan_task", {"kind": "normal", "duration_seconds": 0})
|
||||
self.assertEqual(clarification.status, clarification.NEEDS_CLARIFICATION)
|
||||
|
||||
def test_reject_and_native_mismatch_fixtures_are_explicit(self):
|
||||
goal = self.goal("navigate")
|
||||
self.node.scenarios["navigate"] = [{"kind": "reject"}]
|
||||
self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.REJECT)
|
||||
self.assertEqual(self.node.inflight, 0)
|
||||
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)
|
||||
|
||||
def test_topic_and_result_fixture_values_are_finite_and_bounded(self):
|
||||
for raw in (
|
||||
'{"dense_progress":{"progress":-0.1}}',
|
||||
'{"dense_progress":{"progress":1.1}}',
|
||||
'{"execute_task":{"completed_quantity":-1}}',
|
||||
'{"goal_registry":{"status_json":"[]"}}',
|
||||
'{"visual_observation":{"image_path":7}}',
|
||||
):
|
||||
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
||||
self.module.parse_scenarios(raw)
|
||||
|
||||
def test_fixture_kinds_must_have_effect_for_the_selected_interface(self):
|
||||
for raw in ('{"navigate":{"kind":"no_free_space"}}',
|
||||
'{"goal_registry":{"kind":"wrong_object"}}',
|
||||
'{"assess_grasp":{"kind":"stale_observation"}}'):
|
||||
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
||||
self.module.parse_scenarios(raw)
|
||||
|
||||
def test_all_mock_topics_publish_configurable_positive_and_negative_fixtures(self):
|
||||
self.node.scenarios.update({
|
||||
"robot_state": [{"kind": "invalid_pose"}],
|
||||
"safety_state": [{"kind": "emergency_stop"}],
|
||||
"visual_observation": [{"kind": "normal", "observation_id": "obs-7", "image_path": "/tmp/scene-7.png"}],
|
||||
"dense_progress": [{"kind": "normal", "state": "IN_PROGRESS", "progress": 0.4}],
|
||||
"goal_registry": [{"kind": "normal", "status_json": '{"goal_id":"g-7","state":"ACTIVE"}'}],
|
||||
})
|
||||
self.node._publish_states()
|
||||
robot, safety = self.node.state_pub.values[-1], self.node.safety_pub.values[-1]
|
||||
self.assertFalse(robot.pose_valid)
|
||||
self.assertTrue(safety.emergency_stop_active)
|
||||
self.assertFalse(safety.motion_allowed)
|
||||
self.assertEqual(self.node.observation_pub.values[-1].observation_id, "obs-7")
|
||||
self.assertEqual(self.node.observation_pub.values[-1].image_path, "/tmp/scene-7.png")
|
||||
self.assertTrue(self.node.progress_pub.values[-1].progress_valid)
|
||||
self.assertAlmostEqual(self.node.progress_pub.values[-1].progress, 0.4)
|
||||
self.assertEqual(json.loads(self.node.registry_pub.values[-1].data)["goal_id"], "g-7")
|
||||
self.assertEqual(self.node.observation_pub.name, "observations/scene")
|
||||
self.assertEqual(self.node.progress_pub.name, "monitor/dense_progress")
|
||||
self.assertEqual(self.node.registry_pub.name, "goal_registry")
|
||||
|
||||
def test_canceled_result_cannot_be_overwritten_by_failed_fixture(self):
|
||||
handle, result = self.execute("execute_task", {"kind": "failed", "duration_seconds": 1}, cancel=True)
|
||||
self.assertEqual(handle.native, "canceled")
|
||||
self.assertEqual(result.result.status, result.result.CANCELED)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,44 @@
|
||||
import unittest,tempfile,sys,copy
|
||||
from pathlib import Path
|
||||
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'coordinator'))
|
||||
from robot_bt_coordinator.service import Coordinator,demo_site
|
||||
from robot_bt_coordinator.backends import ManualBackend
|
||||
from robot_bt_coordinator.replay import replay_events
|
||||
from test_plan_v2 import sample
|
||||
class MultiItemTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.tmp=tempfile.TemporaryDirectory();self.backend=ManualBackend();self.site=demo_site();self.site['execution_route']='OBJECT_TABLE'
|
||||
self.c=Coordinator(self.tmp.name+'/tasks.db',self.backend,['r'],self.site)
|
||||
def tearDown(self):self.c.close();self.tmp.cleanup()
|
||||
def start(self):
|
||||
try:t=self.c.submit(dict(client_request_id='q',robot_id='r',instruction='two waters then one doll',known_info=sample()['slots']))
|
||||
except Exception as ex:self.fail(str(ex))
|
||||
self.tid=t['task_id'];self.c.tick();t=self.c.get(self.tid)
|
||||
self.backend.emit(dict(type='plan',task_id=self.tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=sample()));self.c.tick()
|
||||
self.assertEqual(self.c.get(self.tid)['status'],'EXECUTING')
|
||||
def complete(self,**changes):
|
||||
t=self.c.get(self.tid);target=t['context']['target_id'];e=dict(type='execution_result',task_id=self.tid,run_id=t['run_id'],status='SUCCEEDED',completed_quantity=1,stop_confirmed=True,evidence=dict(passed=True,empty_hand=True,in_destination=True,valid=True,safe_to_release=True,evidence_id='ev'+t['run_id'],target_ref=target,destination_ref='tote_A'));e.update(changes)
|
||||
self.backend.emit(e);self.c.tick();return e
|
||||
def test_three_runs_and_idempotent_delivery(self):
|
||||
self.start();runs=[]
|
||||
for n in range(3):
|
||||
runs.append(self.c.get(self.tid)['run_id']);e=self.complete();self.backend.emit(e);self.c.tick()
|
||||
self.assertEqual(self.c.get(self.tid)['completed_quantity'],n+1)
|
||||
t=self.c.get(self.tid);self.assertEqual(t['status'],'SUCCEEDED');self.assertEqual(len(set(runs)),3)
|
||||
self.assertEqual([x['context']['target_id'] for x in self.backend.executions],['water','water','doll'])
|
||||
self.assertEqual(replay_events(self.c.events(self.tid,limit=500))['completed_quantity'],3)
|
||||
def test_partial_failure_retains_quantity_and_blocks_queue(self):
|
||||
self.start();self.complete();self.complete(status='INTERVENTION_REQUIRED',stop_confirmed=False,completed_quantity=0)
|
||||
t=self.c.get(self.tid);self.assertEqual(t['status'],'INTERVENTION_REQUIRED');self.assertEqual(t['completed_quantity'],1);self.assertEqual(len(self.backend.executions),2)
|
||||
def test_cancel_racing_success_does_not_start_next_item(self):
|
||||
self.start();self.c.control(self.tid,'cancel');self.complete()
|
||||
self.assertEqual(self.c.get(self.tid)['status'],'CANCELED');self.assertEqual(len(self.backend.executions),1)
|
||||
def test_restart_after_partial_delivery_requires_reconciliation(self):
|
||||
self.start();self.complete();self.c.close();self.backend=ManualBackend()
|
||||
self.c=Coordinator(self.tmp.name+'/tasks.db',self.backend,['r'],self.site)
|
||||
self.assertEqual(self.c.get(self.tid)['status'],'INTERVENTION_REQUIRED');self.assertEqual(self.c.get(self.tid)['completed_quantity'],1)
|
||||
self.c.tick();self.assertEqual(self.backend.executions,[])
|
||||
def test_advisory_progress_cannot_commit_delivery(self):
|
||||
self.start();t=self.c.get(self.tid)
|
||||
self.backend.emit(dict(type='advisory',task_id=self.tid,run_id=t['run_id'],state='RUNNING',progress=1.));self.c.tick()
|
||||
self.assertEqual(self.c.get(self.tid)['status'],'EXECUTING');self.assertEqual(self.c.get(self.tid)['completed_quantity'],0)
|
||||
@@ -0,0 +1,10 @@
|
||||
import unittest
|
||||
class CatalogTests(unittest.TestCase):
|
||||
def test_names_cells_and_version_are_exact(self):
|
||||
from navigation_gateway.catalog import Catalog
|
||||
site={'registry_version':3,'locations':{'stop':{'frame_id':'map','x':1,'y':2,'z':0,'qx':0,'qy':0,'qz':0,'qw':1}},'object_locations':{'water':'stop'},'cell_locations':{'s/FRONT/1/2':'stop'}}
|
||||
c=Catalog(site)
|
||||
self.assertEqual(c.resolve('OBJECT','water',3)['pose']['x'],1)
|
||||
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)
|
||||
@@ -0,0 +1,492 @@
|
||||
"""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()
|
||||
@@ -0,0 +1,29 @@
|
||||
import sys, unittest, copy
|
||||
from pathlib import Path
|
||||
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'coordinator'))
|
||||
from robot_bt_coordinator.plan import validate_plan, validate_known
|
||||
from robot_bt_coordinator.errors import ApiError
|
||||
|
||||
def sample(route='OBJECT_TABLE'):
|
||||
items=[{'target_name':'water','quantity':2,'source_location':'shelf_A'},{'target_name':'doll','quantity':1,'source_location':'shelf_A'}]
|
||||
p=dict(schema_version=2,plan_version=1,task_type='multi_item_pick_transport_place',goal='two waters then one doll',route=route,slots={'items':items,'destination':'tote_A'},missing_information=[],subtasks=[])
|
||||
for idx,target in enumerate(['water','water','doll']):
|
||||
rows=[('NAVIGATE',{'target':target}),('PICK',{'target':target}),('NAVIGATE',{'destination':'tote_A'}),('PLACE',{'target':target,'destination':'tote_A'})]
|
||||
if route=='SHELF_CELL': rows=[('NAVIGATE',{'source_location':'shelf_A','mode':'observation'}),('ROBOBRAIN_SHELF_LOCALIZE',{'target':target}),('NAVIGATE',{'source_location':'shelf_A','mode':'shelf_cell'})]+rows[1:]
|
||||
for skill,args in rows:
|
||||
n=len(p['subtasks']);p['subtasks'].append(dict(id=f'S{n+1}',skill=skill,arguments=dict(args,item_index=idx),depends_on=[] if not n else [f'S{n}']))
|
||||
return p
|
||||
class PlanV2Tests(unittest.TestCase):
|
||||
def test_accept_two_routes_and_multi_items(self):
|
||||
for route in ['OBJECT_TABLE','SHELF_CELL']:
|
||||
try:p=validate_plan(sample(route))
|
||||
except ApiError as e:self.fail('new DR plan rejected: '+str(e))
|
||||
self.assertEqual(p['slots']['items'][0]['quantity'],2)
|
||||
def test_known_multi(self):
|
||||
try:result=validate_known(sample()['slots'])
|
||||
except ApiError as e:self.fail(str(e))
|
||||
self.assertEqual(result['items'][1]['target_name'],'doll')
|
||||
def test_reject_broken_dependency_unknown_skill_quantity_order(self):
|
||||
for edit in [lambda p:p['subtasks'][4].update(depends_on=[]),lambda p:p['subtasks'][1].update(skill='<PICK>'),lambda p:p['slots']['items'][0].update(quantity=True),lambda p:p['slots']['items'][0].update(quantity=21),lambda p:p['subtasks'][1]['arguments'].update(target='doll')]:
|
||||
p=sample();edit(p)
|
||||
with self.assertRaises(ApiError):validate_plan(p)
|
||||
@@ -0,0 +1,23 @@
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'coordinator'))
|
||||
from robot_bt_coordinator.service import Coordinator
|
||||
from robot_bt_coordinator.backends import ManualBackend
|
||||
from robot_bt_coordinator.replay import replay_events
|
||||
|
||||
class ReplayTest(unittest.TestCase):
|
||||
def test_real_canceled_task_replays_without_transport(self):
|
||||
with tempfile.TemporaryDirectory() as tmp:
|
||||
c=Coordinator(str(Path(tmp)/'db'),ManualBackend(),{'robot_01'})
|
||||
try:
|
||||
tid=c.submit(dict(client_request_id='replay',robot_id='robot_01',instruction='取水'))['task_id']
|
||||
c.control(tid,'cancel');events=c.events(tid)
|
||||
out=replay_events(events)
|
||||
self.assertEqual(out['status'],'CANCELED');self.assertFalse(out['connects_to_robot'])
|
||||
finally:c.close()
|
||||
def test_missing_status_event_detected(self):
|
||||
with self.assertRaises(ValueError):replay_events([dict(event_id=10,kind='state',status_version=3,status='EXECUTING')])
|
||||
def test_success_without_commit_is_not_replayed_as_success(self):
|
||||
with self.assertRaises(ValueError):replay_events([dict(event_id=1,kind='state',status_version=1,status='SUCCEEDED')])
|
||||
@@ -0,0 +1,99 @@
|
||||
import unittest,sys,tempfile,json,time,threading
|
||||
from pathlib import Path
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
sys.path[:0]=[str(ROOT/'coordinator'),str(ROOT/'robobrain')]
|
||||
class RoboBrainTests(unittest.TestCase):
|
||||
def service(self,raw):
|
||||
from robot_robobrain.service import BrainService
|
||||
from robot_robobrain.backends import FixtureBackend
|
||||
self.tmp=tempfile.TemporaryDirectory();self.addCleanup(self.tmp.cleanup)
|
||||
return BrainService(FixtureBackend(raw),self.tmp.name)
|
||||
def goal(self):
|
||||
return dict(task_id='t',task_revision=1,planning_generation=1,instruction='two waters then one doll',known_info={'items':[{'target_name':'water','quantity':2,'source_location':'shelf_A'},{'target_name':'doll','quantity':1,'source_location':'shelf_A'}],'destination':'tote_A'},context={},constraints={'route':'OBJECT_TABLE','schema_version':2},timeout=2)
|
||||
def test_plan_raw_archive_and_semantic_mismatch(self):
|
||||
from test_plan_v2 import sample
|
||||
p=sample();s=self.service(json.dumps(p));r=s.plan(self.goal());self.assertEqual(r['status'],'PLAN_READY');self.assertTrue(Path(r['record_ref']).is_file())
|
||||
g=self.goal();g['known_info']['items'][0]['quantity']=1
|
||||
self.assertEqual(s.plan(g)['error_code'],'SEMANTIC_MISMATCH')
|
||||
def test_invalid_json_and_missing_information(self):
|
||||
self.assertEqual(self.service('```json {} ```').plan(self.goal())['error_code'],'PARSE_ERROR')
|
||||
r=self.service('{"missing_information":["which destination?"]}').plan(self.goal())
|
||||
self.assertEqual(r['status'],'NEEDS_CLARIFICATION')
|
||||
def test_shelf_ambiguity_freshness_and_geometry_untrusted(self):
|
||||
from robot_robobrain.observations import Observation
|
||||
media=tempfile.TemporaryDirectory();self.addCleanup(media.cleanup);path=Path(media.name)/'image.jpg';path.write_bytes(b'fixture-image')
|
||||
o=Observation('obs',100,'camera',str(path),'observe_A',1,'shelf_A')
|
||||
s=self.service('{"status":"SUCCEEDED","shelf_id":"shelf_A","side_id":"FRONT","column_id":"1","tier_id":"2","confidence":0.95}')
|
||||
g=dict(task_id='t',subtask_id='s',target_ref='water',source_region_ref='shelf_A',observation_station_id='observe_A',station_registry_version=1,capture_after=90,timeout=1)
|
||||
self.assertEqual(s.shelf(g,o,now_ns=110,max_age_ns=50)['status'],'SUCCEEDED')
|
||||
self.assertEqual(s.shelf(g,o,now_ns=200,max_age_ns=50)['error_code'],'OBSERVATION_INVALID')
|
||||
self.assertEqual(self.service('{"status":"AMBIGUOUS"}').shelf(g,o,now_ns=110,max_age_ns=50)['status'],'AMBIGUOUS')
|
||||
r=self.service('{"point":[1,2,3]}').localize(g,o,now_ns=110,max_age_ns=50)
|
||||
self.assertFalse(r['geometry_valid']);self.assertEqual(r['target_point']['point'],[1,2,3])
|
||||
def test_progress_never_completes_and_rejects_stale(self):
|
||||
from robot_robobrain.progress import ProgressMonitor
|
||||
m=ProgressMonitor('run','pick',capture_after=1,max_age=2,stall_seconds=3)
|
||||
for n in range(1,8):r=m.update(dict(run_id='run',subtask_id='pick',sequence=n,stamp=float(n),progress=.8,hop={'raw':1}),now=float(n))
|
||||
self.assertEqual(r['state'],'STALLED');self.assertFalse(r['completion_authority'])
|
||||
self.assertEqual(m.update(dict(run_id='old',subtask_id='pick',sequence=9,stamp=9.,progress=1.,hop=1),now=9.)['state'],'UNKNOWN')
|
||||
self.assertEqual(m.snapshot(20.)['state'],'UNKNOWN')
|
||||
|
||||
class WorkerAndMonitorTests(unittest.TestCase):
|
||||
def test_worker_roundtrip_timeout_and_cancel(self):
|
||||
from robot_robobrain.backends import ProcessBackend,InferenceError
|
||||
command=[sys.executable,'-u','-c','import sys,json,time\nfor l in sys.stdin:\n q=json.loads(l);time.sleep(q.get("delay",0));print(json.dumps({"raw":"ok"}),flush=True)']
|
||||
b=ProcessBackend(command,'test-worker');self.addCleanup(b.close)
|
||||
self.assertEqual(b.infer({},1),'ok');self.assertEqual(b.infer({},1),'ok')
|
||||
with self.assertRaises(InferenceError) as ctx:b.infer({'delay':2},.1)
|
||||
self.assertEqual(ctx.exception.code,'TIMEOUT');self.assertIsNone(b.process)
|
||||
cancel=threading.Event();timer=threading.Timer(.05,cancel.set);timer.start()
|
||||
with self.assertRaises(InferenceError) as ctx:b.infer({'delay':2},1,cancel)
|
||||
timer.join();self.assertEqual(ctx.exception.code,'CANCELED');self.assertIsNone(b.process)
|
||||
def test_worker_crash_and_unbounded_output(self):
|
||||
from robot_robobrain.backends import ProcessBackend,InferenceError
|
||||
for source in ['import sys;sys.exit(1)','import sys;sys.stdin.readline();print("x"*300000,flush=True)']:
|
||||
b=ProcessBackend([sys.executable,'-u','-c',source],'test');self.addCleanup(b.close)
|
||||
with self.assertRaises(InferenceError):b.infer({},1)
|
||||
def test_monitor_full_progress_does_not_complete_regression_and_nan(self):
|
||||
from robot_robobrain.progress import ProgressMonitor
|
||||
m=ProgressMonitor('r','s',1)
|
||||
for n,p in enumerate([1,1,1,.1,.1,.1,.1,.1],1):r=m.update(dict(run_id='r',subtask_id='s',sequence=n,stamp=n,progress=p,hop=1),n)
|
||||
self.assertEqual(r['state'],'REGRESSED');self.assertFalse(r['completion_authority'])
|
||||
for p in [float('nan'),True,-.1,1.01]:self.assertEqual(m.update(dict(run_id='r',subtask_id='s',sequence=9,stamp=9,progress=p),9)['state'],'UNKNOWN')
|
||||
def test_dense_window_service_and_future_window(self):
|
||||
from robot_robobrain.backends import FixtureBackend
|
||||
from robot_robobrain.dopamine import DenseFeedbackService
|
||||
with tempfile.TemporaryDirectory() as tmp:
|
||||
frame=Path(tmp)/'frame.jpg';frame.write_bytes(b'fixture-image')
|
||||
s=DenseFeedbackService(FixtureBackend('{"progress":1,"hop":3}'),tmp)
|
||||
g=dict(task_id='t',run_id='r',subtask_id='pick',task_description='pick water',capture_after=1,sequence=1,timeout=1)
|
||||
r=s.evaluate(g,[{'stamp':2.,'views':{'front':str(frame)}}],2.)
|
||||
self.assertEqual(r['state'],'RUNNING');self.assertFalse(r['completion_authority'])
|
||||
self.assertEqual(s.evaluate(g,[{'stamp':9.,'views':{'front':str(frame)}}],2.)['state'],'UNKNOWN')
|
||||
def test_observation_path_cannot_escape_media_root(self):
|
||||
from robot_robobrain.observations import Observation,ObservationCache
|
||||
with tempfile.TemporaryDirectory() as tmp:
|
||||
root=Path(tmp)/'media';root.mkdir();outside=Path(tmp)/'private';outside.write_text('x');(root/'link').symlink_to(outside)
|
||||
cache=ObservationCache(root)
|
||||
with self.assertRaises(ValueError):cache.put(Observation('o',1,'camera',str(root/'link'),'s',1,'shelf'))
|
||||
|
||||
class IntentTests(unittest.TestCase):
|
||||
def test_explicit_quantity_and_order_are_checked_without_known_info(self):
|
||||
from robot_robobrain.intent import extract
|
||||
from robot_robobrain.service import BrainService
|
||||
from robot_robobrain.backends import FixtureBackend
|
||||
from robot_bt_coordinator.plan_v2 import make_plan
|
||||
site=json.loads((ROOT/'config/sim_site_object_table.json').read_text());instruction='把货架A的两瓶水和一个玩偶放到周转箱A'
|
||||
slots=extract(instruction,site);self.assertEqual([x['quantity'] for x in slots['items']],[2,1])
|
||||
bad=json.loads(json.dumps(slots));bad['items'][0]['quantity']=1
|
||||
with tempfile.TemporaryDirectory() as tmp:
|
||||
s=BrainService(FixtureBackend(json.dumps(make_plan(instruction,bad,'OBJECT_TABLE'))),tmp)
|
||||
g=dict(task_id='t',task_revision=1,planning_generation=1,instruction=instruction,known_info={},context=site,constraints={'route':'OBJECT_TABLE'},timeout=1)
|
||||
self.assertEqual(s.plan(g)['error_code'],'SEMANTIC_MISMATCH')
|
||||
g['instruction']='拿一些水'
|
||||
self.assertEqual(s.plan(g)['status'],'NEEDS_CLARIFICATION')
|
||||
def test_frame_window_is_bounded_and_reset_on_clock_reversal(self):
|
||||
from robot_robobrain.windows import FrameWindow
|
||||
w=FrameWindow(3)
|
||||
for i in range(10):w.add(i,'front',str(i))
|
||||
self.assertEqual(len(w.since(0,9)),3);w.add(1,'front','new');self.assertEqual(len(w.since(0,1)),1)
|
||||
@@ -0,0 +1,226 @@
|
||||
"""Source contract + local static checks; this suite does NOT run ROS2/colcon."""
|
||||
import ast
|
||||
import importlib.util
|
||||
import re
|
||||
import unittest
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
INTERFACES = ROOT / "ros2" / "bt_skill_interfaces"
|
||||
MOCKS = ROOT / "ros2" / "bt_mock_servers"
|
||||
PRIMITIVES = {"bool", "byte", "char", "float32", "float64", "int8", "uint8", "int16", "uint16", "int32", "uint32", "int64", "uint64", "string", "wstring"}
|
||||
EXTERNAL = {"builtin_interfaces/Time", "builtin_interfaces/Duration", "geometry_msgs/PoseStamped", "geometry_msgs/PointStamped", "std_msgs/Header"}
|
||||
|
||||
|
||||
def fields(path):
|
||||
return [line.split("#", 1)[0].strip() for line in path.read_text().splitlines()
|
||||
if line.split("#", 1)[0].strip()]
|
||||
|
||||
|
||||
def sections(name):
|
||||
return "\n".join(fields(INTERFACES / "action" / (name + ".action"))).split("\n---\n")
|
||||
|
||||
|
||||
def load_scenarios():
|
||||
spec = importlib.util.spec_from_file_location("mock_scenarios_test", MOCKS / "bt_mock_servers" / "scenarios.py")
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(module)
|
||||
return module
|
||||
|
||||
|
||||
class RosContractTests(unittest.TestCase):
|
||||
def test_ros_goal_builders_fill_required_target_descriptions(self):
|
||||
# Static serialization guard only: native generated ROS types still need
|
||||
# the Humble build/live gate. These fields are required by the mock and
|
||||
# ObjectTarget/RegionTarget contracts, not optional display labels.
|
||||
source = (ROOT / "ros2/bt_executor/src/ros_driver.cpp").read_text()
|
||||
assess = source.split("Assess::Goal g;", 1)[1].split("send_typed<Assess>", 1)[0]
|
||||
verify = source.split("Verify::Goal g;", 1)[1].split("send_typed<Verify>", 1)[0]
|
||||
self.assertIn("g.target_binding.target.description=r.target_id;", assess)
|
||||
self.assertIn("g.target.description=r.target_id;", verify)
|
||||
self.assertIn("g.destination.description=r.destination_id;", verify)
|
||||
|
||||
def test_action_and_service_sections(self):
|
||||
self.assertEqual(len(list((INTERFACES / "action").glob("*.action"))), 12)
|
||||
for suffix, expected in (("action", 2), ("srv", 1), ("msg", 0)):
|
||||
for path in (INTERFACES / suffix).glob("*." + suffix):
|
||||
with self.subTest(path=path.name):
|
||||
self.assertEqual(fields(path).count("---"), expected)
|
||||
self.assertNotIn("\u200b", path.read_text())
|
||||
|
||||
def test_all_types_resolve_and_fields_are_valid(self):
|
||||
messages = {"bt_skill_interfaces/" + p.stem for p in (INTERFACES / "msg").glob("*.msg")}
|
||||
for folder in ("msg", "srv", "action"):
|
||||
for path in (INTERFACES / folder).glob("*." + folder):
|
||||
names = set()
|
||||
for line in fields(path):
|
||||
if line == "---":
|
||||
names = set()
|
||||
continue
|
||||
tokens = line.split(maxsplit=1)
|
||||
self.assertEqual(len(tokens), 2, (path.name, line))
|
||||
raw_type, assignment = tokens
|
||||
kind = re.sub(r"\[(?:\d*)\]$", "", raw_type)
|
||||
self.assertIn(kind, PRIMITIVES | EXTERNAL | messages, (path.name, line))
|
||||
name = assignment.split("=", 1)[0]
|
||||
self.assertNotIn(name, names, (path.name, line))
|
||||
names.add(name)
|
||||
if "=" in assignment:
|
||||
self.assertRegex(name, r"^[A-Z][A-Z0-9_]*$")
|
||||
value = assignment.split("=", 1)[1]
|
||||
self.assertRegex(value, r"^\d+$")
|
||||
self.assertLessEqual(int(value), 255)
|
||||
else:
|
||||
self.assertRegex(name, r"^[a-z][a-z0-9_]*$")
|
||||
|
||||
def test_navigate_exact_source_outer_contract(self):
|
||||
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",
|
||||
]))
|
||||
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",
|
||||
]))
|
||||
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",
|
||||
"bool blocked", "builtin_interfaces/Duration elapsed_time", "string message",
|
||||
]))
|
||||
self.assertFalse((INTERFACES / "action" / "ExecuteNavigation.action").exists())
|
||||
|
||||
def test_manipulation_exact_source_outer_contract(self):
|
||||
goal, result, feedback = sections("ExecuteManipulation")
|
||||
self.assertEqual(goal, "\n".join([
|
||||
"bt_skill_interfaces/TaskTrace trace", "string skill", "string instruction",
|
||||
"bt_skill_interfaces/ObjectTarget target", "bt_skill_interfaces/RegionTarget destination",
|
||||
"builtin_interfaces/Duration timeout",
|
||||
]))
|
||||
self.assertEqual(result, "bt_skill_interfaces/ExecutionResult result\nstring execution_record_ref")
|
||||
self.assertEqual(feedback, "\n".join([
|
||||
"uint8 PREPARING=0", "uint8 WAITING_OBSERVATION=1", "uint8 INFERRING=2", "uint8 EXECUTING=3",
|
||||
"uint8 COMPLETING=4", "uint8 STOPPING=5", "builtin_interfaces/Time stamp", "uint32 sequence",
|
||||
"uint8 phase", "bool progress_valid", "float32 progress", "builtin_interfaces/Duration elapsed_time", "string message",
|
||||
]))
|
||||
|
||||
def test_shared_result_and_target_contracts(self):
|
||||
lines = fields(INTERFACES / "msg" / "ExecutionResult.msg")
|
||||
self.assertEqual(lines[:7], ["uint8 COMPLETED=0", "uint8 FAILED=1", "uint8 CANCELED=2",
|
||||
"uint8 TIMED_OUT=3", "uint8 REJECTED=4", "uint8 UNKNOWN=0", "uint8 CONFIRMED=1"])
|
||||
self.assertEqual(fields(INTERFACES / "msg" / "ObjectTarget.msg"), ["string object_ref", "string description"])
|
||||
self.assertEqual(fields(INTERFACES / "msg" / "RegionTarget.msg"), ["string region_ref", "string description"])
|
||||
self.assertEqual(fields(INTERFACES / "msg" / "TaskTrace.msg"), [
|
||||
"string task_id", "string subtask_id", "uint32 attempt", "uint32 task_revision",
|
||||
"uint32 plan_version", "string run_id", "uint64 execution_generation",
|
||||
])
|
||||
|
||||
def test_source_perception_statuses_and_required_observations(self):
|
||||
for name in ("LocateShelfColumn", "LocalizeTarget3D"):
|
||||
self.assertEqual(sections(name)[1].splitlines()[:4],
|
||||
["uint8 SUCCEEDED=0", "uint8 FAILED=1", "uint8 NOT_FOUND=2", "uint8 AMBIGUOUS=3"])
|
||||
self.assertEqual(sections("CheckFreeSpace")[1].splitlines()[:4],
|
||||
["uint8 SUCCEEDED=0", "uint8 FAILED=1", "uint8 NO_FREE_SPACE=2", "uint8 AMBIGUOUS=3"])
|
||||
for name in ("LocateShelfColumn", "LocalizeTarget3D", "CheckFreeSpace"):
|
||||
self.assertIn("builtin_interfaces/Time capture_after", sections(name)[0])
|
||||
self.assertIn("string observation_id", sections(name)[1])
|
||||
localization = sections("LocalizeTarget3D")
|
||||
self.assertIn("uint64 expected_geometry_epoch", localization[0])
|
||||
self.assertIn("uint64 geometry_epoch", localization[1])
|
||||
self.assertIn("bool grasp_point_valid", localization[1])
|
||||
self.assertIn("builtin_interfaces/Time valid_until", sections("CheckFreeSpace")[1])
|
||||
self.assertEqual(sections("PlanTask")[1].splitlines()[:3],
|
||||
["uint8 PLAN_READY=0", "uint8 NEEDS_CLARIFICATION=1", "uint8 FAILED=2"])
|
||||
|
||||
def test_task_boundary_and_reconciliation_have_bound_evidence(self):
|
||||
goal, result, feedback = sections("ExecuteTask")
|
||||
self.assertEqual(goal.splitlines(), ["bt_skill_interfaces/TaskTrace trace", "string approved_plan_json",
|
||||
"string context_json", "builtin_interfaces/Duration timeout"])
|
||||
self.assertEqual(result.splitlines(), ["bt_skill_interfaces/ExecutionResult result", "uint32 completed_quantity", "string evidence_json"])
|
||||
self.assertEqual(feedback.splitlines(), ["builtin_interfaces/Time stamp", "uint32 sequence", "string stage", "string status_json"])
|
||||
reconcile = fields(INTERFACES / "srv" / "ReconcileGoal.srv")
|
||||
request = reconcile[:reconcile.index("---")]
|
||||
self.assertIn("bt_skill_interfaces/TaskTrace trace", request)
|
||||
self.assertIn("string goal_id", request)
|
||||
self.assertIn("bt_skill_interfaces/VerificationEvidence evidence", request)
|
||||
self.assertFalse(any(line.startswith("bool ") for line in request))
|
||||
context = fields(INTERFACES / "msg" / "ObservationContext.msg")
|
||||
for line in ("bt_skill_interfaces/TaskTrace trace", "string source_goal_id", "uint64 geometry_epoch",
|
||||
"builtin_interfaces/Time observed_at", "builtin_interfaces/Time valid_until", "string writer"):
|
||||
self.assertIn(line, context)
|
||||
|
||||
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"))
|
||||
self.assertEqual(sorted(declarations), actual)
|
||||
self.assertEqual(len(declarations), len(set(declarations)))
|
||||
manifest = ET.parse(INTERFACES / "package.xml").getroot()
|
||||
self.assertEqual(manifest.findtext("name"), "bt_skill_interfaces")
|
||||
self.assertEqual(manifest.findtext("member_of_group"), "rosidl_interface_packages")
|
||||
self.assertIn("geometry_msgs", [node.text for node in manifest.findall("depend")])
|
||||
mock_manifest = ET.parse(MOCKS / "package.xml").getroot()
|
||||
self.assertEqual(mock_manifest.findtext("export/build_type"), "ament_python")
|
||||
self.assertTrue((MOCKS / "resource" / "bt_mock_servers").exists())
|
||||
|
||||
def test_mock_python_syntax_compiles_without_claiming_ros_execution(self):
|
||||
for path in MOCKS.rglob("*.py"):
|
||||
with self.subTest(path=path.name):
|
||||
source = path.read_text()
|
||||
ast.parse(source, filename=str(path))
|
||||
compile(source, str(path), "exec")
|
||||
|
||||
|
||||
class MockFixtureTests(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
cls.module = load_scenarios()
|
||||
|
||||
def test_default_verification_is_unknown(self):
|
||||
self.assertEqual(self.module.fixture_at({}, "verify_state", 0)["kind"], "unknown")
|
||||
self.assertEqual(self.module.fixture_at({}, "execute_manipulation", 0)["kind"], "normal")
|
||||
|
||||
def test_fixture_sequences_hold_last_state(self):
|
||||
fixtures = self.module.parse_scenarios('{"verify_state":[{"kind":"passed"},{"kind":"wrong_destination"}]}')
|
||||
self.assertEqual(self.module.fixture_at(fixtures, "verify_state", 0)["kind"], "passed")
|
||||
self.assertEqual(self.module.fixture_at(fixtures, "verify_state", 9)["kind"], "wrong_destination")
|
||||
|
||||
def test_fixture_parser_rejects_ambiguous_unbounded_inputs(self):
|
||||
for raw in ('{"navigate":{},"navigate":{}}', '{"navigate":{"duration_seconds":NaN}}',
|
||||
'{"navigate":{"duration_seconds":121}}', '{"navigate":{"duration_seconds":true}}',
|
||||
'{"navigate":{"kind":"anything"}}', '{"navigate":{"execute":true}}',
|
||||
'{"verify_state":[]}', '[]'):
|
||||
with self.subTest(raw=raw), self.assertRaises(ValueError):
|
||||
self.module.parse_scenarios(raw)
|
||||
|
||||
def test_duration_accepts_only_positive_normalized_values(self):
|
||||
self.assertEqual(self.module.duration_seconds(SimpleNamespace(sec=1, nanosec=500000000)), 1.5)
|
||||
for sec, nanosec in ((0, 0), (-1, 500), (1, 1000000000), (1, -1)):
|
||||
with self.assertRaises(ValueError):
|
||||
self.module.duration_seconds(SimpleNamespace(sec=sec, nanosec=nanosec))
|
||||
|
||||
def test_trace_requires_revision_run_and_generation(self):
|
||||
values = dict(task_id="task", subtask_id="pick", attempt=1, task_revision=1, plan_version=1, run_id="run", execution_generation=1)
|
||||
self.module.validate_trace(SimpleNamespace(**values))
|
||||
for key in values:
|
||||
broken = dict(values)
|
||||
broken[key] = "" if isinstance(broken[key], str) else 0
|
||||
with self.subTest(key=key), self.assertRaises(ValueError):
|
||||
self.module.validate_trace(SimpleNamespace(**broken))
|
||||
|
||||
def test_planner_fixture_is_fixed_chain_or_clarification(self):
|
||||
self.assertTrue(self.module.fixed_plan("fetch", {})["missing_information"])
|
||||
self.assertEqual(self.module.fixed_plan("fetch", {})["subtasks"], [])
|
||||
plan = self.module.fixed_plan("fetch", {"target_name": "bottle", "source_location": "shelf_A", "destination": "tote_A"})
|
||||
self.assertEqual([step["skill"] for step in plan["subtasks"]],
|
||||
["NAVIGATE", "GROUND_TARGET", "PICK", "NAVIGATE", "CHECK_FREE_SPACE", "PLACE"])
|
||||
self.assertEqual(plan["subtasks"][-1]["arguments"], {"target": "bottle", "destination": "tote_A"})
|
||||
self.assertEqual(plan["slots"]["quantity"], 1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user