behavior-tree/ros2/bt_executor/tools/test_ros_backend.py

"""Transport lifecycle tests using controlled callback objects; no robot simulation."""
import sys
from pathlib import Path
import threading
from collections import deque
from types import SimpleNamespace as S
import unittest
import time
import json

sys.path.insert(0, str(Path(__file__).resolve().parents[3] / 'coordinator'))
from robot_bt_coordinator.ros_backend import RosBackend


class Future:
    def __init__(self, value): self.value = value
    def result(self): return self.value
    def add_done_callback(self, cb): self.callback = cb


class Handle:
    accepted = True
    def __init__(self): self.cancels = 0; self.future = Future(None)
    def cancel_goal_async(self): self.cancels += 1
    def get_result_async(self): return self.future


class BackendLifecycle(unittest.TestCase):
    def setUp(self):
        self.backend = RosBackend.__new__(RosBackend)
        self.backend._lock = threading.RLock()
        self.backend._events = deque()
        self.backend._feedback_timeout = 5
        self.backend.node = S(get_clock=lambda: S(now=lambda: S(nanoseconds=10_000_000_000)))
        self.key = ('task', 'run')
        self.rec = dict(task_id='task', run_id='run', sent_at=time.monotonic(), last_feedback=0,
                        sequence=0, handle=None, done=False, cancel_intent=False,
                        cancel_at=None, unknown_emitted=False, wire_uuid='wire')
        self.backend._runs = {self.key: self.rec}
        self.backend._planning = {}

    def result(self, native=4, business=0, stop=1, evidence='{"stop_confirmed":true}'):
        return Future(S(status=native, result=S(result=S(status=business, stop_state=stop,
            stopped_at=S(sec=10, nanosec=0), stop_evidence_ref='proof', error_code=''),
            completed_quantity=1, evidence_json=evidence)))

    def test_cancel_before_late_accept(self):
        self.backend.cancel(*self.key)
        handle = Handle()
        self.backend._execution_accepted(self.key, Future(handle))
        self.assertEqual(handle.cancels, 1)
        self.assertFalse(self.rec['done'])
        self.assertEqual(len(self.backend._events), 0)

    def test_planning_cancel_cancels_handle(self):
        handle = Handle()
        planning = dict(task_id='task', task_revision=1, planning_generation=1, done=False, handle=handle)
        self.backend._planning = {('task', 1, 1): planning}
        self.backend.cancel('task', '')
        self.assertTrue(planning['done'])
        self.assertEqual(handle.cancels, 1)
        self.assertFalse(self.backend._events)

    def test_native_business_mismatch_quarantines(self):
        self.backend._execution_result(self.key, self.result(native=5, business=0))
        event = self.backend._events.pop()
        self.assertEqual(event['status'], 'INTERVENTION_REQUIRED')
        self.assertFalse(event['stop_confirmed'])
        self.assertFalse(self.rec['done'])  # tracking survives unknown result

    def test_unknown_stop_does_not_become_success(self):
        self.backend._execution_result(self.key, self.result(stop=0))
        self.assertEqual(self.backend._events.pop()['status'], 'INTERVENTION_REQUIRED')

    def test_stale_stop_proof_does_not_release(self):
        future = self.result()
        future.value.result.result.stopped_at.sec = 1
        self.backend._execution_result(self.key, future)
        self.assertFalse(self.backend._events.pop()['stop_confirmed'])

    def test_future_stop_proof_does_not_release(self):
        future = self.result()
        future.value.result.result.stopped_at.sec = 11
        self.backend._execution_result(self.key, future)
        self.assertFalse(self.backend._events.pop()['stop_confirmed'])

    def test_cancel_ack_never_is_stop(self):
        self.rec['handle'] = Handle()
        self.backend.cancel(*self.key)
        self.assertEqual(len(self.backend._events), 0)
        self.assertFalse(self.rec['done'])

    def test_stale_and_duplicate_feedback_do_not_refresh_liveness(self):
        def feedback(sec, sequence, stage='Navigate'):
            self.backend._feedback(self.key, S(feedback=S(stamp=S(sec=sec, nanosec=0),
                sequence=sequence, stage=stage, status_json='{}')))
        feedback(1, 1)
        self.assertEqual(self.rec['last_feedback'], 0)
        feedback(10, 2)
        accepted = self.rec['last_feedback']
        feedback(10, 2)
        self.assertEqual(self.rec['last_feedback'], accepted)
        self.assertEqual(len(self.backend._events), 1)

    def test_late_terminal_after_unknown_still_emits_evidence(self):
        self.backend._unknown(self.rec, 'ACCEPTANCE_TIMEOUT')
        self.backend._execution_result(self.key, self.result(native=5, business=2))
        events = list(self.backend._events)
        self.assertEqual([e['status'] for e in events], ['INTERVENTION_REQUIRED', 'CANCELED'])
        self.assertTrue(events[-1]['stop_confirmed'])

    def test_duplicate_json_result_fails_closed(self):
        self.backend._execution_result(self.key, self.result(evidence='{"stop_confirmed":false,"stop_confirmed":true}'))
        self.assertEqual(self.backend._events.pop()['status'], 'INTERVENTION_REQUIRED')

    def test_failed_planning_preserves_archive_reference(self):
        key = ('task', 1, 1)
        self.backend._planning[key] = dict(task_id='task', task_revision=1,
            planning_generation=1, done=False)
        self.backend._plan_result(key, Future(S(status=4, result=S(status=2,
            error_code='SEMANTIC_MISMATCH', message='conflict', planning_record_ref='archive/42'))))
        self.assertEqual(self.backend._events.pop()['planning_record_ref'], 'archive/42')

    def test_native_planner_protocol_failure_preserves_archive_reference(self):
        for native in (5,6):
            with self.subTest(native=native):
                key=('task',1,native)
                self.backend._planning[key]=dict(task_id='task',task_revision=1,planning_generation=native,done=False)
                self.backend._plan_result(key,Future(S(status=native,result=S(planning_record_ref='archive/native-'+str(native)))))
                event=self.backend._events.pop()
                self.assertEqual(event['status'],'FAILED')
                self.assertEqual(event['error_code'],'PLANNER_PROTOCOL_ERROR')
                self.assertEqual(event['planning_record_ref'],'archive/native-'+str(native))

    def test_invalid_planner_json_retains_archive_for_diagnosis(self):
        key=('task',1,1)
        self.backend._planning[key]=dict(task_id='task',task_revision=1,planning_generation=1,done=False)
        self.backend._plan_result(key,Future(S(status=4,result=S(status=0,task_plan_json='{invalid',planning_record_ref='archive/raw'))))
        event=self.backend._events.pop()
        self.assertEqual(event['error_code'],'PLANNER_PROTOCOL_ERROR')
        self.assertEqual(event['planning_record_ref'],'archive/raw')

    def test_evicted_late_acceptance_is_canceled_without_recreating_tracking(self):
        self.backend._terminal_retention=1
        old_run=('old','run');old_plan=('old',1,1)
        self.backend._runs[old_run]=dict(done=True)
        self.backend._runs[('new','run')]=dict(done=True)
        self.backend._planning[old_plan]=dict(done=True)
        self.backend._planning[('new',1,1)]=dict(done=True)
        self.backend._trim_terminal()
        self.assertNotIn(old_run,self.backend._runs);self.assertNotIn(old_plan,self.backend._planning)
        execution_handle=Handle();planning_handle=Handle()
        self.backend._execution_accepted(old_run,Future(execution_handle))
        self.backend._plan_accepted(old_plan,Future(planning_handle))
        self.assertEqual(execution_handle.cancels,1);self.assertEqual(planning_handle.cancels,1)
        self.assertNotIn(old_run,self.backend._runs);self.assertNotIn(old_plan,self.backend._planning)
        self.assertFalse(self.backend._events)

    def test_retention_never_evicts_unresolved_executions_or_plans(self):
        self.backend._terminal_retention=2
        pending_plan=('pending',1,1)
        self.backend._planning[pending_plan]=dict(done=False)
        self.backend._unknown(self.rec,'FEEDBACK_TIMEOUT')
        for index in range(8):
            self.backend._runs[('finished',str(index))]=dict(done=True)
            self.backend._planning[('finished',1,index)]=dict(done=True)
        self.backend._trim_terminal()
        self.assertIs(self.backend._runs[self.key],self.rec)
        self.assertFalse(self.rec['done']);self.assertIn(pending_plan,self.backend._planning)
        self.assertEqual(len(self.backend._runs),3);self.assertEqual(len(self.backend._planning),3)

    def test_feedback_retains_snapshot(self):
        self.backend._feedback(self.key, S(feedback=S(stamp=S(sec=10, nanosec=0),
            sequence=1, stage='PICK', status_json='{"holding_state":"UNKNOWN"}')))
        self.assertEqual(self.backend._events.pop()['detail'], {'holding_state':'UNKNOWN'})

    def test_retention_keeps_uncertain_run_and_ignores_late_evicted_callbacks(self):
        self.backend._terminal_retention = 2
        for i in range(6):
            self.backend._runs[('old', str(i))] = dict(done=True)
        self.backend._trim_terminal()
        self.assertIn(self.key, self.backend._runs)
        self.assertEqual(len(self.backend._runs), 3)
        self.backend._feedback(('old', '0'), S())
        self.backend._execution_result(('old', '0'), Future(None))

    def test_recovery_missing_authorization_never_calls_service(self):
        self.backend.config = {}
        with self.assertRaisesRegex(ValueError, 'recovery'):
            self.backend.reconcile({}, {})

    def test_recovery_validates_physical_state_before_retiring_tracking(self):
        task = dict(task_id='task',run_id='run',task_revision=2,planning_generation=3,
                    execution_generation=4,plan={'plan_version':5})
        request = dict(evidence_ref='operator-proof',resolution='resume_task')
        self.backend.config = dict(recovery_token='x'*24,recovery_operator_id='operator')
        self.backend._ReconcileTask = S(Request=lambda:S(trace=S()))
        calls=[]
        class ReadyFuture(Future):
            def add_done_callback(self, cb): cb(self)
        state = dict(verified=True,stop_confirmed=False,holding_state='EMPTY',
                     run_id='run',evidence_ref='operator-proof',receipts=[])
        def call(message):
            calls.append(message)
            return ReadyFuture(S(accepted=True,state_json=json.dumps(state)))
        self.backend._recovery = S(service_is_ready=lambda:True,call_async=call)
        with self.assertRaises(ValueError): self.backend.reconcile(task,request)
        self.assertFalse(self.rec['done'])
        state['stop_confirmed']=True
        self.assertTrue(self.backend.reconcile(task,request)['verified'])
        self.assertTrue(self.rec['done'])
        self.assertEqual(calls[-1].trace.execution_generation,4)
        self.assertEqual(calls[-1].trace.plan_version,5)


if __name__ == '__main__':
    unittest.main()