227 lines
13 KiB
Python
227 lines
13 KiB
Python
|
"""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()
|