behavior-tree/tests/test_mock_runtime.py

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.yaw_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.SUCCEEDED if name == "navigate" else 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],
            "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.current_pose_valid and nav.error_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], "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, 2])
        self.assertEqual([item.blocked for item in recovery.feedback], [False, False, False, True, False])
        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.TIMEOUT))
        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_navigation_status_mapping_and_readiness_reasons(self):
        cases = [("normal", 0, "succeeded", ""), ("canceled", 1, "canceled", "MOCK_TERMINATED"),
                 ("timeout", 2, "aborted", "MOCK_TERMINATED"), ("blocked", 3, "aborted", "BLOCKED"),
                 ("failed", 5, "aborted", "MOCK_TERMINATED")]
        cases += [("not_ready", 4, "aborted", code) for code in
                  ("INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED",
                   "ROBOT_EMERGENCY_STOP", "ROBOT_PROTECTIVE_STOP", "ROBOT_MOTION_NOT_ALLOWED")]
        for kind, status, native, code in cases:
            self.node.counts["navigate"] = 0
            fixture = {"kind": "normal" if kind == "canceled" else kind, "duration_seconds": 0}
            if kind == "not_ready": fixture["error_code"] = code
            self.module.parse_scenarios(json.dumps({"navigate": fixture}))
            handle, result = self.execute("navigate", fixture, cancel=kind == "canceled")
            with self.subTest(kind=kind, code=code):
                self.assertEqual((result.result.status, handle.native), (status, native))
                self.assertEqual(result.result.error_code, code)
                self.assertEqual(result.final_pose_valid, status == 0)
                if result.final_pose_valid:
                    self.assertEqual((result.final_position_error, result.final_yaw_error), (0.0, 0.0))
                self.assertFalse(hasattr(result, "errors_valid"))
        self.node.counts["navigate"] = 0
        _, result = self.execute("navigate", {"kind": "stop_unknown", "duration_seconds": 0})
        self.assertEqual((result.result.status, result.result.stop_state), (5, 0))
        self.assertEqual((result.result.stopped_at.sec, result.result.stopped_at.nanosec,
                          result.result.stop_evidence_ref), (0, 0, ""))

    def test_navigation_goal_requires_map_and_finite_nonzero_quaternion(self):
        mutations = (
            lambda g: setattr(g.target_pose.header, "frame_id", "odom"),
            lambda g: setattr(g.target_pose.pose.orientation, "w", 0.0),
            lambda g: setattr(g.target_pose.pose.orientation, "w", float("nan")),
            lambda g: setattr(g.target_pose.pose.position, "x", float("nan")),
            lambda g: setattr(g, "yaw_tolerance", float("nan")),
        )
        for mutate in mutations:
            self.node.inflight, self.node.motion_reserved = 0, False
            goal = self.goal("navigate"); mutate(goal)
            with self.subTest(mutate=mutate):
                self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.REJECT)

    def test_navigation_accepts_large_yaw_tolerance_and_normalizes_quaternion(self):
        for magnitude in (2.0, 1e308, 1e-308):
            goal = self.goal("navigate")
            goal.yaw_tolerance = 4.0
            goal.target_pose.pose.orientation.w = magnitude
            self.node.scenarios["navigate"] = [{"kind": "normal", "duration_seconds": 0.1}]
            with self.subTest(magnitude=magnitude):
                self.assertEqual(self.node._goal("navigate", goal), self.module.GoalResponse.ACCEPT)
                handle = GoalHandle(goal); self.node._accepted("navigate", handle)
                result = self.node._execute("navigate", handle)
                self.assertEqual(result.result.status, 0)
                self.assertTrue(result.final_pose_valid)
                self.assertEqual(result.final_pose.pose.orientation.w, 1.0)
                self.assertTrue(handle.feedback)
                self.assertTrue(all(item.current_pose.pose.orientation.w == 1.0 for item in handle.feedback))
                self.assertEqual(goal.target_pose.pose.orientation.w, magnitude)

    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.SUCCEEDED)

    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()