behavior-tree/tests/test_ros_contracts.py

"""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"
NAVIGATION = ROOT / "ros2" / "navigation_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"))), 10)
        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_flat_canonical_contract(self):
        goal, result, feedback = "\n".join(fields(NAVIGATION / "action/NavigateToPose.action")).split("\n---\n")
        self.assertEqual(goal.splitlines(), ["string task_id", "string subtask_id",
            "geometry_msgs/PoseStamped target_pose", "float64 position_tolerance",
            "float64 yaw_tolerance", "builtin_interfaces/Duration timeout"])
        self.assertEqual(result.splitlines(), ["uint8 SUCCEEDED=0", "uint8 CANCELED=1",
            "uint8 TIMEOUT=2", "uint8 BLOCKED=3", "uint8 NOT_READY=4", "uint8 FAILED=5",
            "uint8 STOP_UNKNOWN=0", "uint8 STOP_CONFIRMED=1", "uint8 status", "string error_code",
            "string message", "bool final_pose_valid", "geometry_msgs/PoseStamped final_pose",
            "float64 final_position_error", "float64 final_yaw_error", "uint8 stop_state",
            "builtin_interfaces/Time stopped_at", "string stop_evidence_ref"])
        self.assertEqual(feedback.splitlines(), ["uint8 ACCEPTED=0", "uint8 CHECKING=1",
            "uint8 PLANNING=2", "uint8 NAVIGATING=3", "uint8 BLOCKED=4", "uint8 STOPPING=5",
            "builtin_interfaces/Time stamp", "uint64 sequence", "uint8 phase", "bool current_pose_valid",
            "geometry_msgs/PoseStamped current_pose", "bool error_valid", "float64 position_error",
            "float64 yaw_error", "bool blocked_valid", "bool blocked",
            "builtin_interfaces/Duration elapsed_time", "string message"])
        self.assertIn('"action/NavigateToPose.action"', (NAVIGATION / "CMakeLists.txt").read_text())
        self.assertEqual(ET.parse(NAVIGATION / "package.xml").getroot().findtext("name"), "navigation_interfaces")

    def test_navigation_has_no_duplicate_wire_contract_or_proxy(self):
        for path in ("action/Navigate.action", "action/NavigateSemantic.action", "msg/NavigationResult.msg"):
            self.assertFalse((INTERFACES / path).exists())
        self.assertEqual(list((ROOT / "navigation_gateway").rglob("*.py")), [])
        header = (ROOT / "ros2/bt_executor/include/bt_executor/ros_driver.hpp").read_text()
        self.assertIn("navigation_interfaces::action::NavigateToPose", header)
        self.assertNotIn("semantic_", header)
        for package in ("bt_executor", "bt_mock_servers"):
            manifest = ET.parse(ROOT / "ros2" / package / "package.xml").getroot()
            self.assertIn("navigation_interfaces", [v.text for v in manifest if v.tag in ("depend", "exec_depend")])

    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(p.relative_to(INTERFACES).as_posix() for p in INTERFACES.rglob("*") if p.suffix in (".msg", ".srv", ".action"))
        self.assertEqual(sorted(declarations), actual)
        self.assertEqual(len(declarations), len(set(declarations)))
        manifest = ET.parse(INTERFACES / "package.xml").getroot()
        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()