From f9d8feb6f01dea9335477bca3ce58ac324e79ac0 Mon Sep 17 00:00:00 2001 From: wangfeiyu Date: Sun, 20 Sep 2026 13:36:48 +0800 Subject: [PATCH] fix: harden task recovery and DR contract handling --- coordinator/robot_bt_coordinator/cli.py | 5 +- coordinator/robot_bt_coordinator/http_api.py | 15 +- coordinator/robot_bt_coordinator/intent.py | 49 +++ coordinator/robot_bt_coordinator/replay.py | 7 +- .../robot_bt_coordinator/ros_backend.py | 101 +++++- coordinator/robot_bt_coordinator/service.py | 88 ++++- coordinator/robot_bt_coordinator/store.py | 13 +- core/CMakeLists.txt | 6 +- core/include/robot_bt/core.hpp | 27 +- core/src/core.cpp | 73 ++-- core/src/workflow.cpp | 54 ++- core/tests/dispatch_audit_test.cpp | 11 + core/tests/evidence_regression_test.cpp | 31 ++ core/tests/readiness_regression_test.cpp | 23 +- core/tests/retention_regression_test.cpp | 8 + core/tests/semantic_failure_test.cpp | 12 + core/tests/settlement_test.cpp | 6 +- core/tests/state_alignment_test.cpp | 16 + robobrain/robot_robobrain/demo_backend.py | 2 +- robobrain/robot_robobrain/intent.py | 34 +- robobrain/robot_robobrain/model_adapter.py | 20 +- robobrain/robot_robobrain/service.py | 15 +- ros2/bt_executor/CMakeLists.txt | 2 +- .../include/bt_executor/recovery_policy.hpp | 18 + .../include/bt_executor/ros_driver.hpp | 38 +- ros2/bt_executor/launch/executor.launch.py | 4 +- ros2/bt_executor/package.xml | 2 +- ros2/bt_executor/src/executor_node.cpp | 304 ++++++++++++++-- ros2/bt_executor/src/ros_driver.cpp | 33 +- ros2/bt_executor/tools/build_humble.sh | 3 + .../tools/test_recovery_policy.cpp | 16 + ros2/bt_executor/tools/test_ros_backend.py | 100 ++++++ ros2/bt_mock_servers/package.xml | 2 +- ros2/bt_skill_interfaces/CMakeLists.txt | 1 + ros2/bt_skill_interfaces/package.xml | 2 +- .../bt_skill_interfaces/srv/ReconcileTask.srv | 11 + ros2/robobrain_services/package.xml | 2 +- .../robobrain_services/nodes.py | 30 +- .../helpers/native_coordinator_regression.py | 203 +++++++++++ tests/helpers/native_workflow_regression.py | 326 ++++++++++++++++++ tests/test_coordinator_recovery.py | 190 ++++++++++ tests/test_http_api.py | 53 +++ tests/test_robobrain_consistency.py | 158 +++++++++ tests/test_tooling.py | 25 ++ tools/build_portable.sh | 5 +- tools/coverage_report.py | 23 +- tools/http_concurrency.py | 71 ++++ tools/test_all.sh | 1 + tools/test_recovery_policy.sh | 9 + 49 files changed, 2083 insertions(+), 165 deletions(-) create mode 100644 coordinator/robot_bt_coordinator/intent.py create mode 100644 core/tests/dispatch_audit_test.cpp create mode 100644 core/tests/evidence_regression_test.cpp create mode 100644 core/tests/retention_regression_test.cpp create mode 100644 core/tests/semantic_failure_test.cpp create mode 100644 core/tests/state_alignment_test.cpp create mode 100644 ros2/bt_executor/include/bt_executor/recovery_policy.hpp create mode 100644 ros2/bt_executor/tools/test_recovery_policy.cpp create mode 100644 ros2/bt_skill_interfaces/srv/ReconcileTask.srv create mode 100644 tests/helpers/native_coordinator_regression.py create mode 100644 tests/helpers/native_workflow_regression.py create mode 100644 tests/test_coordinator_recovery.py create mode 100644 tests/test_robobrain_consistency.py create mode 100644 tests/test_tooling.py create mode 100644 tools/http_concurrency.py create mode 100644 tools/test_recovery_policy.sh diff --git a/coordinator/robot_bt_coordinator/cli.py b/coordinator/robot_bt_coordinator/cli.py index e9ea466..b5083bd 100644 --- a/coordinator/robot_bt_coordinator/cli.py +++ b/coordinator/robot_bt_coordinator/cli.py @@ -27,7 +27,10 @@ def main(): backend=BrainDemoBackend(args.demo_executable,state/'simulator',site) else: from .ros_backend import RosBackend - backend=RosBackend(namespace=args.ros_namespace,config={'planning_context':site,'dense_progress_enabled':args.dense_progress}) + backend=RosBackend(namespace=args.ros_namespace,config={'planning_context':site, + 'dense_progress_enabled':args.dense_progress, + 'recovery_token':os.environ.get('ROBOT_BT_RECOVERY_TOKEN',''), + 'recovery_operator_id':os.environ.get('ROBOT_BT_RECOVERY_OPERATOR','')}) if args.backend=='mock':backend.route=site.get('execution_route','OBJECT_TABLE') coordinator=Coordinator(str(state/'tasks.sqlite3'),backend,{args.robot_id},site) server=make_server(coordinator,args.host,args.port,token,os.environ.get('ROBOT_BT_OPERATOR_TOKEN','')) diff --git a/coordinator/robot_bt_coordinator/http_api.py b/coordinator/robot_bt_coordinator/http_api.py index d1eed17..074381e 100644 --- a/coordinator/robot_bt_coordinator/http_api.py +++ b/coordinator/robot_bt_coordinator/http_api.py @@ -8,6 +8,12 @@ from .plan import strict_json MAX_BODY=65536 + +class TaskHTTPServer(ThreadingHTTPServer): + # Accept a burst of task submissions without the default five-slot listen + # queue forcing clients into a TCP retransmission before admission. + request_queue_size=64 + def make_server(coordinator,host,port,api_token,operator_token=''): if not api_token:raise ValueError('API token must be explicitly configured') class Handler(BaseHTTPRequestHandler): @@ -60,7 +66,12 @@ def make_server(coordinator,host,port,api_token,operator_token=''): if data:raise ApiError('INVALID_CONTROL','control body must be empty') return 202,coordinator.control(tid,action) if action=='clarifications':return 202,coordinator.clarify(tid,data) - if action=='interventions':return 202,coordinator.intervene(tid,data) + if action=='interventions': + try:return 202,coordinator.intervene(tid,data) + except TimeoutError: + raise ApiError('RECONCILIATION_TIMEOUT','recovery outcome is unknown; retry with the same evidence reference',504) from None + except RuntimeError: + raise ApiError('RECONCILIATION_UNAVAILABLE','recovery service is unavailable or rejected the request; task state remains queryable',503) from None raise ApiError('NOT_FOUND','endpoint not found',404) def handle_request(self): try:status,payload=self.dispatch();self.reply(status,payload) @@ -70,5 +81,5 @@ def make_server(coordinator,host,port,api_token,operator_token=''): self.reply(500,{'error_code':'INTERNAL_ERROR','message':'request failed; task state remains queryable'}) do_GET=handle_request do_POST=handle_request - server=ThreadingHTTPServer((host,port),Handler);server.daemon_threads=True + server=TaskHTTPServer((host,port),Handler);server.daemon_threads=True return server diff --git a/coordinator/robot_bt_coordinator/intent.py b/coordinator/robot_bt_coordinator/intent.py new file mode 100644 index 0000000..db38d06 --- /dev/null +++ b/coordinator/robot_bt_coordinator/intent.py @@ -0,0 +1,49 @@ +"""Conservative independent intent gate for instructions without confirmed slots. +Only registered aliases and explicit quantities are accepted. Other language asks +for structured clarification; this parser is not claimed to cover arbitrary NLP. +""" +import re +COUNTS={'一':1,'二':2,'两':2,'三':3,'四':4,'五':5,'六':6,'七':7,'八':8,'九':9,'十':10,'one':1,'two':2,'three':3} +def matches(instruction,registry,aliases): + candidates=[] + for key in registry: + for term in [key]+list(aliases.get(key,[])): + if not term:continue + for m in re.finditer(re.escape(term),instruction):candidates.append((m.start(),m.end(),key)) + chosen=[] + for item in sorted(candidates,key=lambda x:(-(x[1]-x[0]),x[0])): + if not any(item[0] 1: + constraints = strict_json(goal.constraints_json) + constraints['clarification_confirmed'] = True + goal.constraints_json = canonical(constraints) self._duration(goal.timeout, self.config.get('planning_timeout', 25)) future = self._planner.send_goal_async(goal) future.add_done_callback(lambda completed: self._plan_accepted(key, completed)) def _plan_accepted(self, key, future): with self._lock: - rec = self._planning[key] + rec = self._planning.get(key) + if rec is None: + try: + handle = future.result() + if handle and handle.accepted: + handle.cancel_goal_async() + except Exception: + pass + return try: handle = future.result() if not handle or not handle.accepted: @@ -147,12 +165,14 @@ class RosBackend: def _plan_result(self, key, future): with self._lock: - rec = self._planning[key] - if rec['done']: + rec = self._planning.get(key) + if rec is None or rec['done']: return rec['done'] = True + record_ref = '' try: wrapped = future.result() + record_ref = getattr(wrapped.result, 'planning_record_ref', '') if wrapped.status != 4: raise ValueError('planning native result was not SUCCEEDED') result = wrapped.result @@ -168,11 +188,12 @@ class RosBackend: raise ValueError('clarification result has no structured questions') self._emit(self._planning_event(rec, 'NEEDS_CLARIFICATION', questions=questions, planning_record_ref=getattr(result,'planning_record_ref',''))) elif result.status == 2: - self._emit(self._planning_event(rec, 'FAILED', error_code=result.error_code, error=result.message)) + self._emit(self._planning_event(rec, 'FAILED', error_code=result.error_code, error=result.message, + planning_record_ref=getattr(result, 'planning_record_ref', ''))) else: raise ValueError('unknown planning status') except Exception as exc: - self._emit(self._planning_event(rec, 'FAILED', error_code='PLANNER_PROTOCOL_ERROR', error=str(exc))) + self._emit(self._planning_event(rec, 'FAILED', error_code='PLANNER_PROTOCOL_ERROR', error=str(exc), planning_record_ref=record_ref)) def start_execution(self, task_dict): with self._lock: @@ -244,7 +265,15 @@ class RosBackend: def _execution_accepted(self, key, future): with self._lock: - rec = self._runs[key] + rec = self._runs.get(key) + if rec is None: + try: + handle = future.result() + if handle and handle.accepted: + handle.cancel_goal_async() + except Exception: + pass + return try: handle = future.result() if not handle or not handle.accepted: @@ -261,8 +290,8 @@ class RosBackend: def _feedback(self, key, wrapped): with self._lock: - rec = self._runs[key] - if rec['done']: + rec = self._runs.get(key) + if rec is None or rec['done']: return msg = wrapped.feedback at = msg.stamp.sec * 1_000_000_000 + msg.stamp.nanosec @@ -278,12 +307,12 @@ class RosBackend: return rec['sequence'], rec['last_feedback'] = int(msg.sequence), time.monotonic() self._emit(dict(type='progress', task_id=key[0], run_id=key[1], - sequence=int(msg.sequence), stage=msg.stage)) + sequence=int(msg.sequence), stage=msg.stage, detail=status)) def _execution_result(self, key, future): with self._lock: - rec = self._runs[key] - if rec['done']: + rec = self._runs.get(key) + if rec is None or rec['done']: return try: wrapped = future.result() @@ -349,6 +378,54 @@ class RosBackend: pause = cancel + def _trim_terminal(self): + limit = getattr(self, '_terminal_retention', 256) + for records in (self._planning, self._runs): + terminal = [key for key, rec in records.items() if rec['done']] + for key in terminal[:-limit]: + del records[key] + + def reconcile(self, task, request): + token = self.config.get('recovery_token', '') + operator = self.config.get('recovery_operator_id', '') + if not isinstance(token, str) or len(token) < 16 or not isinstance(operator, str) or not operator: + raise ValueError('recovery credentials are not configured') + timeout = float(self.config.get('recovery_timeout', 8)) + if not math.isfinite(timeout) or not 0 < timeout <= 30: + raise ValueError('invalid recovery timeout') + if not self._recovery.service_is_ready(): + raise RuntimeError('recovery service unavailable') + message = self._ReconcileTask.Request() + message.trace.task_id = task['task_id'] + message.trace.run_id = task['run_id'] + message.trace.subtask_id = 'execute_task' + message.trace.attempt = 1 + message.trace.task_revision = task['task_revision'] + message.trace.plan_version = task['plan']['plan_version'] + message.trace.execution_generation = task.get('execution_generation', task['planning_generation']) + message.operator_id, message.authorization = operator, token + message.evidence_ref = request['evidence_ref'] + message.resolution = request['resolution'] + future = self._recovery.call_async(message) + complete = threading.Event() + future.add_done_callback(lambda _: complete.set()) + if not complete.wait(timeout): + future.cancel() + raise TimeoutError('recovery outcome unknown; retry with same evidence reference') + response = future.result() + if response is None or not response.accepted: + raise RuntimeError('recovery rejected: ' + (response.error_code if response else 'NO_RESPONSE')) + result = strict_json(response.state_json) + if (not isinstance(result, dict) or result.get('verified') is not True or + result.get('stop_confirmed') is not True or result.get('holding_state') != 'EMPTY' or + result.get('run_id') != task['run_id'] or result.get('evidence_ref') != request['evidence_ref']): + raise ValueError('recovery response identity or verification mismatch') + with self._lock: + rec = self._runs.get((task['task_id'], task['run_id'])) + if rec is not None: + rec['done'] = True + return result + def poll(self): with self._lock: now = time.monotonic() @@ -369,6 +446,7 @@ class RosBackend: self._unknown(rec, 'STOP_TIMEOUT') events = list(self._events) self._events.clear() + self._trim_terminal() return events def close(self): @@ -391,6 +469,7 @@ class RosBackend: self._executor.remove_node(self.node) self._planner.destroy() self._execution.destroy() + self.node.destroy_client(self._recovery) if self._own_node: self.node.destroy_node() self._context.shutdown() diff --git a/coordinator/robot_bt_coordinator/service.py b/coordinator/robot_bt_coordinator/service.py index 2bac490..f59ac06 100644 --- a/coordinator/robot_bt_coordinator/service.py +++ b/coordinator/robot_bt_coordinator/service.py @@ -9,6 +9,7 @@ from .errors import ApiError from .plan import canonical, text, validate_known, validate_plan from .store import Store from .plan_v2 import instances, item_plan +from .intent import conflicts_with_instruction TERMINAL = {'SUCCEEDED','FAILED','CANCELED','EXPIRED'} EXECUTION = {'READY','EXECUTING','PAUSING','CANCELING','INTERVENTION_REQUIRED'} @@ -30,7 +31,7 @@ class Coordinator: self.clock=clock;self.queue_timeout=queue_timeout;self.closed=False self.steady=steady;self.planning_timeout=planning_timeout;self._planning_started={} with self.lock,self.store.db: - for t in self.store.all(): + for t in self.store.active(): if t['status'] in EXECUTION or (t['status']=='PAUSED' and t.get('motion_dispatched')): self._set(t,'INTERVENTION_REQUIRED','RESTART_RECONCILIATION_REQUIRED') elif t['status']=='PLANNING': @@ -45,9 +46,19 @@ class Coordinator: if t is None:raise ApiError('NOT_FOUND','task not found',404) return t def _set(self,t,status,error='',kind='state'): + if status!='PLANNING':self._planning_started.pop(t['task_id'],None) t['status']=status;t['error_code']=error;t['updated_at']=self.clock();t['status_version']+=1 self.store.put(t) - self.store.event(t,kind,{'status':status,'status_version':t['status_version'],'error_code':error},self.clock()) + event={'status':status,'status_version':t['status_version'],'error_code':error} + if kind=='progress':event.update(run_id=t['run_id'],stage=t['stage'],sequence=t['last_sequence'],detail=t.get('detail','')) + self.store.event(t,kind,event,self.clock()) + def _intent_question(self,t): + t['question']={'question_id':str(uuid.uuid4()),'task_revision':t['task_revision'],'questions':['The instruction and structured item, quantity, source or destination disagree. Confirm the intended values.']} + self._set(t,'NEEDS_CLARIFICATION','INTENT_MISMATCH') + def _runtime_question(self,t,questions): + t['question']={'question_id':str(uuid.uuid4()),'task_revision':t['task_revision'],'source_run_id':t['run_id'],'questions':questions} + t['motion_dispatched']=False;t['stop_confirmed']=True + self._set(t,'NEEDS_CLARIFICATION','EXECUTION_CLARIFICATION_REQUIRED') def get(self,tid): with self.lock:return self._task(tid) def events(self,tid,after=0,limit=100): @@ -74,8 +85,11 @@ class Coordinator: return dict(json.loads(existing[1]),deduplicated=True) now=self.clock() t=dict(task_id=str(uuid.uuid4()),robot_id=body['robot_id'],request=body,task_revision=1,planning_generation=0,planning_attempts=0,plan_version=0,run_id='',status='RECEIVED',status_version=0,accepted_at=now,updated_at=now,stage='',error_code='',completed_quantity=0,plan=None,context=None,question=None,motion_dispatched=False,stop_confirmed=True,last_sequence=0) + t['clarification_confirmed']=False self.store.db.execute('INSERT INTO tasks(task_id,robot_id,request_id,request_hash,data) VALUES(?,?,?,?,?)',(t['task_id'],t['robot_id'],body['client_request_id'],fingerprint,canonical(t))) - self._set(t,'QUEUED');return dict(t,deduplicated=False) + if conflicts_with_instruction(body['instruction'],info,self.site):self._intent_question(t) + else:self._set(t,'QUEUED') + return dict(t,deduplicated=False) def control(self,tid,action): if action not in {'cancel','pause','resume'}:raise ApiError('INVALID_CONTROL','unsupported control') with self.lock: @@ -83,10 +97,10 @@ class Coordinator: if t['status'] in TERMINAL:return t if action=='resume': if t['status']!='PAUSED':raise ApiError('INVALID_STATE','task is not paused',409) + if t['motion_dispatched']: + raise ApiError('RECONCILIATION_REQUIRED','operator reconciliation with fresh physical evidence is required',409) with self.store.db: - if t['motion_dispatched']: - self._set(t,'INTERVENTION_REQUIRED','RESUME_REQUIRES_PHYSICAL_RECHECK') - else:self._set(t,'QUEUED') + self._set(t,'QUEUED') return t if t['status']=='INTERVENTION_REQUIRED': # A second cancel is useful, but never removes the physical quarantine. @@ -116,26 +130,64 @@ class Coordinator: if t['status']!='NEEDS_CLARIFICATION' or not t['question'] or answer['question_id']!=t['question']['question_id'] or answer['task_revision']!=t['task_revision']: raise ApiError('STALE_ANSWER','answer does not match current question/revision',409) info=validate_known(answer['known_info']) + if not info:raise ApiError('INVALID_ANSWER','explicit confirmed information is required') if 'items' in info: previous=t['request']['known_info'];t['request']['known_info']={k:v for k,v in previous.items() if k=='destination'} elif 'items' in t['request']['known_info'] and set(info)-{'destination'}:raise ApiError('INVALID_ANSWER','use items representation for multi-item clarification') t['request']['known_info'].update(info);t['task_revision']+=1;t['planning_generation']+=1;t['planning_attempts']=0;t['question']=None + t['clarification_confirmed']=True self._set(t,'QUEUED');return t def intervene(self,tid,data): """Reconciliation delegates evidence validation to a trusted physical backend.""" - if not isinstance(data,dict) or set(data)!={'run_id','evidence_ref','resolution'} or data['resolution']!='cancel_task': - raise ApiError('INVALID_RECONCILIATION','run_id, evidence_ref, resolution=cancel_task required') + if not isinstance(data,dict) or set(data)!={'run_id','evidence_ref','resolution'} or data['resolution'] not in {'cancel_task','resume_task','replan_task'}: + raise ApiError('INVALID_RECONCILIATION','run_id, evidence_ref, resolution=cancel_task/resume_task/replan_task required') text(data['evidence_ref'],'evidence_ref',512) with self.lock: t=self._task(tid) - if t['status']!='INTERVENTION_REQUIRED' or data['run_id']!=t['run_id']: + if t['status'] not in {'INTERVENTION_REQUIRED','PAUSED'} or not t['motion_dispatched'] or data['run_id']!=t['run_id']: raise ApiError('STALE_RECONCILIATION','task/run does not require this reconciliation',409) if not hasattr(self.backend,'reconcile'):raise ApiError('RECONCILIATION_UNAVAILABLE','trusted physical verification backend is required',503) verified=self.backend.reconcile(t,data) - if not isinstance(verified,dict) or verified.get('stop_confirmed') is not True or verified.get('holding')!='EMPTY' or verified.get('run_id')!=t['run_id'] or verified.get('evidence_ref')!=data['evidence_ref']: + if not isinstance(verified,dict) or verified.get('verified') is not True or verified.get('stop_confirmed') is not True or verified.get('holding_state')!='EMPTY' or verified.get('run_id')!=t['run_id'] or verified.get('evidence_ref')!=data['evidence_ref']: raise ApiError('RECONCILIATION_REJECTED','matching stop and empty-hand evidence required',409) + receipts=verified.get('receipts') + if not isinstance(receipts,list) or len(receipts)>20: + raise ApiError('RECONCILIATION_REJECTED','bounded receipt list required',409) + approved=instances(t['plan']);pending={} + existing={row['item_index']:json.loads(row['evidence']) for row in self.store.db.execute('SELECT item_index,evidence FROM deliveries WHERE task_id=?',(tid,))} + for receipt in receipts: + if not isinstance(receipt,dict):raise ApiError('RECONCILIATION_REJECTED','invalid receipt',409) + index=receipt.get('item_index');quantity=receipt.get('completed_quantity',1);evidence=receipt.get('evidence') + if receipt.get('task_id')!=tid or type(index) is not int or not 0<=indext.get('active_item_index',0) or type(quantity) is not int or quantity!=1: + raise ApiError('RECONCILIATION_REJECTED','receipt task/item/quantity mismatch',409) + expected=approved[index];destination=self.site['destinations'][expected['destination']]['region_ref'] + if not isinstance(evidence,dict) or any(evidence.get(k) is not True for k in ('passed','empty_hand','in_destination','valid')) or not isinstance(evidence.get('evidence_id'),str) or not evidence['evidence_id'] or evidence.get('target_ref')!=expected['target_name'] or evidence.get('destination_ref')!=destination: + raise ApiError('RECONCILIATION_REJECTED','receipt delivery proof invalid',409) + previous=pending.get(index,existing.get(index)) + if previous is not None and any(previous.get(k)!=evidence.get(k) for k in ('evidence_id','target_ref','destination_ref','passed','empty_hand','in_destination','valid')): + raise ApiError('RECONCILIATION_REJECTED','receipt conflicts with committed delivery',409) + pending[index]=evidence + credited=set(existing)|set(pending) + if credited!=set(range(len(credited))):raise ApiError('RECONCILIATION_REJECTED','delivery ledger has an item gap',409) + remaining=next((i for i in range(len(approved)) if i not in credited),None) + if data['resolution']=='resume_task' and remaining is not None and t.get('active_item_index',0) not in credited and verified.get('safe_to_retry') is not True: + raise ApiError('RECONCILIATION_REJECTED','cannot replay item without proof no manipulation was dispatched',409) + if data['resolution']=='replan_task' and (credited or verified.get('safe_to_retry') is not True): + raise ApiError('RECONCILIATION_REJECTED','replanning requires zero deliveries and proof no manipulation was dispatched',409) with self.store.db: - t['stop_confirmed']=True;self._set(t,'CANCELED','RECONCILED_BY_OPERATOR') + for index,evidence in pending.items(): + if index not in existing: + self.store.db.execute('INSERT INTO deliveries(task_id,item_index,evidence,created_at) VALUES(?,?,?,?)',(tid,index,canonical(evidence),self.clock())) + self.store.event(t,'delivery_committed',{'item_index':index,'evidence':evidence},self.clock()) + t['completed_quantity']=len(credited);t['stop_confirmed']=True + if pending:t['delivery_evidence']=pending[max(pending)] + self.store.event(t,'reconciliation',{'run_id':t['run_id'],'resolution':data['resolution'],'evidence_ref':data['evidence_ref'],'credited_items':sorted(credited)},self.clock()) + if data['resolution']=='cancel_task':self._set(t,'CANCELED','RECONCILED_BY_OPERATOR') + elif data['resolution']=='replan_task':self._runtime_question(t,['Confirm the item, quantity, source and destination before a new plan is requested.']) + elif remaining is None:self._set(t,'SUCCEEDED') + else: + t['active_item_index']=remaining;t['execution_generation']=t.get('execution_generation',t['planning_generation'])+1 + self._dispatch_item(t) return t def tick(self): with self.lock: @@ -144,10 +196,10 @@ class Coordinator: t=self.store.get(e['task_id']) if not t:continue with self.store.db:self._event(t,e) - for waiting in self.store.all(): + for waiting in self.store.active(): if waiting['status']=='PLANNING' and self.steady()-self._planning_started.get(waiting['task_id'],self.steady())>=self.planning_timeout: with self.store.db:self._planning_failure(waiting,'PLANNING_TIMEOUT') - all_tasks=self.store.all() + all_tasks=self.store.active() for robot in sorted(self.robots): tasks=[t for t in all_tasks if t['robot_id']==robot] if any(t['status'] not in TERMINAL|{'QUEUED'} for t in tasks):continue @@ -186,6 +238,8 @@ class Coordinator: if p['subtasks'][0]['skill']=='ASK_USER': t['question']={'question_id':str(uuid.uuid4()),'task_revision':t['task_revision'],'questions':[p['subtasks'][0]['arguments']['question']]} self._set(t,'NEEDS_CLARIFICATION');return + if conflicts_with_instruction(t['request']['instruction'],p['slots'],self.site,clarification_confirmed=t.get('clarification_confirmed') is True): + self._intent_question(t);return if self.site.get('execution_route') and p['schema_version']!=2:raise ApiError('SCHEMA_MISMATCH','v2 deployment requires v2 planner') slots=p['slots'];known=t['request']['known_info'];compare=slots if p['schema_version']==2 and 'items' not in known and known: @@ -210,6 +264,7 @@ class Coordinator: seq=e.get('sequence') if type(seq) is not int or seq<=t['last_sequence'] or not isinstance(e.get('stage'),str):return t['last_sequence']=seq;t['stage']=e['stage'][:100] + t['detail']=e.get('detail','')[:2000] if isinstance(e.get('detail',''),str) else '' self._set(t,t['status'],t['error_code'],'progress');return self.store.event(t,'execution_result',{'payload':e},self.clock()) ev=e.get('evidence',{});quantity=e.get('completed_quantity',0) @@ -228,6 +283,13 @@ class Coordinator: if e.get('stop_confirmed') is not True: self._set(t,'INTERVENTION_REQUIRED','STOP_UNKNOWN');return t['stop_confirmed']=True + if isinstance(ev,dict) and ev.get('needs_clarification') is True and t['status']=='EXECUTING': + questions=ev.get('questions') + valid_questions=isinstance(questions,list) and 1<=len(questions)<=8 and all(isinstance(q,str) and q.strip() and len(q)<=500 for q in questions) + safe=all(ev.get(k) is True for k in ('safe_to_retry','empty_hand','valid','safe_to_release')) + if quantity!=0 or t['completed_quantity']!=0 or not safe or not valid_questions: + self._set(t,'INTERVENTION_REQUIRED','CLARIFICATION_REQUIRES_PHYSICAL_RECHECK');return + self._runtime_question(t,questions);return if e.get('status')=='SUCCEEDED': if quantity!=1: self._set(t,'INTERVENTION_REQUIRED','DELIVERY_EVIDENCE_INVALID');return diff --git a/coordinator/robot_bt_coordinator/store.py b/coordinator/robot_bt_coordinator/store.py index b1ffacc..b97bb1e 100644 --- a/coordinator/robot_bt_coordinator/store.py +++ b/coordinator/robot_bt_coordinator/store.py @@ -24,13 +24,24 @@ class Store: CREATE INDEX IF NOT EXISTS events_task_cursor ON events(task_id,event_id); CREATE TABLE IF NOT EXISTS deliveries(task_id TEXT NOT NULL REFERENCES tasks(task_id),item_index INTEGER NOT NULL,evidence TEXT NOT NULL,created_at REAL NOT NULL,PRIMARY KEY(task_id,item_index)); ''') + # Migrate old databases once; terminal history remains queryable without + # being decoded on every scheduler tick. + columns={row[1] for row in self.db.execute('PRAGMA table_info(tasks)')} + with self.db: + if 'status' not in columns: + self.db.execute("ALTER TABLE tasks ADD COLUMN status TEXT NOT NULL DEFAULT 'RECEIVED'") + for row in self.db.execute('SELECT task_id,data FROM tasks').fetchall(): + self.db.execute('UPDATE tasks SET status=? WHERE task_id=?',(json.loads(row['data'])['status'],row['task_id'])) + self.db.execute("CREATE INDEX IF NOT EXISTS tasks_active_seq ON tasks(seq) WHERE status NOT IN ('SUCCEEDED','FAILED','CANCELED','EXPIRED')") def get(self,tid): r=self.db.execute('SELECT data FROM tasks WHERE task_id=?',(tid,)).fetchone() return json.loads(r[0]) if r else None def all(self): return [json.loads(r[0]) for r in self.db.execute('SELECT data FROM tasks ORDER BY seq')] + def active(self): + return [json.loads(r[0]) for r in self.db.execute("SELECT data FROM tasks WHERE status NOT IN ('SUCCEEDED','FAILED','CANCELED','EXPIRED') ORDER BY seq")] def put(self,t): - self.db.execute('UPDATE tasks SET data=? WHERE task_id=?',(canonical(t),t['task_id'])) + self.db.execute('UPDATE tasks SET data=?,status=? WHERE task_id=?',(canonical(t),t['status'],t['task_id'])) def event(self,t,kind,data,now): self.db.execute('INSERT INTO events(task_id,kind,created_at,data) VALUES(?,?,?,?)',(t['task_id'],kind,now,canonical(data))) def events(self,tid,after,limit): diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index aab3f75..a535eac 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -13,8 +13,10 @@ target_link_libraries(robot_bt_demo PRIVATE robot_bt_sim) target_compile_options(robot_bt_demo PRIVATE -Wall -Wextra -Werror) include(CTest) if(BUILD_TESTING) - foreach(test_name core_test workflow_test journal_failure_test preflight_test settlement_test proof_regression_test readiness_regression_test scenario_test lifecycle_test) - add_executable(${test_name} tests/${test_name}.cpp) + file(GLOB core_test_sources CONFIGURE_DEPENDS "tests/*_test.cpp") + foreach(test_source IN LISTS core_test_sources) + get_filename_component(test_name "${test_source}" NAME_WE) + add_executable(${test_name} "${test_source}") target_link_libraries(${test_name} PRIVATE robot_bt_core) target_compile_options(${test_name} PRIVATE -Wall -Wextra -Werror -UNDEBUG) add_test(NAME ${test_name} COMMAND ${test_name}) diff --git a/core/include/robot_bt/core.hpp b/core/include/robot_bt/core.hpp index d988ba4..8853b3e 100644 --- a/core/include/robot_bt/core.hpp +++ b/core/include/robot_bt/core.hpp @@ -1,5 +1,6 @@ #pragma once #include +#include #include #include #include @@ -50,7 +51,7 @@ struct SkillResponse { Holding holding{Holding::UNKNOWN}; bool verified{false}, in_destination{false}, base_stopped{false}; }; -struct ExecutionResult { ResultCode code{ResultCode::FAILED}; StopState stop{StopState::UNKNOWN}; SkillResponse response; std::string detail; }; +struct ExecutionResult { ResultCode code{ResultCode::FAILED}; StopState stop{StopState::UNKNOWN}; SkillResponse response; std::string detail; std::string error_code{}, execution_record_ref{}, wire_result_type{}, wire_result_snapshot{}; }; struct GoalRequest { std::string goal_id, robot_id; Trace trace; @@ -63,6 +64,7 @@ struct GoalRequest { std::uint64_t geometry_epoch{0}; RosTime capture_after{0}; double position_tolerance_m{0.05}, orientation_tolerance_rad{0.1}; + std::string wire_request_type{}, wire_request_snapshot{}; }; enum class EventKind { ACCEPTED, REJECTED, FEEDBACK, CANCEL_ACK, RESULT }; struct GoalEvent { @@ -72,6 +74,7 @@ struct GoalEvent { std::uint64_t sequence{0}; NativeStatus native_status{NativeStatus::UNKNOWN}; ExecutionResult result; + std::string feedback_snapshot{}; }; class GoalDriver { public: @@ -81,6 +84,14 @@ class GoalDriver { virtual void send(const GoalRequest&) = 0; virtual void cancel(const std::string& goal_id) = 0; virtual std::vector drain_events() = 0; + using RequestRecorder = std::function; + void set_request_recorder(RequestRecorder recorder) { request_recorder_=std::move(recorder); } + protected: + void record_wire_request(const std::string& id,const std::string& type,const std::string& snapshot) { + if(request_recorder_)request_recorder_(id,type,snapshot); + } + private: + RequestRecorder request_recorder_; }; struct Budgets { Milliseconds readiness{2000}, acceptance{2000}, feedback{5000}, execution{120000}, cancel_stop{5000}; }; enum class GoalState { SENDING, ACTIVE, CANCEL_REQUESTED, STOP_UNKNOWN, TERMINAL }; @@ -91,6 +102,7 @@ struct GoalRecord { std::uint64_t last_sequence{0}; SteadyTime sent_at{}, accepted_at{}, last_feedback{}, cancel_at{}; std::optional result; + std::string feedback_snapshot{}; }; class ActiveGoalRegistry { public: @@ -101,15 +113,19 @@ class ActiveGoalRegistry { // Registration is flushed before send. Failed journal writes throw and prevent send. std::optional start(GoalRequest, SteadyTime); void pump(SteadyTime); + void set_dispatch_recorder(std::function recorder) { dispatch_recorder_=std::move(recorder); } void request_cancel(const std::string&, SteadyTime); bool robot_locked(const std::string&) const; bool has_unresolved() const; const GoalRecord* find(const std::string&) const; + std::optional history(const std::string&) const; + std::vector task_records(const std::string& run_id) const; const std::map& records() const { return records_; } // Caller must verify the authenticated dedicated physical-reconciliation interface. bool reconcile(const std::string& goal_id, const Trace&, StopState, bool authorized, const std::string& evidence_id); private: GoalDriver& driver_; + std::function dispatch_recorder_; std::string journal_path_; Budgets budgets_; std::map records_; @@ -118,6 +134,8 @@ class ActiveGoalRegistry { void append(const GoalRecord&); void ingest(const GoalEvent&, SteadyTime); void load(); + void prune_terminal(); + std::deque terminal_order_; }; class ContextStore { public: @@ -165,6 +183,7 @@ class StageRunner { TickStatus settle(SteadyTime, RosTime); void update_safety(SafetySnapshot value) { safety_ = value; } const std::string& detail() const { return detail_; } + const std::string& error_code() const { return error_code_; } const std::string& active_goal_id() const { return active_goal_; } std::uint64_t geometry_epoch() const { return geometry_epoch_; } Holding holding() const { return holding_; } @@ -180,11 +199,13 @@ class StageRunner { SafetySnapshot safety_; Holding holding_{Holding::UNKNOWN}; std::uint64_t geometry_epoch_{0}; - RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, empty_valid_until_{0}, empty_observed_at_{0}; + RosTime capture_after_{0}, last_ros_time_{0}, holding_valid_until_{0}, holding_observed_at_{0}, empty_valid_until_{0}, empty_observed_at_{0}; std::size_t stage_index_{0}; - std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_; + std::string active_goal_, source_location_, source_side_, source_column_, source_tier_, verification_goal_, pending_posture_, detail_, error_code_; std::optional waiting_since_; std::optional motion_waiting_since_; + std::optional stopped_waiting_since_; + std::optional corroboration_waiting_since_, settlement_waiting_since_; unsigned reobservations_{0}, adjustments_{0}, serial_{0}; enum class PickPhase { ASSESS, REOBSERVE, ADJUST, EXECUTE }; PickPhase pick_phase_{PickPhase::ASSESS}; diff --git a/core/src/core.cpp b/core/src/core.cpp index 937148c..c6cb53e 100644 --- a/core/src/core.cpp +++ b/core/src/core.cpp @@ -38,6 +38,30 @@ bool protocol_matches(NativeStatus native,ResultCode result) { default:return false; } } +std::string hex_encode(const std::string& text) { + static const char hex[]="0123456789abcdef";std::string out; + for(unsigned char c:text){out.push_back(hex[c>>4]);out.push_back(hex[c&15]);}return out; +} +std::string hex_decode(const std::string& text) { + if(text.size()%2||text.find_first_not_of("0123456789abcdef")!=std::string::npos)throw std::runtime_error("corrupt journal evidence"); + std::string out;for(std::size_t i=0;i(std::stoi(text.substr(i,2),nullptr,16)));return out; +} +GoalRecord parse_record(const std::string& line) { + std::istringstream row(line);std::string detail;GoalRecord r;auto& t=r.request.trace;int version=0,skill=-1,state=-1,code=-1,stop=-1; + if(!(row>>version>>std::quoted(r.request.goal_id)>>std::quoted(r.request.robot_id)>>std::quoted(t.task_id)>>std::quoted(t.subtask_id)>>std::quoted(t.run_id)>>t.task_revision>>t.plan_version>>t.execution_generation>>t.attempt>>skill>>state>>r.cancel_intent>>r.accepted>>r.last_sequence>>code>>stop>>std::quoted(detail)) || (version!=2&&version!=3&&version!=4) || skill<0 || skill>static_cast(Skill::VERIFY_EMPTY) || state<0 || state>static_cast(GoalState::TERMINAL) || code < -1 || code>static_cast(ResultCode::REJECTED) || stop<0 || stop>1 || r.request.goal_id.empty() || r.request.robot_id.empty() || !valid_trace(t))throw std::runtime_error("corrupt goal journal; startup refused for physical reconciliation"); + r.request.skill=static_cast(skill);r.state=static_cast(state); + const auto decoded=hex_decode(detail); + if(code>=0){ExecutionResult result;result.code=static_cast(code);result.stop=static_cast(stop);result.detail=decoded;r.result=result;} + if(version>=3){std::string type,snapshot,feedback,error,reference;if(!(row>>std::quoted(type)>>std::quoted(snapshot)>>std::quoted(feedback)>>std::quoted(error)>>std::quoted(reference)))throw std::runtime_error("truncated journal evidence");r.request.wire_request_type=hex_decode(type);r.request.wire_request_snapshot=hex_decode(snapshot);r.feedback_snapshot=hex_decode(feedback);const auto e=hex_decode(error),ref=hex_decode(reference);if(r.result){r.result->error_code=e;r.result->execution_record_ref=ref;}else if(!e.empty()||!ref.empty())throw std::runtime_error("result evidence without result");} + if(version>=4){std::string type,snapshot;if(!(row>>std::quoted(type)>>std::quoted(snapshot)))throw std::runtime_error("truncated result snapshot");const auto t=hex_decode(type),b=hex_decode(snapshot);if(r.result){r.result->wire_result_type=t;r.result->wire_result_snapshot=b;}else if(!t.empty()||!b.empty())throw std::runtime_error("wire result without result");} + row>>std::ws;if(!row.eof())throw std::runtime_error("unexpected goal journal fields");return r; +} +void scan_journal(const std::string& path,const std::function& visitor) { + if(!std::filesystem::exists(path))return; + std::ifstream in(path);if(!in)throw std::runtime_error("goal journal unreadable; startup refused");std::string line; + while(std::getline(in,line)){if(in.eof())throw std::runtime_error("truncated goal journal; startup refused");visitor(parse_record(line));} + if(in.bad())throw std::runtime_error("goal journal read failed"); +} bool valid_meta(const SnapshotMeta& m,const Trace& t,std::uint64_t epoch,RosTime after,RosTime now) { return m.schema_version==1 && same_context(m.trace,t) && valid_trace(m.trace) && !m.source_goal_id.empty() && !m.writer.empty() && m.geometry_epoch==epoch && m.observed_at>0 && m.observed_at>=after && m.observed_at<=now && m.valid_until>now && m.valid_until>=m.observed_at; } @@ -65,15 +89,15 @@ ActiveGoalRegistry::ActiveGoalRegistry(GoalDriver& driver,std::string path,Budge lock_fd_=::open((journal_path_+".lock").c_str(),O_RDWR|O_CREAT|O_CLOEXEC|O_NOFOLLOW,0600); if(lock_fd_<0) throw std::runtime_error("goal journal process lock cannot be opened"); if(::flock(lock_fd_,LOCK_EX|LOCK_NB)!=0) {::close(lock_fd_);lock_fd_=-1;throw std::runtime_error("another executor owns this goal journal");} - try {load();} catch(...) {::flock(lock_fd_,LOCK_UN);::close(lock_fd_);lock_fd_=-1;throw;} + try {load();driver_.set_request_recorder([this](const std::string& id,const std::string& type,const std::string& snapshot){auto it=records_.find(id);if(it==records_.end()||it->second.state!=GoalState::SENDING||type.empty()||snapshot.empty())throw std::runtime_error("invalid wire request snapshot");auto& r=it->second;if(!r.request.wire_request_snapshot.empty())throw std::runtime_error("wire request already frozen");r.request.wire_request_type=type;r.request.wire_request_snapshot=snapshot;append(r);});} catch(...) {::flock(lock_fd_,LOCK_UN);::close(lock_fd_);lock_fd_=-1;throw;} } -ActiveGoalRegistry::~ActiveGoalRegistry() {if(lock_fd_>=0){::flock(lock_fd_,LOCK_UN);::close(lock_fd_);}} +ActiveGoalRegistry::~ActiveGoalRegistry() {driver_.set_request_recorder({});if(lock_fd_>=0){::flock(lock_fd_,LOCK_UN);::close(lock_fd_);}} void ActiveGoalRegistry::append(const GoalRecord& r) { std::ostringstream out; const auto& t=r.request.trace; std::string detail_hex; if(r.result) {static const char hex[]="0123456789abcdef";for(unsigned char c:r.result->detail){detail_hex.push_back(hex[c>>4]);detail_hex.push_back(hex[c&15]);}} - out<<2<<' '<(r.request.skill)<<' '<(r.state)<<' '<(r.result->code):-1)<<' '<<(r.result?static_cast(r.result->stop):0)<<' '<(r.request.skill)<<' '<(r.state)<<' '<(r.result->code):-1)<<' '<<(r.result?static_cast(r.result->stop):0)<<' '<error_code:std::string{}))<<' '<execution_record_ref:std::string{}))<<' '<wire_result_type:std::string{}))<<' '<wire_result_snapshot:std::string{}))<<'\n'; const std::string bytes=out.str(); const int fd=::open(journal_path_.c_str(),O_WRONLY|O_CREAT|O_APPEND|O_CLOEXEC|O_NOFOLLOW,0600); bool ok=fd>=0; @@ -85,29 +109,16 @@ void ActiveGoalRegistry::append(const GoalRecord& r) { if(!ok) { journal_failed_=true; throw std::runtime_error("goal journal sync failed; executor quarantined"); } } void ActiveGoalRegistry::load() { - if(!std::filesystem::exists(journal_path_)) return; - std::ifstream in(journal_path_); if(!in) throw std::runtime_error("goal journal unreadable; startup refused"); - std::string line; - while(std::getline(in,line)) { - if(in.eof())throw std::runtime_error("truncated goal journal; startup refused"); - std::istringstream row(line); std::string detail_hex; GoalRecord r; auto& t=r.request.trace; int version=0,skill=-1,state=-1,code=-1,stop=-1; - if(!(row>>version>>std::quoted(r.request.goal_id)>>std::quoted(r.request.robot_id)>>std::quoted(t.task_id)>>std::quoted(t.subtask_id)>>std::quoted(t.run_id)>>t.task_revision>>t.plan_version>>t.execution_generation>>t.attempt>>skill>>state>>r.cancel_intent>>r.accepted>>r.last_sequence>>code>>stop>>std::quoted(detail_hex)) || version!=2 || skill<0 || skill>static_cast(Skill::VERIFY_EMPTY) || state<0 || state>static_cast(GoalState::TERMINAL) || code < -1 || code>static_cast(ResultCode::REJECTED) || stop<0 || stop>1 || r.request.goal_id.empty() || r.request.robot_id.empty() || !valid_trace(t)) throw std::runtime_error("corrupt goal journal; startup refused for physical reconciliation"); - if(detail_hex.size()%2!=0||detail_hex.find_first_not_of("0123456789abcdef")!=std::string::npos)throw std::runtime_error("corrupt journal evidence"); - row>>std::ws; if(!row.eof()) throw std::runtime_error("unexpected goal journal fields"); - r.request.skill=static_cast(skill); r.state=static_cast(state); - if(code>=0) { ExecutionResult result; result.code=static_cast(code); result.stop=static_cast(stop); for(std::size_t i=0;i(std::stoi(detail_hex.substr(i,2),nullptr,16))); r.result=result; } - records_[r.request.goal_id]=r; - } - if(in.bad()) throw std::runtime_error("goal journal read failed"); - for(auto& entry:records_) if(entry.second.state!=GoalState::TERMINAL) { entry.second.state=GoalState::STOP_UNKNOWN; entry.second.restarted=true; entry.second.cancel_intent=true; } + scan_journal(journal_path_,[this](GoalRecord r){const auto id=r.request.goal_id;const auto terminal=r.state==GoalState::TERMINAL;records_[id]=std::move(r);if(terminal){terminal_order_.erase(std::remove(terminal_order_.begin(),terminal_order_.end(),id),terminal_order_.end());terminal_order_.push_back(id);prune_terminal();}}); + for(auto& entry:records_)if(entry.second.state!=GoalState::TERMINAL){entry.second.state=GoalState::STOP_UNKNOWN;entry.second.restarted=true;entry.second.cancel_intent=true;} } std::optional ActiveGoalRegistry::start(GoalRequest request,SteadyTime now) { if(journal_failed_||request.robot_id.empty()||!valid_trace(request.trace)||robot_locked(request.robot_id)||!driver_.ready(request.skill)) return {}; - for(const auto& item:records_) if(same_trace(item.second.request.trace,request.trace)) return {}; + bool duplicate=false;scan_journal(journal_path_,[&](GoalRecord r){if(same_trace(r.request.trace,request.trace))duplicate=true;});if(duplicate)return {}; request.goal_id=uuid(); GoalRecord record; record.request=std::move(request); record.sent_at=now; record.last_feedback=now; auto inserted=records_.emplace(record.request.goal_id,std::move(record)); auto& r=inserted.first->second; append(r); // This happens before transport can observe the request. - try { driver_.send(r.request); } catch(...) { r.state=GoalState::STOP_UNKNOWN; r.cancel_intent=true; r.cancel_at=now; + try { if(dispatch_recorder_)dispatch_recorder_(r.request);driver_.send(r.request); } catch(...) { r.state=GoalState::STOP_UNKNOWN; r.cancel_intent=true; r.cancel_at=now; try {driver_.cancel(r.request.goal_id);} catch(...) {} append(r); } return r.request.goal_id; @@ -119,6 +130,18 @@ bool ActiveGoalRegistry::robot_locked(const std::string& robot) const { } bool ActiveGoalRegistry::has_unresolved() const { if(journal_failed_) return true; for(const auto& item:records_) if(item.second.state!=GoalState::TERMINAL) return true; return false; } const GoalRecord* ActiveGoalRegistry::find(const std::string& id) const { auto it=records_.find(id); return it==records_.end()?nullptr:&it->second; } +void ActiveGoalRegistry::prune_terminal() { + while(terminal_order_.size()>256){const auto id=terminal_order_.front();terminal_order_.pop_front();auto it=records_.find(id);if(it!=records_.end()&&it->second.state==GoalState::TERMINAL)records_.erase(it);} +} +std::optional ActiveGoalRegistry::history(const std::string& id) const { + if(auto r=find(id))return *r; + std::optional out;scan_journal(journal_path_,[&](GoalRecord r){if(r.request.goal_id==id)out=std::move(r);});return out; +} +std::vector ActiveGoalRegistry::task_records(const std::string& run_id) const { + std::map selected;scan_journal(journal_path_,[&](GoalRecord r){if(r.request.trace.run_id==run_id){const auto id=r.request.goal_id;selected[id]=std::move(r);}}); + for(const auto& item:records_)if(item.second.request.trace.run_id==run_id)selected[item.first]=item.second; + std::vector out;for(auto& item:selected)out.push_back(std::move(item.second));return out; +} void ActiveGoalRegistry::request_cancel(const std::string& id,SteadyTime now) { auto it=records_.find(id); if(it==records_.end()) return; auto& r=it->second; if(r.state==GoalState::TERMINAL||r.cancel_intent) return; @@ -146,10 +169,10 @@ void ActiveGoalRegistry::ingest(const GoalEvent& event,SteadyTime now) { break; case EventKind::REJECTED: if(r.accepted||r.result) { r.state=GoalState::STOP_UNKNOWN; append(r); break; } - r.state=GoalState::TERMINAL; r.result=ExecutionResult{ResultCode::REJECTED,StopState::CONFIRMED,{},"server rejected before execution"}; append(r); break; + r.state=GoalState::TERMINAL; r.result=ExecutionResult{ResultCode::REJECTED,StopState::CONFIRMED,{},"server rejected before execution",{},{} }; append(r);terminal_order_.push_back(event.goal_id);prune_terminal(); break; case EventKind::FEEDBACK: - if(!r.accepted||r.cancel_intent||r.restarted||event.sequence<=r.last_sequence) return; - r.last_sequence=event.sequence; r.last_feedback=now; break; + if(!r.accepted||r.restarted||event.sequence<=r.last_sequence) return; + r.last_sequence=event.sequence; if(!r.cancel_intent)r.last_feedback=now; r.feedback_snapshot=event.feedback_snapshot; append(r); break; case EventKind::CANCEL_ACK: break; // An ACK says nothing about physical stop. case EventKind::RESULT: if(!protocol_matches(event.native_status,event.result.code)||event.result.stop!=StopState::CONFIRMED) { @@ -157,7 +180,7 @@ void ActiveGoalRegistry::ingest(const GoalEvent& event,SteadyTime now) { if(!r.cancel_intent) { r.cancel_intent=true; r.cancel_at=now; append(r); try { driver_.cancel(event.goal_id); } catch(...) {} } break; } - r.result=event.result; r.state=GoalState::TERMINAL; append(r); break; + r.result=event.result; r.state=GoalState::TERMINAL; append(r);terminal_order_.push_back(event.goal_id);prune_terminal(); break; } } void ActiveGoalRegistry::pump(SteadyTime now) { @@ -183,7 +206,7 @@ void ActiveGoalRegistry::pump(SteadyTime now) { bool ActiveGoalRegistry::reconcile(const std::string& id,const Trace& trace,StopState stop,bool authorized,const std::string& evidence) { auto it=records_.find(id); if(!authorized||evidence.empty()||stop!=StopState::CONFIRMED||it==records_.end()||!same_trace(it->second.request.trace,trace)||it->second.state==GoalState::TERMINAL) return false; - auto& r=it->second; r.result=ExecutionResult{ResultCode::CANCELED,StopState::CONFIRMED,{},"authorized reconciliation: "+evidence}; r.state=GoalState::TERMINAL; append(r); return true; + auto& r=it->second; r.result=ExecutionResult{ResultCode::CANCELED,StopState::CONFIRMED,{},"authorized reconciliation: "+evidence,{},{} }; r.state=GoalState::TERMINAL; append(r);terminal_order_.push_back(id);prune_terminal(); return true; } void ContextStore::replace_target(TargetBinding b) { std::lock_guard lock(mutex_); target_=std::move(b); } void ContextStore::replace_placement(PlacementBinding b) { std::lock_guard lock(mutex_); placement_=std::move(b); } diff --git a/core/src/workflow.cpp b/core/src/workflow.cpp index 417b3b6..7f781d6 100644 --- a/core/src/workflow.cpp +++ b/core/src/workflow.cpp @@ -33,15 +33,31 @@ TickStatus StageRunner::settle(SteadyTime now,RosTime ros) { if(last_ros_time_>0&&ros=budgets_.readiness)return TickStatus::INTERVENTION_REQUIRED; + return TickStatus::RUNNING; + } + return safety_.holding==Holding::EMPTY?TickStatus::SUCCESS:TickStatus::INTERVENTION_REQUIRED; + } try { registry_.pump(now); } catch(const std::exception& e) {detail_=e.what();return TickStatus::INTERVENTION_REQUIRED;} for(const auto& entry:registry_.records())if(entry.second.request.robot_id==task_.robot_id&&entry.second.state==GoalState::STOP_UNKNOWN)return TickStatus::INTERVENTION_REQUIRED; + if(!settlement_started_&®istry_.robot_locked(task_.robot_id))return TickStatus::RUNNING; + if(!safe(ros))return TickStatus::INTERVENTION_REQUIRED; + if(!safety_.stationary) { + if(!settlement_waiting_since_)settlement_waiting_since_=now; + if(now-*settlement_waiting_since_>=budgets_.readiness)return TickStatus::INTERVENTION_REQUIRED; + return TickStatus::RUNNING; + } + settlement_waiting_since_.reset(); if(!settlement_started_) { if(registry_.robot_locked(task_.robot_id))return TickStatus::RUNNING; if(!safe(ros)||!safety_.stationary)return TickStatus::INTERVENTION_REQUIRED; - if(delivered_&&empty_verified(ros)){settlement_done_=true;return TickStatus::SUCCESS;} - for(const auto& entry:registry_.records()) { - const auto& q=entry.second.request; + if(delivered_&&empty_verified(ros)){settlement_done_=true;return settle(now,ros);} + for(const auto& record:registry_.task_records(task_.trace.run_id)) { + const auto& q=record.request; if(q.trace.task_id==task_.trace.task_id&&q.trace.run_id==task_.trace.run_id&&q.skill==Skill::PLACE)settlement_place_=true; } settlement_started_=true;active_goal_.clear();waiting_since_.reset();capture_after_=ros; @@ -59,7 +75,7 @@ TickStatus StageRunner::settle(SteadyTime now,RosTime ros) { catch(const std::exception& e){detail_=e.what();settlement_failure_=TickStatus::INTERVENTION_REQUIRED;return *settlement_failure_;} delivered_=true; } - holding_=Holding::EMPTY;empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;settlement_done_=true;return TickStatus::SUCCESS; + holding_=Holding::EMPTY;empty_valid_until_=response.evidence->valid_until;empty_observed_at_=response.evidence->observed_at;settlement_done_=true;return settle(now,ros); } GoalRequest StageRunner::make_request(Stage stage,Skill skill,RosTime ros) { GoalRequest q;q.robot_id=task_.robot_id;q.trace=task_.trace;q.trace.subtask_id=std::string(stage_name(stage))+"/"+std::to_string(++serial_);q.trace.attempt=1;q.skill=skill;q.target_id=task_.target_id;q.destination_id=task_.destination_id;q.shelf=task_.source_shelf;q.geometry_epoch=geometry_epoch_;q.capture_after=std::max(capture_after_,ros);q.position_tolerance_m=task_.position_tolerance_m;q.orientation_tolerance_rad=task_.orientation_tolerance_rad; @@ -92,9 +108,17 @@ TickStatus StageRunner::run_goal(Stage stage,Skill skill,SteadyTime now,RosTime } if(active_goal_.empty()) { if(registry_.robot_locked(task_.robot_id))return fail("unresolved goal retains robot resource"); - if(!safety_.stationary)return fail("fresh stationary evidence required before dispatch"); + if(!safety_.stationary) { if(!stopped_waiting_since_)stopped_waiting_since_=now; if(now-*stopped_waiting_since_>=budgets_.readiness)return fail("fresh stationary evidence deadline exceeded before dispatch",false); return TickStatus::RUNNING; } + stopped_waiting_since_.reset(); const bool carrying_motion=skill==Skill::TRANSPORT_POSTURE||skill==Skill::PLACE||(skill==Skill::NAVIGATE&&stage==Stage::NAVIGATE_DESTINATION); const bool empty_motion=skill==Skill::PICK||skill==Skill::ADJUST_POSTURE||(skill==Skill::NAVIGATE&&stage!=Stage::NAVIGATE_DESTINATION); + const auto proof_at=carrying_motion?holding_observed_at_:empty_motion?empty_observed_at_:0; + if(proof_at>0&&safety_.observed_at=budgets_.readiness)return fail("robot state did not corroborate independent verification before deadline",false); + return TickStatus::RUNNING; + } + corroboration_waiting_since_.reset(); if(carrying_motion&&holding_valid_until_<=ros){holding_=Holding::UNKNOWN;return fail("independent held-target verification expired before motion dispatch");} if(carrying_motion&&safety_.holding!=Holding::HOLDING_TARGET){holding_=Holding::UNKNOWN;return fail("fresh holding state contradicts verified target; motion blocked");} if(empty_motion&&safety_.holding!=Holding::EMPTY){holding_=Holding::UNKNOWN;return fail("fresh empty-hand evidence required for motion");} @@ -104,12 +128,16 @@ TickStatus StageRunner::run_goal(Stage stage,Skill skill,SteadyTime now,RosTime catch(const std::exception& error) { return fail(error.what()); } return TickStatus::RUNNING; } - const auto* record=registry_.find(active_goal_);if(!record)return fail("active goal missing from registry"); - if(record->state==GoalState::STOP_UNKNOWN)return fail("physical stop unknown; robot quarantined"); + const auto saved_record=registry_.history(active_goal_);const auto* record=saved_record?&*saved_record:nullptr;if(!record)return fail("active goal missing from registry"); + if(error_code_.empty()&&record->result&&!record->result->error_code.empty()&&(record->state==GoalState::STOP_UNKNOWN||record->cancel_intent||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED||!record->result->response.valid))error_code_=record->result->error_code; + if(record->state==GoalState::STOP_UNKNOWN)return fail("physical stop unknown; robot quarantined"); if(record->state!=GoalState::TERMINAL)return TickStatus::RUNNING; - if(record->cancel_intent||!record->result||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED)return fail("goal failed/canceled/timed out; automatic motion retry disabled",false); + if(record->cancel_intent||!record->result||record->result->code!=ResultCode::COMPLETED||record->result->stop!=StopState::CONFIRMED) {return fail("goal failed/canceled/timed out; automatic motion retry disabled",false);} response=record->result->response;completed_goal=active_goal_; - if(!response.valid)return fail("typed skill response invalid or absent"); + if(!response.valid) { + const bool semantic_ambiguity=(skill==Skill::LOCATE_SHELF_COLUMN||skill==Skill::LOCALIZE_TARGET)&&(record->result->error_code=="NOT_FOUND"||record->result->error_code=="AMBIGUOUS"); + return fail("typed skill response invalid or absent",!semantic_ambiguity); + } if(skill==Skill::NAVIGATE&&(!response.base_stopped||!response.final_pose||!record->request.registered_pose||!within_tolerance(*response.final_pose,*record->request.registered_pose,task_.position_tolerance_m,task_.orientation_tolerance_rad)))return fail("navigation stopped pose outside exact tolerance or unknown"); if((skill==Skill::VERIFY_PICK||skill==Skill::VERIFY_TRANSPORT||skill==Skill::VERIFY_PLACE||skill==Skill::VERIFY_EMPTY)&&!evidence_valid(response,record->request,ros))return fail("verification evidence stale or mismatched"); if(skill==Skill::LOCALIZE_TARGET&&(!response.target||response.target->meta.source_goal_id!=active_goal_||!valid_target(*response.target,task_.trace,task_.target_id,geometry_epoch_,record->request.capture_after,ros)))return fail("invalid localization snapshot"); @@ -181,12 +209,12 @@ TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) { if(pick_phase_==PickPhase::ADJUST) {result=invoke(Skill::ADJUST_POSTURE);if(result!=TickStatus::SUCCESS)return result;if(!response.base_stopped)return fail("posture stop evidence missing");geometry_changed();pick_phase_=PickPhase::REOBSERVE;return TickStatus::RUNNING;} result=invoke(Skill::PICK);if(result==TickStatus::SUCCESS)holding_=Holding::UNKNOWN;break; case Stage::VERIFY_PICK: - result=invoke(Skill::VERIFY_PICK);if(result==TickStatus::SUCCESS) {if(!response.verified||response.target_id!=task_.target_id||response.holding!=Holding::HOLDING_TARGET||!response.base_stopped)return fail("independent pick verification not confirmed");holding_=Holding::HOLDING_TARGET;holding_valid_until_=response.evidence->valid_until;}break; + result=invoke(Skill::VERIFY_PICK);if(result==TickStatus::SUCCESS) {if(!response.verified||response.target_id!=task_.target_id||response.holding!=Holding::HOLDING_TARGET||!response.base_stopped)return fail("independent pick verification not confirmed");holding_=Holding::HOLDING_TARGET;holding_valid_until_=response.evidence->valid_until;holding_observed_at_=response.evidence->observed_at;}break; case Stage::TRANSPORT_POSTURE: if(holding_!=Holding::HOLDING_TARGET)return fail("transport posture requires verified held target"); result=invoke(Skill::TRANSPORT_POSTURE);if(result==TickStatus::SUCCESS) {if(!response.base_stopped)return fail("transport posture stop unconfirmed");geometry_changed();}break; case Stage::VERIFY_TRANSPORT: - result=invoke(Skill::VERIFY_TRANSPORT);if(result==TickStatus::SUCCESS){if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("independent transport verification not confirmed");holding_valid_until_=response.evidence->valid_until;}break; + result=invoke(Skill::VERIFY_TRANSPORT);if(result==TickStatus::SUCCESS){if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("independent transport verification not confirmed");holding_valid_until_=response.evidence->valid_until;holding_observed_at_=response.evidence->observed_at;}break; case Stage::CHECK_FREE_SPACE: if(task_.route!="LEGACY"){result=TickStatus::SUCCESS;break;} result=invoke(Skill::CHECK_FREE_SPACE);if(result==TickStatus::SUCCESS)context_.replace_placement(*response.placement);break; @@ -196,7 +224,7 @@ TickStatus StageRunner::tick(Stage stage,SteadyTime now,RosTime ros) { if(place_refresh_started_){ result=invoke(Skill::VERIFY_TRANSPORT);if(result!=TickStatus::SUCCESS)return result; if(!response.verified||response.holding!=Holding::HOLDING_TARGET||response.target_id!=task_.target_id||!response.base_stopped)return fail("held-target refresh before place not confirmed"); - holding_valid_until_=response.evidence->valid_until;place_refresh_started_=false;place_refresh_done_=true;return TickStatus::RUNNING; + holding_valid_until_=response.evidence->valid_until;holding_observed_at_=response.evidence->observed_at;place_refresh_started_=false;place_refresh_done_=true;return TickStatus::RUNNING; } result=invoke(Skill::PLACE);if(result==TickStatus::SUCCESS)holding_=Holding::UNKNOWN;break; case Stage::VERIFY_PLACE: diff --git a/core/tests/dispatch_audit_test.cpp b/core/tests/dispatch_audit_test.cpp new file mode 100644 index 0000000..965c2ab --- /dev/null +++ b/core/tests/dispatch_audit_test.cpp @@ -0,0 +1,11 @@ +#include "workflow_fixture.hpp" +#include +int main(){ + Simulator driver;const auto path=Fixture::test_root()+"/dispatch-audit.journal"; + ActiveGoalRegistry registry(driver,path);GoalRequest q;q.robot_id="robot";q.trace={"task","pick","run",1,1,1,1};q.skill=Skill::PICK; + bool observed=false; + registry.set_dispatch_recorder([&](const GoalRequest& request){std::ifstream durable(path);std::string line;std::getline(durable,line);assert(line.find(request.goal_id)!=std::string::npos);assert(driver.sent.empty());observed=true;throw std::runtime_error("task audit journal unavailable");}); + auto id=registry.start(q,SteadyTime{});assert(id);assert(observed);assert(driver.sent.empty());assert(registry.robot_locked("robot")); + q.trace.attempt=2;assert(!registry.start(q,SteadyTime{})); + std::cout<<"dispatch manifest audit failure prevents transport and retains lock\n"; +} diff --git a/core/tests/evidence_regression_test.cpp b/core/tests/evidence_regression_test.cpp new file mode 100644 index 0000000..32b1090 --- /dev/null +++ b/core/tests/evidence_regression_test.cpp @@ -0,0 +1,31 @@ +#include "workflow_fixture.hpp" +struct EvidenceDriver:Simulator { + std::string sabotage; + void send(const GoalRequest& q)override { + if(!sabotage.empty()){std::filesystem::remove(sabotage);std::filesystem::create_directory(sabotage);} + record_wire_request(q.goal_id,"bt_skill_interfaces/action/ExecuteManipulation_Goal","000102ff"); + sent.push_back(q); + } +}; +int main(){ + const auto path=Fixture::test_root()+"/evidence.journal"; EvidenceDriver driver; std::string id; + GoalRequest q;q.robot_id="robot";q.trace={"task","pick","run",1,1,1,1};q.skill=Skill::PICK; + {ActiveGoalRegistry registry(driver,path);id=*registry.start(q,SteadyTime{}); + assert(registry.find(id)->request.wire_request_snapshot=="000102ff"); + GoalEvent e;e.goal_id=id;e.trace=q.trace;e.kind=EventKind::ACCEPTED;driver.events.push_back(e);registry.pump(SteadyTime{}); + e.kind=EventKind::FEEDBACK;e.sequence=2;e.feedback_snapshot="{\"phase\":3,\"message\":\"executing\\nchunk\",\"progress\":0.25}";driver.events.push_back(e);registry.pump(SteadyTime{}); + e.sequence=1;e.feedback_snapshot="old feedback";driver.events.push_back(e);registry.pump(SteadyTime{}); + assert(registry.find(id)->feedback_snapshot.find("executing")!=std::string::npos); + registry.request_cancel(id,SteadyTime{});e.sequence=3;e.feedback_snapshot="STOPPING";driver.events.push_back(e);registry.pump(SteadyTime{});assert(registry.find(id)->feedback_snapshot=="STOPPING");assert(registry.robot_locked("robot")); + e.kind=EventKind::RESULT;e.native_status=NativeStatus::ABORTED;e.result.code=ResultCode::FAILED;e.result.stop=StopState::CONFIRMED;e.result.error_code="VLA_INFERENCE_TIMEOUT";e.result.execution_record_ref="records/run/pick.json";e.result.wire_result_type="bt_skill_interfaces/action/ExecuteManipulation_Result";e.result.wire_result_snapshot="000abbff";driver.events.push_back(e);registry.pump(SteadyTime{}); + } + {ActiveGoalRegistry registry(driver,path);auto r=registry.find(id);assert(r); + assert(r->request.wire_request_type=="bt_skill_interfaces/action/ExecuteManipulation_Goal"); + assert(r->request.wire_request_snapshot=="000102ff");assert(r->feedback_snapshot=="STOPPING"); + assert(r->result->wire_result_type=="bt_skill_interfaces/action/ExecuteManipulation_Result");assert(r->result->wire_result_snapshot=="000abbff");assert(r->result->error_code=="VLA_INFERENCE_TIMEOUT");assert(r->result->execution_record_ref=="records/run/pick.json"); + } + const auto bad=Fixture::test_root()+"/wire-write-failure.journal";EvidenceDriver broken;broken.sabotage=bad; + {ActiveGoalRegistry registry(broken,bad);try{registry.start(q,SteadyTime{});}catch(const std::exception&){}assert(broken.sent.empty());assert(registry.robot_locked("robot"));} + std::filesystem::remove(bad); + std::cout<<"wire request, ordered feedback and structured result survive restart; snapshot storage failure prevents transport\n"; +} diff --git a/core/tests/readiness_regression_test.cpp b/core/tests/readiness_regression_test.cpp index 960ee03..5ebb6a3 100644 --- a/core/tests/readiness_regression_test.cpp +++ b/core/tests/readiness_regression_test.cpp @@ -1,2 +1,23 @@ #include "workflow_fixture.hpp" -int main(){Fixture f("refresh_readiness");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS);f.driver.unavailable_skill=Skill::NAVIGATE;TickStatus status=TickStatus::RUNNING;for(int i=1;i<=60&&status==TickStatus::RUNNING;++i){f.driver.now=2000000+i*100000000LL;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});status=r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(i*100),f.driver.now);}assert(status==TickStatus::FAILURE);assert(f.count(Skill::NAVIGATE)==0);std::cout<<"after 6000ms unavailable navigation: still_running="<<(status==TickStatus::RUNNING)<<" verify_empty_calls="< +int main(){Simulator d;const auto path=Fixture::test_root()+"/retention.journal";std::string first;Trace original; + {ActiveGoalRegistry r(d,path);for(int i=0;i<280;++i){GoalRequest q;q.robot_id="r";q.trace={"task","step"+std::to_string(i),"run",1,1,1,1};auto id=r.start(q,SteadyTime{});assert(id);if(i==0){first=*id;original=q.trace;}r.pump(SteadyTime{});}assert(r.records().size()<=256);assert(!r.robot_locked("r"));auto old=r.history(first);assert(old&&old->state==GoalState::TERMINAL&&same_trace(old->request.trace,original));const auto history=r.task_records("run");assert(history.size()==280);std::set identities;for(const auto& record:history){assert(!record.request.goal_id.empty());identities.insert(record.request.goal_id);}assert(identities.size()==280);} + {ActiveGoalRegistry r(d,path);assert(r.records().size()<=256);GoalRequest q;q.robot_id="r";q.trace=original;assert(!r.start(q,SteadyTime{}));q.trace.subtask_id="pending";auto id=r.start(q,SteadyTime{});assert(id);} + {ActiveGoalRegistry r(d,path);assert(r.robot_locked("r"));assert(r.records().size()<=257);} + std::cout<<"terminal history bounded and archived attempts never replayed\n"; +} diff --git a/core/tests/semantic_failure_test.cpp b/core/tests/semantic_failure_test.cpp new file mode 100644 index 0000000..cb205fe --- /dev/null +++ b/core/tests/semantic_failure_test.cpp @@ -0,0 +1,12 @@ +#include "workflow_fixture.hpp" +void semantic_failure(const std::string& code,bool ordinary) { + Fixture f("semantic_"+code);auto r=f.runner();Workflow flow(r);unsigned i=0; + for(;f.count(Skill::LOCATE_SHELF_COLUMN)==0&&i<40;++i){f.driver.now=1000000+i*1000000;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);} + assert(f.count(Skill::LOCATE_SHELF_COLUMN)==1); + for(auto& e:f.driver.events)if(e.kind==EventKind::RESULT){e.result.response.valid=false;e.result.error_code=code;} + f.driver.now+=1000000;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000}); + assert(flow.tick(SteadyTime{}+Milliseconds(++i),f.driver.now)==(ordinary?TickStatus::FAILURE:TickStatus::INTERVENTION_REQUIRED)); + assert(r.error_code()==code);assert(f.count(Skill::PICK)==0); + if(ordinary){r.halt(SteadyTime{}+Milliseconds(i));TickStatus status=TickStatus::RUNNING;for(unsigned n=0;n<5&&status==TickStatus::RUNNING;++n){f.driver.now+=1000000;r.update_safety({true,true,Holding::EMPTY,f.driver.now,f.driver.now+1000000000});status=r.settle(SteadyTime{}+Milliseconds(++i),f.driver.now);}assert(status==TickStatus::SUCCESS);assert(r.error_code()==code);assert(f.count(Skill::PICK)==0);} +} +int main(){semantic_failure("AMBIGUOUS",true);semantic_failure("NOT_FOUND",true);semantic_failure("INVALID_RESULT",false);std::cout<<"typed readonly ambiguity allows independently verified safe settlement\n";} diff --git a/core/tests/settlement_test.cpp b/core/tests/settlement_test.cpp index b1d61a2..8f5b3e0 100644 --- a/core/tests/settlement_test.cpp +++ b/core/tests/settlement_test.cpp @@ -2,7 +2,7 @@ struct FailedPlaceDriver : Simulator { void send(const GoalRequest& q) override { Simulator::send(q); - if(q.skill==Skill::PLACE){auto& event=events.back();event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::FAILED;} + if(q.skill==Skill::PLACE){auto& event=events.back();event.native_status=NativeStatus::ABORTED;event.result.code=ResultCode::FAILED;event.result.error_code="VLA_EXECUTION_FAILED";} } }; int main(){ @@ -13,9 +13,11 @@ int main(){ for(;i<200&&status==TickStatus::RUNNING;++i){driver.now=1000000+i*1000000;runner.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000});status=flow.tick(SteadyTime{}+Milliseconds(i),driver.now);} assert(status==TickStatus::FAILURE);assert(deliveries==0); runner.halt(SteadyTime{}+Milliseconds(i)); + runner.update_safety({true,false,driver.sensor_holding,driver.now,driver.now+1000000000}); + assert(runner.settle(SteadyTime{}+Milliseconds(i),driver.now)==TickStatus::RUNNING); // Settlement is read-only except for the idempotent business receipt. for(status=TickStatus::RUNNING;i<300&&status==TickStatus::RUNNING;++i){driver.now=1000000+i*1000000;runner.update_safety({true,true,driver.sensor_holding,driver.now,driver.now+1000000000});status=runner.settle(SteadyTime{}+Milliseconds(i),driver.now);} - assert(status==TickStatus::SUCCESS);assert(deliveries==1); + assert(status==TickStatus::SUCCESS);assert(deliveries==1);assert(runner.error_code()=="VLA_EXECUTION_FAILED"); unsigned place=0,verify=0;for(const auto&q:driver.sent){place+=q.skill==Skill::PLACE;verify+=q.skill==Skill::VERIFY_PLACE;} assert(place==1&&verify==1);assert(runner.empty_verified(driver.now)); assert(runner.settle(SteadyTime{}+Milliseconds(i),driver.now)==TickStatus::SUCCESS);assert(deliveries==1); diff --git a/core/tests/state_alignment_test.cpp b/core/tests/state_alignment_test.cpp new file mode 100644 index 0000000..eab22af --- /dev/null +++ b/core/tests/state_alignment_test.cpp @@ -0,0 +1,16 @@ +#include "workflow_fixture.hpp" +void holding_alignment(bool contradictory){Fixture f(contradictory?"fresh_contradiction":"holding_lag");auto r=f.runner();Workflow flow(r);unsigned i=0; + for(;flow.current_stage()!=Stage::TRANSPORT_POSTURE&&i<100;++i){f.driver.now=1000000+i*1000000;r.update_safety({true,true,f.driver.sensor_holding,f.driver.now,f.driver.now+1000000000});assert(flow.tick(SteadyTime{}+Milliseconds(i),f.driver.now)==TickStatus::RUNNING);} + assert(flow.current_stage()==Stage::TRANSPORT_POSTURE);auto proof=f.registry.history(f.driver.sent.back().goal_id)->result->response.evidence->observed_at; + f.driver.now+=1000000;r.update_safety({true,true,Holding::EMPTY,contradictory?f.driver.now:proof-1,f.driver.now+1000000000}); + auto status=flow.tick(SteadyTime{}+Milliseconds(++i),f.driver.now); + if(contradictory){assert(status==TickStatus::INTERVENTION_REQUIRED);assert(f.count(Skill::TRANSPORT_POSTURE)==0);return;} + assert(status==TickStatus::RUNNING);assert(f.count(Skill::TRANSPORT_POSTURE)==0); + f.driver.now+=1000000;r.update_safety({true,true,Holding::HOLDING_TARGET,f.driver.now,f.driver.now+1000000000}); + assert(flow.tick(SteadyTime{}+Milliseconds(++i),f.driver.now)==TickStatus::RUNNING);assert(f.count(Skill::TRANSPORT_POSTURE)==1); +} +void empty_alignment(){Fixture f("empty_lag");auto r=f.runner();r.update_safety({true,true,Holding::EMPTY,1000000,9000000000});assert(r.tick(Stage::PREFLIGHT,SteadyTime{},1000000)==TickStatus::RUNNING);assert(r.tick(Stage::PREFLIGHT,SteadyTime{}+Milliseconds(1),2000000)==TickStatus::SUCCESS); + r.update_safety({true,true,Holding::UNKNOWN,999999,9000000000});assert(r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(2),3000000)==TickStatus::RUNNING);assert(f.count(Skill::NAVIGATE)==0); + r.update_safety({true,true,Holding::EMPTY,4000000,9000000000});assert(r.tick(Stage::NAVIGATE_OBSERVE,SteadyTime{}+Milliseconds(3),4000000)==TickStatus::RUNNING);assert(f.count(Skill::NAVIGATE)==1); +} +int main(){holding_alignment(false);holding_alignment(true);empty_alignment();std::cout<<"older robot state waits for proof corroboration; fresh contradictions block motion\n";} diff --git a/robobrain/robot_robobrain/demo_backend.py b/robobrain/robot_robobrain/demo_backend.py index a9b52cd..b684aad 100644 --- a/robobrain/robot_robobrain/demo_backend.py +++ b/robobrain/robot_robobrain/demo_backend.py @@ -18,7 +18,7 @@ class BrainDemoBackend(DemoBackend): self.brain=BrainService(FixtureBackend(fixture),self.state_dir/'planning_records') def start_planning(self,t): self.plans.append(t) - result=self.brain.plan(dict(task_id=t['task_id'],task_revision=t['task_revision'],planning_generation=t['planning_generation'],instruction=t['request']['instruction'],known_info=t['request']['known_info'],context=self.site,constraints={'schema_version':2,'route':self.site['execution_route']},timeout=10)) + result=self.brain.plan(dict(task_id=t['task_id'],task_revision=t['task_revision'],planning_generation=t['planning_generation'],instruction=t['request']['instruction'],known_info=t['request']['known_info'],context=self.site,constraints={'schema_version':2,'route':self.site['execution_route'],'clarification_confirmed':t.get('clarification_confirmed') is True},timeout=10)) event=dict(type='plan',task_id=t['task_id'],task_revision=t['task_revision'],planning_generation=t['planning_generation'],status=result['status'],error_code=result.get('error_code',''),planning_record_ref=result['record_ref']) if result['status']=='NEEDS_CLARIFICATION':event['questions']=result['plan']['missing_information'] else:event['plan']=result.get('plan') diff --git a/robobrain/robot_robobrain/intent.py b/robobrain/robot_robobrain/intent.py index dbb4369..3467396 100644 --- a/robobrain/robot_robobrain/intent.py +++ b/robobrain/robot_robobrain/intent.py @@ -1,30 +1,4 @@ -"""Conservative independent intent gate for instructions without confirmed slots. -Only registered aliases and explicit quantities are accepted. Other language asks -for structured clarification; this parser is not claimed to cover arbitrary NLP. -""" -import re -COUNTS={'一':1,'二':2,'两':2,'三':3,'四':4,'五':5,'六':6,'七':7,'八':8,'九':9,'十':10,'one':1,'two':2,'three':3} -def matches(instruction,registry,aliases): - candidates=[] - for key in registry: - for term in [key]+list(aliases.get(key,[])): - if not term:continue - for m in re.finditer(re.escape(term),instruction):candidates.append((m.start(),m.end(),key)) - chosen=[] - for item in sorted(candidates,key=lambda x:(-(x[1]-x[0]),x[0])): - if not any(item[0]262144:raise InferenceError('OUTPUT_TOO_LARGE') return raw @@ -49,6 +51,10 @@ class BrainService: if type(goal.get(key)) is not int or goal[key]<1:raise InferenceError('INVALID_INPUT') if not goal.get('task_id') or goal['constraints'].get('route') not in ROUTES:raise InferenceError('INVALID_INPUT') known=validate_known(goal.get('known_info',{})) + from .intent import conflicts_with_instruction + confirmed=(goal['task_revision']>1 and goal['constraints'].get('clarification_confirmed') is True) + if conflicts_with_instruction(goal['instruction'],known,goal.get('context',{}),clarification_confirmed=confirmed): + raise InferenceError('SEMANTIC_MISMATCH','structured information contradicts explicit instruction') complete=('items' in known and 'destination' in known) or all(k in known for k in ('target_name','quantity','source_location','destination')) if not complete: from .intent import extract @@ -71,6 +77,8 @@ class BrainService: try:plan=validate_plan(data) except ApiError as ex:raise InferenceError('PLAN_INVALID',str(ex)) if plan['schema_version']!=2 or plan['route']!=goal['constraints']['route']:raise InferenceError('PLAN_INVALID') + if conflicts_with_instruction(goal['instruction'],plan['slots'],goal.get('context',{}),clarification_confirmed=confirmed): + raise InferenceError('SEMANTIC_MISMATCH','model plan contradicts explicit instruction') # Explicit user-confirmed structured slots are an independent check. expected=plan['slots'] if 'items' not in known and known: @@ -90,7 +98,7 @@ class BrainService: obs=self._snapshot(obs) raw=self._call('shelf',goal,'Identify target in this one registered shelf. JSON only {status:SUCCEEDED|NOT_FOUND|AMBIGUOUS,shelf_id,side_id,column_id,tier_id,confidence}. Never guess missing row/column. Target and input: '+canonical(goal),cancel,obs) data=strict_json(raw) - if data.get('status') in ('NOT_FOUND','AMBIGUOUS'):result=dict(status=data['status'],error_code=data['status']) + if data.get('status') in ('NOT_FOUND','AMBIGUOUS'):result=dict(status=data['status'],error_code=data['status'],observation_id=observation.observation_id,observed_at=observation.stamp_ns) else: if set(data)!={'status','shelf_id','side_id','column_id','tier_id','confidence'} or data['status']!='SUCCEEDED' or data['shelf_id']!=observation.shelf_id or any(not isinstance(data[k],str) or not data[k].strip() or len(data[k])>100 for k in ('side_id','column_id')) or not isinstance(data['tier_id'],str) or len(data['tier_id'])>100 or (data['tier_id'] and not data['tier_id'].strip()):raise InferenceError('SHELF_INVALID') c=data['confidence'] @@ -106,7 +114,8 @@ class BrainService: obs=self._snapshot(obs) raw=self._call('localize3d',goal,'Diagnostic object-center estimate only, never grasp point or navigation pose. Return JSON {point:[x,y,z]} or {status:NOT_FOUND|AMBIGUOUS}. Input: '+canonical(goal),cancel,obs) data=strict_json(raw) - if data.get('status') in ('NOT_FOUND','AMBIGUOUS'):result=dict(status=data['status'],geometry_valid=False) + if data.get('status') in ('NOT_FOUND','AMBIGUOUS'): + result=dict(status=data['status'],geometry_valid=False,error_code=data['status'],observation_id=observation.observation_id,observed_at=observation.stamp_ns,calibration_id=observation.calibration_id,geometry_epoch=observation.geometry_epoch) else: point=data['point'] if set(data)!={'point'} or not isinstance(point,list) or len(point)!=3 or any(type(v) not in (int,float) or not math.isfinite(v) for v in point):raise InferenceError('POINT_INVALID') diff --git a/ros2/bt_executor/CMakeLists.txt b/ros2/bt_executor/CMakeLists.txt index dcea38f..9052a56 100644 --- a/ros2/bt_executor/CMakeLists.txt +++ b/ros2/bt_executor/CMakeLists.txt @@ -17,7 +17,7 @@ add_library(robot_bt_core STATIC ../../core/src/core.cpp ../../core/src/workflow target_include_directories(robot_bt_core PUBLIC ../../core/include) add_executable(bt_executor_node src/executor_node.cpp src/ros_driver.cpp) target_include_directories(bt_executor_node PRIVATE include) -target_link_libraries(bt_executor_node robot_bt_core BT::behaviortree_cpp nlohmann_json::nlohmann_json) +target_link_libraries(bt_executor_node robot_bt_core behaviortree_cpp::behaviortree_cpp nlohmann_json::nlohmann_json) ament_target_dependencies(bt_executor_node ament_index_cpp rclcpp rclcpp_action bt_skill_interfaces geometry_msgs std_msgs) target_compile_options(bt_executor_node PRIVATE -Wall -Wextra -Wpedantic) install(TARGETS bt_executor_node DESTINATION lib/${PROJECT_NAME}) diff --git a/ros2/bt_executor/include/bt_executor/recovery_policy.hpp b/ros2/bt_executor/include/bt_executor/recovery_policy.hpp new file mode 100644 index 0000000..34e9674 --- /dev/null +++ b/ros2/bt_executor/include/bt_executor/recovery_policy.hpp @@ -0,0 +1,18 @@ +#pragma once +#include +#include + +namespace bt_executor { +// Compare every byte in the configured secret without an early mismatch exit. +inline bool recovery_authorized(const std::string& expected,const std::string& supplied) { + if(expected.size()<16||expected.size()>1024||supplied.size()>1024)return false; + std::size_t difference=expected.size()^supplied.size(); + for(std::size_t i=0;i(expected[i])^ + static_cast(i #include +#include #include #include #include @@ -44,8 +45,10 @@ class RosDriver final : public robot_bt::GoalDriver { std::vector drain_events() override; robot_bt::SafetySnapshot safety(const std::string& target_id) const; json mappings() const; + json mappings(const std::string& run_id) const; bool faulted() const { return faulted_; } void bind_task(const robot_bt::TaskConfig& task,const robot_bt::SiteConfig& site,std::uint32_t registry_version) { + shelf_bindings_.clear(); current_task_=task;allowed_postures_=site.allowed_postures;registry_version_=registry_version; } std::optional geometry_epoch() const; @@ -90,6 +93,7 @@ class RosDriver final : public robot_bt::GoalDriver { rclcpp::Publisher::SharedPtr target_pub_; rclcpp::Publisher::SharedPtr placement_pub_; void record_mapping(const robot_bt::GoalRequest&, const rclcpp_action::GoalUUID&); + void prune_mappings(const std::string& keep = {}); builtin_interfaces::msg::Duration timeout() const { return skill_timeout_; } bool fresh(robot_bt::RosTime observed, robot_bt::RosTime valid_until, robot_bt::RosTime capture_after = 0) const; @@ -104,6 +108,9 @@ class RosDriver final : public robot_bt::GoalDriver { typename Action::Goal goal, const robot_bt::GoalRequest& request, unsigned max_phase, Decode decode) { using Handle = rclcpp_action::ClientGoalHandle; + // Capture the exact generated Goal after construction and before transport. + // The registry synchronously fsyncs this record; failure prevents sending. + record_wire_request(request.goal_id,rosidl_generator_traits::name(),serialized_hex(goal)); typename rclcpp_action::Client::SendGoalOptions options; options.goal_response_callback = [this, client, request](typename Handle::SharedPtr handle) { robot_bt::GoalEvent event; @@ -151,7 +158,21 @@ class RosDriver final : public robot_bt::GoalDriver { if (at <= 0 || at > now || now - at > observation_lifetime_ns_) return; robot_bt::GoalEvent event; event.kind = robot_bt::EventKind::FEEDBACK; event.goal_id = request.goal_id; event.trace = request.trace; - event.sequence = feedback->sequence; events_.push_back(event); + event.sequence = feedback->sequence; + json payload={{"type",rosidl_generator_traits::name()},{"cdr_hex",serialized_hex(*feedback)}, + {"stamp_ns",at},{"sequence",feedback->sequence},{"phase",feedback->phase},{"message",feedback->message}}; + if constexpr (std::is_same_v||std::is_same_v) { + payload["elapsed_time_ns"]=std::int64_t(feedback->elapsed_time.sec)*1000000000LL+feedback->elapsed_time.nanosec; + } + if constexpr (std::is_same_v) { + payload["progress_valid"]=feedback->progress_valid;payload["progress"]=feedback->progress; + } + if constexpr (std::is_same_v) { + payload["pose_valid"]=feedback->pose_valid;payload["errors_valid"]=feedback->errors_valid; + payload["position_error"]=feedback->position_error;payload["orientation_error"]=feedback->orientation_error; + payload["blocked_valid"]=feedback->blocked_valid;payload["blocked"]=feedback->blocked; + } + event.feedback_snapshot=payload.dump();events_.push_back(event); }; options.result_callback = [this, request, decode](const typename Handle::WrappedResult& result) { robot_bt::GoalEvent event; event.kind = robot_bt::EventKind::RESULT; @@ -160,13 +181,28 @@ class RosDriver final : public robot_bt::GoalDriver { if (result.result) { try { event.result = decode(*result.result, result.code); } catch (const std::exception& e) { event.result.detail = std::string("invalid result: ") + e.what(); } + try { + event.result.wire_result_type=rosidl_generator_traits::name(); + event.result.wire_result_snapshot=serialized_hex(*result.result); + } catch(const std::exception& e) { + event.result.stop=robot_bt::StopState::UNKNOWN;event.result.response.valid=false; + event.result.detail=std::string("result snapshot unavailable: ")+e.what(); + } } events_.push_back(event); cancelers_.erase(request.goal_id); cancel_intents_.erase(request.goal_id); + prune_mappings(); }; // No spin_until_future_complete, wait_for_action_server or blocking get here. (void)client->async_send_goal(goal, options); } + template static std::string serialized_hex(const Message& message) { + rclcpp::Serialization serialization;rclcpp::SerializedMessage bytes; + serialization.serialize_message(&message,&bytes); + const auto& raw=bytes.get_rcl_serialized_message();static const char hex[]="0123456789abcdef"; + std::string out;out.reserve(raw.buffer_length*2); + for(std::size_t i=0;i>4]);out.push_back(hex[raw.buffer[i]&15]);}return out; + } }; } // namespace bt_executor diff --git a/ros2/bt_executor/launch/executor.launch.py b/ros2/bt_executor/launch/executor.launch.py index 52c1c71..9e71554 100644 --- a/ros2/bt_executor/launch/executor.launch.py +++ b/ros2/bt_executor/launch/executor.launch.py @@ -1,7 +1,7 @@ """Explicit deployment identity/site/journal; motion disabled unless opted in.""" from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, EnvironmentVariable from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -18,4 +18,6 @@ def generate_launch_description(): 'site_config_file': LaunchConfiguration('site_config_file'), 'journal_directory': LaunchConfiguration('journal_directory'), 'execution_enabled': ParameterValue(LaunchConfiguration('execution_enabled'), value_type=bool), + 'recovery_token': ParameterValue(EnvironmentVariable('ROBOT_BT_RECOVERY_TOKEN', default_value=''), value_type=str), + 'recovery_operator_id': ParameterValue(EnvironmentVariable('ROBOT_BT_RECOVERY_OPERATOR', default_value=''), value_type=str), }])]) diff --git a/ros2/bt_executor/package.xml b/ros2/bt_executor/package.xml index 2143139..c17e7e1 100644 --- a/ros2/bt_executor/package.xml +++ b/ros2/bt_executor/package.xml @@ -1,4 +1,4 @@ - + bt_executor1.2.0 Fixed BehaviorTree.CPP execution with persistent asynchronous ROS2 goal tracking. diff --git a/ros2/bt_executor/src/executor_node.cpp b/ros2/bt_executor/src/executor_node.cpp index 7539ad1..afb0cda 100644 --- a/ros2/bt_executor/src/executor_node.cpp +++ b/ros2/bt_executor/src/executor_node.cpp @@ -1,7 +1,9 @@ #include +#include #include #include #include +#include #include #include #include @@ -10,6 +12,8 @@ #include #include #include +#include +#include namespace bt_executor { using namespace robot_bt; @@ -50,12 +54,41 @@ static void append_receipt(const std::string& path,const json& data) { if(n<=0){::close(fd);throw std::runtime_error("receipt journal write failed");}offset+=n; } auto ok=::fsync(fd);::close(fd);if(ok)throw std::runtime_error("receipt journal sync failed"); + int directory=::open(std::filesystem::path(path).parent_path().c_str(),O_RDONLY|O_DIRECTORY); + if(directory<0)throw std::runtime_error("journal directory unavailable"); + ok=::fsync(directory);::close(directory);if(ok)throw std::runtime_error("journal directory sync failed"); +} +static json trace_json(const iface::msg::TaskTrace& trace) { + return {{"task_id",trace.task_id},{"subtask_id",trace.subtask_id},{"run_id",trace.run_id}, + {"attempt",trace.attempt},{"task_revision",trace.task_revision},{"plan_version",trace.plan_version}, + {"execution_generation",trace.execution_generation}}; +} +static std::string recovery_id() { + std::random_device random;std::ostringstream out;out<<"reconcile/"< static void scan_records(const std::string& path,Visit visit) { + std::ifstream stream(path); + if(!stream) { + if(std::filesystem::exists(path))throw std::runtime_error("history unreadable"); + return; + } + std::string line; + while(std::getline(stream,line)) { + require(!stream.eof(),"journal row is not durably newline-terminated"); + require(line.size()<=1048576,"journal row exceeds bound"); + if(!line.empty())visit(json::parse(line)); + } + require(stream.eof(),"history read failed"); } class ExecutorNode final:public rclcpp::Node { public: using Action=iface::action::ExecuteTask; using Handle=rclcpp_action::ServerGoalHandle; - ExecutorNode():Node("bt_executor") { + using Reconcile=iface::srv::ReconcileTask; + using Verify=iface::action::VerifyState; + ExecutorNode():Node("bt_executor",rclcpp::NodeOptions().start_parameter_services(false).start_parameter_event_publisher(false)) { robot_id_=declare_parameter("robot_id",""); auto allowed=declare_parameter>("allowed_robots",std::vector{}); enabled_=declare_parameter("execution_enabled",false); @@ -92,21 +125,29 @@ class ExecutorNode final:public rclcpp::Node { registry_=std::make_unique(*driver_,journal_dir_+"/goal_registry.log",budgets_); receipt_path_=journal_dir_+"/deliveries.jsonl"; task_journal_=journal_dir_+"/task_runs.jsonl"; - std::ifstream task_history(task_journal_);std::string task_line; - while(std::getline(task_history,task_line))if(!task_line.empty()) { - const auto record=strict_json(task_line);seen_runs_.insert(record.at("run_id").get()); - faulted_=record.at("state")!="RELEASED"; - } - std::ifstream receipts(receipt_path_);std::string line; - while(std::getline(receipts,line))if(!line.empty()) { - auto receipt=strict_json(line);receipts_[json::array({receipt.at("task_id"),receipt.at("item_index")}).dump()]=receipt; - } + registry_->set_dispatch_recorder([this](const GoalRequest& request){ + append_receipt(task_journal_,{{"task_id",request.trace.task_id},{"run_id",request.trace.run_id},{"state","ACTIVE"}, + {"dispatch",{{"goal_id",request.goal_id},{"skill",static_cast(request.skill)},{"trace",trace_json(trace_msg(request.trace))}}}}); + }); + scan_records(task_journal_,[this](const json& record){ + const auto run=record.at("run_id").get(); + if(record.at("state")=="RELEASED")unresolved_runs_.erase(run);else unresolved_runs_.insert(run); + }); + faulted_=!unresolved_runs_.empty(); + scan_records(receipt_path_,[this](const json& receipt){cache_receipt(receipt);}); + recovery_token_=declare_parameter("recovery_token",""); + require(recovery_token_.empty()||(recovery_token_.size()>=16&&recovery_token_.size()<=1024),"recovery_token must be empty (disabled) or 16..1024 bytes"); + recovery_operator_=declare_parameter("recovery_operator_id",""); + recovery_timeout_=timeout_policy("recovery_timeout_ms",5000); + verifier_=rclcpp_action::create_client(this,"skills/verify_state"); + reconcile_=create_service("tasks/reconcile", + [this](std::shared_ptr header,std::shared_ptr request){begin_recovery(header,request);}); factory_.registerNodeType("RunStage"); factory_.registerSimpleCondition("ApprovedPlanGate",[](BT::TreeNode& node) { - return node.config().blackboard->get>("runtime")->admitted?BT::NodeStatus::SUCCESS:BT::NodeStatus::FAILURE; + return static_cast(node).config().blackboard->get>("runtime")->admitted?BT::NodeStatus::SUCCESS:BT::NodeStatus::FAILURE; }); factory_.registerSimpleAction("RequestClarification",[](BT::TreeNode& node) { - auto rt=node.config().blackboard->get>("runtime"); + auto rt=static_cast(node).config().blackboard->get>("runtime"); rt->clarification=true;rt->stage="NeedsClarification";return BT::NodeStatus::FAILURE; }); // The only XML comes from this installed package. No action field, plan, or @@ -119,8 +160,8 @@ class ExecutorNode final:public rclcpp::Node { try { auto plan=strict_json(goal->approved_plan_json),context=strict_json(goal->context_json); auto task=admit(plan,context,goal->trace,trusted_,robot_id_); - require(!receipts_.count(json::array({task.trace.task_id,task.item_index}).dump()),"task already delivered; reconcile receipt without replay"); - require(!seen_runs_.count(task.trace.run_id),"execution run already dispatched"); + require(receipt_for(task.trace.task_id,task.item_index).is_null(),"task already delivered; reconcile receipt without replay"); + require(task_record(task.trace.run_id).empty(),"execution run already dispatched"); require(goal->timeout.sec>0&&goal->timeout.sec<=3600&&goal->timeout.nanosec<1000000000,"invalid task timeout"); reserved_=true;return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }catch(const std::exception& e){RCLCPP_WARN(get_logger(),"Task rejected: %s",e.what());return rclcpp_action::GoalResponse::REJECT;} @@ -148,29 +189,189 @@ class ExecutorNode final:public rclcpp::Node { rclcpp::Publisher::SharedPtr registry_pub_; rclcpp::TimerBase::SharedPtr timer_; std::map receipts_; - std::set seen_runs_; + std::set unresolved_runs_; + std::string recovery_token_,recovery_operator_; + Milliseconds recovery_timeout_{5000}; + rclcpp::Service::SharedPtr reconcile_; + rclcpp_action::Client::SharedPtr verifier_; + struct Recovery { + std::shared_ptr header; + iface::msg::TaskTrace trace; + json task; + std::string operator_id,evidence_ref,resolution,verification_id; + RosTime issued_at{0};std::uint64_t geometry_epoch{0};SteadyTime deadline; + rclcpp_action::ClientGoalHandle::SharedPtr handle; + }; + std::shared_ptr recovery_; unsigned quantity_{0};std::uint32_t sequence_{0};std::uint64_t tick_count_{0}; SteadyTime deadline_{}; std::string requested_status_,detail_,active_receipt_key_; + void cache_receipt(const json& receipt) { + receipts_[json::array({receipt.at("task_id"),receipt.at("item_index")}).dump()]=receipt; + while(receipts_.size()>256)receipts_.erase(receipts_.begin()); + } + json receipt_for(const std::string& task,unsigned item) { + const auto key=json::array({task,item}).dump(); + if(receipts_.count(key))return receipts_.at(key); + json found; + scan_records(receipt_path_,[&](const json& row){if(row.at("task_id")==task&&row.at("item_index")==item)found=row;}); + if(!found.is_null())cache_receipt(found); + return found; + } + json task_record(const std::string& run) const { + json found=json::object(),dispatches=json::object(); + scan_records(task_journal_,[&](const json& row){if(row.at("run_id")==run){ + found.update(row); + if(row.contains("dispatch")){ + const auto& dispatch=row.at("dispatch");const auto id=dispatch.at("goal_id").get(); + require(!dispatches.contains(id)||dispatches.at(id)==dispatch,"conflicting dispatch manifest");dispatches[id]=dispatch; + } + }}); + if(!found.empty())found["dispatches"]=dispatches; + return found; + } + bool complete_dispatch_history(const json& task,const std::vector& records) const { + if(!task.value("history_complete",false)||task.value("dispatch_manifest_version",0)!=1||!task.contains("dispatches"))return false; + const auto& manifest=task.at("dispatches"); + if(!manifest.is_object()||manifest.size()!=records.size())return false; + for(const auto& record:records) { + if(!manifest.contains(record.request.goal_id))return false; + const auto& saved=manifest.at(record.request.goal_id); + if(saved.at("skill")!=static_cast(record.request.skill)||saved.at("trace")!=trace_json(trace_msg(record.request.trace)))return false; + } + return true; + } + json task_receipts(const std::string& task) const { + std::map found; + scan_records(receipt_path_,[&](const json& row){if(row.at("task_id")==task){ + const auto item=row.at("item_index").get();require(item<20,"invalid receipt item index");found[item]=row; + }}); + json result=json::array(); + for(const auto& entry:found){const auto& row=entry.second;json evidence=json::object(); + for(const auto* key:{"evidence_id","target_ref","destination_ref","passed","empty_hand","in_destination","valid"})evidence[key]=row.at(key); + result.push_back({{"task_id",task},{"item_index",entry.first},{"run_id",row.at("run_id")},{"completed_quantity",1},{"evidence",evidence}}); + } + return result; + } + void recovery_reply(const std::shared_ptr& header,bool accepted,const std::string& error,const std::string& message,const json& state=json::object()) { + Reconcile::Response response;response.accepted=accepted;response.error_code=error;response.message=message;response.state_json=state.dump(); + try{reconcile_->send_response(*header,response);}catch(const std::exception& e){RCLCPP_ERROR(get_logger(),"Recovery response failed: %s",e.what());} + } + void reject_recovery(std::shared_ptr pending,const std::string& reason) { + if(recovery_!=pending)return; + if(pending->handle) { + try{verifier_->async_cancel_goal(pending->handle);}catch(...){} + } + recovery_.reset(); + faulted_=true; + recovery_reply(pending->header,false,"RECONCILIATION_FAILED",reason); + } + void begin_recovery(const std::shared_ptr& header,const std::shared_ptr& request) { + try { + require(!recovery_operator_.empty()&&request->operator_id==recovery_operator_&&recovery_authorized(recovery_token_,request->authorization),"recovery authorization rejected"); + require(!active_&&!reserved_&&!recovery_&&!driver_->faulted(),"executor busy or driver durability fault"); + require(request->resolution=="resume_task"||request->resolution=="cancel_task"||request->resolution=="replan_task","unsupported recovery resolution"); + require(!request->evidence_ref.empty()&&request->evidence_ref.size()<=500,"operator evidence reference required"); + const auto saved=task_record(request->trace.run_id); + require(!saved.empty()&&saved.contains("trace")&&saved.at("trace")==trace_json(request->trace),"exact durable task trace required"); + require(saved.contains("approved_plan_json")&&saved.contains("context_json"),"legacy task lacks complete recovery snapshot"); + for(const auto& run:unresolved_runs_)require(run==request->trace.run_id,"another task remains unresolved"); + for(const auto& entry:registry_->records())if(entry.second.state!=GoalState::TERMINAL) + require(entry.second.request.trace.run_id==request->trace.run_id,"another goal remains unresolved"); + require(verifier_->action_server_is_ready(),"independent verifier unavailable"); + auto task=admit(strict_json(saved.at("approved_plan_json").get()),strict_json(saved.at("context_json").get()),request->trace,trusted_,robot_id_); + const auto state=driver_->safety(task.target_id);const auto epoch=driver_->geometry_epoch(); + require(state.safe&&state.stationary&&state.holding==Holding::EMPTY&&epoch.has_value(),"fresh safe stationary empty RobotState/Safety required"); + auto pending=std::make_shared();pending->header=header;pending->trace=request->trace;pending->task=saved; + pending->operator_id=request->operator_id;pending->evidence_ref=request->evidence_ref;pending->resolution=request->resolution; + pending->verification_id=recovery_id();pending->issued_at=now().nanoseconds();pending->geometry_epoch=*epoch;pending->deadline=SteadyClock::now()+recovery_timeout_; + // Record the read-only verification intent, never the authorization secret. + unresolved_runs_.insert(request->trace.run_id);faulted_=true; + append_receipt(task_journal_,{{"task_id",request->trace.task_id},{"run_id",request->trace.run_id},{"state","RECONCILING"}, + {"recovery",{{"operator_id",pending->operator_id},{"evidence_ref",pending->evidence_ref},{"resolution",pending->resolution},{"verification_id",pending->verification_id},{"issued_at_ns",pending->issued_at}}}}); + recovery_=pending; + Verify::Goal goal;goal.trace=request->trace;goal.check=Verify::Goal::PRECHECK;goal.source_goal_id=pending->verification_id; + goal.target.object_ref=task.target_id;goal.target.description=task.target_id;goal.destination.region_ref=task.destination_id;goal.destination.description=task.destination_id; + goal.expected_geometry_epoch=*epoch;goal.capture_after=stamp(pending->issued_at); + goal.timeout.sec=static_cast(recovery_timeout_.count()/1000);goal.timeout.nanosec=static_cast((recovery_timeout_.count()%1000)*1000000); + rclcpp_action::Client::SendGoalOptions options; + options.goal_response_callback=[this,pending](rclcpp_action::ClientGoalHandle::SharedPtr handle){ + if(recovery_!=pending) { + if(handle)verifier_->async_cancel_goal(handle); + return; + } + if(!handle) { + reject_recovery(pending,"independent verification rejected"); + return; + } + pending->handle=handle; + }; + options.result_callback=[this,pending](const rclcpp_action::ClientGoalHandle::WrappedResult& result){complete_recovery(pending,result);}; + try{verifier_->async_send_goal(goal,options);}catch(const std::exception& e){reject_recovery(pending,e.what());} + }catch(const std::exception& e){recovery_reply(header,false,"RECONCILIATION_REJECTED",e.what());} + } + void complete_recovery(const std::shared_ptr& pending,const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + if(recovery_!=pending)return; + try { + require(SteadyClock::now()deadline&&result.code==rclcpp_action::ResultCode::SUCCEEDED&&result.result,"independent verification failed or late"); + const auto& e=result.result->evidence;const auto current=now().nanoseconds(); + const auto context=strict_json(pending->task.at("context_json").get()); + const auto target=context.at("target_id").get();const auto state=driver_->safety(target);const auto epoch=driver_->geometry_epoch(); + require(e.context.schema_version==1&&same_trace(trace_core(e.context.trace),trace_core(pending->trace))&&e.context.source_goal_id==pending->verification_id, + "verification identity mismatch"); + require(epoch&&*epoch==pending->geometry_epoch&&e.context.geometry_epoch==pending->geometry_epoch,"geometry changed during recovery"); + require(ns(e.context.observed_at)>pending->issued_at&&ns(e.context.observed_at)<=current&&ns(e.context.valid_until)>current&¤t-ns(e.context.observed_at)<=2000000000LL, + "verification stale, future, or predates request"); + require(!e.context.observation_id.empty()&&!e.context.writer.empty()&&!e.evidence_ref.empty()&&!e.source.empty()&&e.target_ref==target, + "verification provenance incomplete"); + require(e.status==0&&e.stopped_valid&&e.stopped&&e.hand_empty_valid&&e.hand_empty&&e.holding_state==0, + "independent stationary empty-hand proof missing"); + require(state.safe&&state.stationary&&state.holding==Holding::EMPTY&&state.observed_at>=pending->issued_at&&!driver_->faulted(),"fresh safety or robot state missing"); + bool manipulation=false;const auto records=registry_->task_records(pending->trace.run_id); + for(const auto& record:records){const auto& t=record.request.trace; + require(t.task_id==pending->trace.task_id&&t.task_revision==pending->trace.task_revision&&t.plan_version==pending->trace.plan_version&&t.execution_generation==pending->trace.execution_generation, + "goal history belongs to another intent"); + manipulation=manipulation||record.request.skill==Skill::PICK||record.request.skill==Skill::PLACE; + } + const auto item=context.value("item_index",0u);const auto receipt=receipt_for(pending->trace.task_id,item); + const bool complete_history=complete_dispatch_history(pending->task,records); + const bool safe_retry=complete_history&&!manipulation; + require(pending->resolution!="resume_task"||recovery_resume_permitted(complete_history,manipulation,!receipt.is_null()), + "resume requires a durable receipt or complete history proving no manipulation dispatch"); + const auto receipts=task_receipts(pending->trace.task_id); + require(pending->resolution!="replan_task"||(safe_retry&&receipts.empty()),"replan requires no manipulation or deliveries"); + json report={{"verified",true},{"stop_confirmed",true},{"holding_state","EMPTY"},{"run_id",pending->trace.run_id}, + {"evidence_ref",pending->evidence_ref},{"verification_ref",e.evidence_ref},{"safe_to_retry",safe_retry},{"receipts",receipts},{"observed_at_ns",ns(e.context.observed_at)}}; + append_receipt(task_journal_,{{"task_id",pending->trace.task_id},{"run_id",pending->trace.run_id},{"state","RECONCILING"},{"recovery_verification",report}}); + for(const auto& record:records)if(record.state!=GoalState::TERMINAL) + require(registry_->reconcile(record.request.goal_id,record.request.trace,StopState::CONFIRMED,true,e.evidence_ref),"goal reconciliation failed"); + require(!registry_->robot_locked(robot_id_),"unresolved registry state remains"); + append_receipt(task_journal_,{{"task_id",pending->trace.task_id},{"run_id",pending->trace.run_id},{"state","RELEASED"},{"status","RECONCILED"},{"recovery_resolution",pending->resolution}}); + unresolved_runs_.erase(pending->trace.run_id);faulted_=!unresolved_runs_.empty();recovery_.reset(); + recovery_reply(pending->header,true,"","independent physical reconciliation completed",report); + }catch(const std::exception& e){reject_recovery(pending,e.what());} + } void accept(std::shared_ptr handle) { active_=std::move(handle);reserved_=false;cancel_requested_=false;halting_=false;timed_out_=false; quantity_=0;sequence_=0;active_receipt_key_.clear();detail_.clear();requested_status_.clear(); try { const auto goal=active_->get_goal();auto task=admit(strict_json(goal->approved_plan_json),strict_json(goal->context_json),goal->trace,trusted_,robot_id_); - const auto epoch=driver_->geometry_epoch();require(epoch.has_value(),"fresh RobotState geometry epoch required"); active_receipt_key_=json::array({task.trace.task_id,task.item_index}).dump(); + append_receipt(task_journal_,{{"schema_version",2},{"robot_id",robot_id_},{"task_id",task.trace.task_id},{"run_id",task.trace.run_id},{"state","ACTIVE"}, + {"trace",trace_json(goal->trace)},{"approved_plan_json",goal->approved_plan_json},{"context_json",goal->context_json}, + {"timeout",{{"sec",goal->timeout.sec},{"nanosec",goal->timeout.nanosec}}},{"history_complete",true},{"dispatch_manifest_version",1},{"accepted_at_ns",now().nanoseconds()}}); + unresolved_runs_.insert(task.trace.run_id); + const auto epoch=driver_->geometry_epoch();require(epoch.has_value(),"fresh RobotState geometry epoch required"); task.initial_geometry_epoch=*epoch; task.max_reobservations=max_reobservations_; task.max_posture_adjustments=max_posture_adjustments_; - append_receipt(task_journal_,{{"task_id",task.trace.task_id},{"run_id",task.trace.run_id},{"state","ACTIVE"}}); - seen_runs_.insert(task.trace.run_id); driver_->bind_task(task,site_,trusted_.at("registry_version").get()); deadline_=SteadyClock::now()+std::chrono::seconds(goal->timeout.sec)+std::chrono::nanoseconds(goal->timeout.nanosec); context_=std::make_unique();runtime_=std::make_shared();runtime_->node=this;runtime_->admitted=true; runtime_->runner=std::make_unique(task,site_,*driver_,*registry_,*context_, [this,task](const std::string& id,unsigned item,const std::string& verification) { if(item!=task.item_index||id!=task.trace.task_id||verification.empty())return false; - if(!receipts_.count(active_receipt_key_)) { + if(receipt_for(task.trace.task_id,task.item_index).is_null()) { const auto* record=registry_->find(verification); if(!record||!record->result||!record->result->response.evidence|| !record->result->response.verified||!record->result->response.in_destination|| @@ -182,17 +383,26 @@ class ExecutorNode final:public rclcpp::Node { {"destination_ref",proof.destination_id},{"passed",proof.verified},{"empty_hand",proof.holding==Holding::EMPTY}, {"in_destination",proof.in_destination},{"valid",proof.valid}, {"observed_at_ns",proof.evidence->observed_at},{"valid_until_ns",proof.evidence->valid_until}}; - append_receipt(receipt_path_,receipt);receipts_[active_receipt_key_]=receipt; + append_receipt(receipt_path_,receipt);cache_receipt(receipt); } quantity_=1;return true; },budgets_); auto blackboard=BT::Blackboard::create();blackboard->set("runtime",runtime_); + blackboard->set("task",trace_json(goal->trace)); + blackboard->set("approved_plan",strict_json(goal->approved_plan_json)); + blackboard->set("context",strict_json(goal->context_json)); + blackboard->set("versions",json{{"registry",trusted_.at("registry_version")},{"plan",goal->trace.plan_version},{"task_revision",goal->trace.task_revision},{"geometry_epoch",*epoch}}); + blackboard->set("execution",json{{"stage","Accepted"},{"completed_quantity",0}}); + blackboard->set("evidence",json::object()); tree_.emplace(factory_.createTree("TaskRoot",blackboard)); }catch(const std::exception& e){detail_=e.what();finish("INTERVENTION_REQUIRED",false);} } void begin_halt(const std::string& status) { - if(halting_)return;halting_=true;requested_status_=status; - if(tree_)tree_->haltTree();if(runtime_&&runtime_->runner)runtime_->runner->halt(SteadyClock::now()); + if(halting_)return; + halting_=true; + requested_status_=status; + if(tree_)tree_->haltTree(); + if(runtime_&&runtime_->runner)runtime_->runner->halt(SteadyClock::now()); } void finish(const std::string& requested_status,bool registry_stop_confirmed) { if(!active_)return; @@ -202,25 +412,54 @@ class ExecutorNode final:public rclcpp::Node { state=driver_->safety(config.at("target_id").get()); }catch(const std::exception& e){detail_=std::string("invalid final context: ")+e.what();} const bool stop_confirmed=registry_stop_confirmed&&state.stationary; - const bool physical_release=stop_confirmed&&state.safe&&state.holding==Holding::EMPTY&& + bool physical_release=stop_confirmed&&state.safe&&state.holding==Holding::EMPTY&& runtime_&&runtime_->runner&&runtime_->runner->empty_verified(now().nanoseconds()); std::string status=physical_release?requested_status:"INTERVENTION_REQUIRED"; if(!physical_release&&detail_.empty())detail_="fresh final empty-hand/stationary/safe evidence missing"; const auto id=active_->get_goal()->trace.task_id; json evidence={{"status",status},{"stop_confirmed",stop_confirmed},{"completed_quantity",quantity_}, - {"detail",detail_},{"goal_uuid_mappings",driver_->mappings()}, + {"detail",detail_}, {"safe_to_release",physical_release&&status!="INTERVENTION_REQUIRED"}, {"current_empty_hand",state.holding==Holding::EMPTY},{"current_stationary",state.stationary}}; - if(receipts_.count(active_receipt_key_)) { - const auto& receipt=receipts_.at(active_receipt_key_);evidence["delivery"]=receipt; + try { + evidence["goal_uuid_mappings"]=driver_->mappings(active_->get_goal()->trace.run_id); + const auto receipt=receipt_for(id,strict_json(active_->get_goal()->context_json).value("item_index",0u)); + if(!receipt.is_null()) { + evidence["delivery"]=receipt; for(const auto* key:{"evidence_id","target_ref","destination_ref","passed","empty_hand","in_destination","valid"}) if(receipt.contains(key))evidence[key]=receipt.at(key); } else {evidence["empty_hand"]=physical_release;evidence["valid"]=physical_release;} - if(runtime_&&runtime_->clarification)evidence["needs_clarification"]=true; + evidence["goal_records"]=json::array();bool manipulation_dispatched=false,localization_ambiguous=false; + const auto task_records=registry_->task_records(active_->get_goal()->trace.run_id); + for(const auto& record:task_records) { + manipulation_dispatched=manipulation_dispatched||record.request.skill==Skill::PICK||record.request.skill==Skill::PLACE; + json row={{"goal_id",record.request.goal_id},{"state",static_cast(record.state)}, + {"request_type",record.request.wire_request_type},{"snapshot_ref",journal_dir_+"/goal_registry.log#"+record.request.goal_id}, + {"request_snapshot_present",!record.request.wire_request_snapshot.empty()},{"feedback_snapshot_present",!record.feedback_snapshot.empty()}}; + if(record.result){ + row.update({{"error_code",record.result->error_code},{"execution_record_ref",record.result->execution_record_ref},{"detail",record.result->detail}, + {"result_type",record.result->wire_result_type},{"result_snapshot_present",!record.result->wire_result_snapshot.empty()}}); + localization_ambiguous=localization_ambiguous||((record.request.skill==Skill::LOCATE_SHELF_COLUMN||record.request.skill==Skill::LOCALIZE_TARGET)&& + (record.result->error_code=="NOT_FOUND"||record.result->error_code=="AMBIGUOUS")); + } + evidence["goal_records"].push_back(row); + } + const bool safe_retry=complete_dispatch_history(task_record(active_->get_goal()->trace.run_id),task_records)&&!manipulation_dispatched; + evidence["safe_to_retry"]=safe_retry; + if(localization_ambiguous&&safe_retry&&physical_release&&quantity_==0) { + evidence["needs_clarification"]=true; + evidence["questions"]=json::array({"Please confirm the target item and its source shelf after localization could not identify it uniquely."}); + } + }catch(const std::exception& e) { + faulted_=true;physical_release=false;status="INTERVENTION_REQUIRED"; + evidence["status"]=status;evidence["safe_to_release"]=false;evidence["safe_to_retry"]=false; + evidence["needs_clarification"]=false;evidence["history_error"]=e.what(); + } const bool release=physical_release&&status!="INTERVENTION_REQUIRED"; try { append_receipt(task_journal_,{{"task_id",id},{"run_id",active_->get_goal()->trace.run_id}, - {"state",release?"RELEASED":"QUARANTINED"},{"status",status}}); + {"state",release?"RELEASED":"QUARANTINED"},{"status",status},{"final_evidence",evidence}}); + if(release)unresolved_runs_.erase(active_->get_goal()->trace.run_id); }catch(const std::exception& e) { faulted_=true;status="INTERVENTION_REQUIRED";evidence["journal_error"]=e.what(); evidence["status"]=status;evidence["safe_to_release"]=false; @@ -229,7 +468,7 @@ class ExecutorNode final:public rclcpp::Node { auto result=std::make_shared(); result->result.status=status=="SUCCEEDED"?0:status=="CANCELED"?2:timed_out_?3:1; result->result.stop_state=stop_confirmed?1:0;result->result.message=detail_; - result->result.error_code=status;result->completed_quantity=quantity_; + result->result.error_code=runtime_&&runtime_->runner&&!runtime_->runner->error_code().empty()?runtime_->runner->error_code():status;result->completed_quantity=quantity_; if(stop_confirmed){result->result.stopped_at=now();result->result.stop_evidence_ref="registry/"+active_->get_goal()->trace.run_id;} result->evidence_json=evidence.dump(); if(status=="SUCCEEDED")active_->succeed(result); @@ -255,6 +494,7 @@ class ExecutorNode final:public rclcpp::Node { const auto steady=SteadyClock::now(); try { registry_->pump(steady);if(++tick_count_%20==0)publish_registry(); + if(recovery_&&steady>=recovery_->deadline)reject_recovery(recovery_,"independent verification timeout"); if(!active_)return; if(cancel_requested_)begin_halt("CANCELED"); if(steady>=deadline_&&!halting_){timed_out_=true;detail_="task execution deadline";begin_halt("FAILED");} @@ -262,6 +502,11 @@ class ExecutorNode final:public rclcpp::Node { if(!halting_) { const auto config=strict_json(active_->get_goal()->context_json); runtime_->runner->update_safety(driver_->safety(config.at("target_id").get())); + auto blackboard=tree_->rootBlackboard(); + const auto safety=driver_->safety(config.at("target_id").get()); + blackboard->set("execution",json{{"stage",runtime_->stage},{"completed_quantity",quantity_},{"cancel_requested",cancel_requested_},{"registry_locked",registry_->robot_locked(robot_id_)}}); + blackboard->set("evidence",json{{"safe",safety.safe},{"stationary",safety.stationary},{"holding",static_cast(safety.holding)}, + {"observed_at_ns",safety.observed_at},{"valid_until_ns",safety.valid_until}}); const auto state=tree_->tickOnce(); if(state==BT::NodeStatus::SUCCESS){detail_=runtime_->runner->detail();begin_halt(quantity_==1?"SUCCEEDED":"FAILED");} else if(state==BT::NodeStatus::FAILURE) { @@ -289,6 +534,7 @@ class ExecutorNode final:public rclcpp::Node { } }catch(const std::exception& e) { faulted_=true;detail_=std::string("executor fault: ")+e.what(); + if(recovery_)reject_recovery(recovery_,detail_); try{begin_halt("INTERVENTION_REQUIRED");}catch(...){} if(active_)finish("INTERVENTION_REQUIRED",false); RCLCPP_ERROR(get_logger(),"%s",detail_.c_str()); diff --git a/ros2/bt_executor/src/ros_driver.cpp b/ros2/bt_executor/src/ros_driver.cpp index 4fef2c5..d856459 100644 --- a/ros2/bt_executor/src/ros_driver.cpp +++ b/ros2/bt_executor/src/ros_driver.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,7 @@ RosDriver::RosDriver(rclcpp::Node& n, std::string robot_id, std::string journal, auto value=json::parse(line);const auto id=value.at("client_goal_id").get(); if(id.empty()||value.at("ros_goal_uuid").get().size()!=32)throw std::runtime_error("corrupt UUID journal"); mappings_[id]=std::move(value); + prune_mappings(); } navigate_=rclcpp_action::create_client(&n,n.declare_parameter("navigate_action","skills/navigate")); manipulate_=rclcpp_action::create_client(&n,n.declare_parameter("execute_manipulation_action","skills/execute_manipulation")); @@ -139,15 +141,27 @@ void RosDriver::record_mapping(const GoalRequest& r,const rclcpp_action::GoalUUI json m={{"client_goal_id",r.goal_id},{"ros_goal_uuid",hex.str()},{"task_id",r.trace.task_id}, {"run_id",r.trace.run_id},{"task_revision",r.trace.task_revision},{"plan_version",r.trace.plan_version}, {"execution_generation",r.trace.execution_generation},{"subtask_id",r.trace.subtask_id},{"attempt",r.trace.attempt}}; - durable_append(uuid_journal_,m.dump()+"\n");mappings_[r.goal_id]=m; + durable_append(uuid_journal_,m.dump()+"\n");mappings_[r.goal_id]=m;prune_mappings(r.goal_id); } json RosDriver::mappings()const {json j=json::array();for(const auto& kv:mappings_)j.push_back(kv.second);return j;} +void RosDriver::prune_mappings(const std::string& keep) { + for(auto it=mappings_.begin();mappings_.size()>256&&it!=mappings_.end();) { + if(it->first!=keep&&!cancelers_.count(it->first)&&!cancel_intents_.count(it->first))it=mappings_.erase(it);else ++it; + } +} +json RosDriver::mappings(const std::string& run_id)const { + std::ifstream input(uuid_journal_);if(!input&&std::filesystem::exists(uuid_journal_))throw std::runtime_error("UUID journal unreadable"); + std::map selected;std::string line; + while(std::getline(input,line)){if(line.empty())continue;auto entry=json::parse(line);if(entry.at("run_id")==run_id){const auto id=entry.at("client_goal_id").get();selected[id]=std::move(entry);}} + if(input.bad())throw std::runtime_error("UUID journal read failed"); + json out=json::array();for(const auto& item:selected)out.push_back(item.second);return out; +} void RosDriver::cancel(const std::string& id) { cancel_intents_.insert(id);auto it=cancelers_.find(id);if(it!=cancelers_.end())it->second(); } std::vector RosDriver::drain_events(){std::vector v;v.swap(events_);return v;} ExecutionResult RosDriver::execution(const iface::msg::ExecutionResult& m,const GoalRequest& request)const { - ExecutionResult r;r.detail=m.error_code+": "+m.message; + ExecutionResult r;r.detail=m.error_code+": "+m.message;r.error_code=m.error_code; switch(m.status){case 0:r.code=ResultCode::COMPLETED;break;case 1:r.code=ResultCode::FAILED;break; case 2:r.code=ResultCode::CANCELED;break;case 3:r.code=ResultCode::TIMED_OUT;break; case 4:r.code=ResultCode::REJECTED;break;default:return r;} @@ -191,7 +205,8 @@ void RosDriver::send(const GoalRequest& r) { Semantic::Goal g;g.trace=trace_msg(r.trace);g.kind=r.navigation_kind;g.reference=r.navigation_ref;g.shelf_id=r.shelf;g.side_id=r.side;g.column_id=r.column;g.tier_id=r.tier;g.registry_version=registry_version_;g.position_tolerance=r.position_tolerance_m;g.orientation_tolerance=r.orientation_tolerance_rad;g.timeout=timeout(); send_typed(semantic_,g,r,6,[this,r](const Semantic::Result& m,auto){ auto out=execution(m.result,r);out.response.valid=m.pose_valid&&m.errors_valid&&std::isfinite(m.final_position_error)&&std::isfinite(m.final_orientation_error)&&m.final_position_error>=0&&m.final_position_error<=r.position_tolerance_m&&std::abs(m.final_orientation_error)<=r.orientation_tolerance_rad; - if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose);out.response.base_stopped=out.stop==StopState::CONFIRMED;return out; + if(m.pose_valid)out.response.final_pose=pose_core(m.final_pose); + out.response.base_stopped=out.stop==StopState::CONFIRMED;return out; });break; } Navigate::Goal g;g.trace=trace_msg(r.trace);g.target_pose=pose_msg(*r.registered_pose,now); @@ -210,7 +225,7 @@ void RosDriver::send(const GoalRequest& r) { if(r.skill==Skill::PLACE){g.destination.region_ref=r.destination_id;g.destination.description=r.destination_id;} g.timeout=timeout(); send_typed(manipulate_,g,r,5,[this,r](const Manipulate::Result& m,auto){ - auto out=execution(m.result,r);out.response.valid=!m.execution_record_ref.empty(); + auto out=execution(m.result,r);out.response.valid=!m.execution_record_ref.empty();out.execution_record_ref=m.execution_record_ref; out.response.base_stopped=out.stop==StopState::CONFIRMED;return out;});break; } case Skill::LOCATE_SHELF_COLUMN: { @@ -223,7 +238,7 @@ void RosDriver::send(const GoalRequest& r) { !m.observation_id.empty()&&!m.record_ref.empty()&&!m.shelf_id.empty()&&!m.side_id.empty()&&!m.column_id.empty()&& fresh(ns(m.observed_at),ns(m.observed_at)+observation_lifetime_ns_,r.capture_after); if(ok)shelf_bindings_[r.trace.run_id]={{"shelf",m.shelf_id},{"side",m.side_id},{"column",m.column_id},{"tier",m.tier_id},{"record",m.record_ref}}; - return readonly_result(code,ok,out);});break; + auto result=readonly_result(code,ok,out);result.error_code=m.status==Locate::Result::NOT_FOUND?"NOT_FOUND":m.status==Locate::Result::AMBIGUOUS?"AMBIGUOUS":m.error_code;result.detail=m.error_code+": "+m.message;result.execution_record_ref=m.record_ref;return result;});break; } case Skill::LOCALIZE_TARGET: { Localize::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id; @@ -247,7 +262,7 @@ void RosDriver::send(const GoalRequest& r) { msg.target.object_ref=m.target_ref;msg.target.description=r.target_id;msg.target_point=m.target_point; msg.grasp_point=m.grasp_point;msg.grasp_point_valid=m.grasp_point_valid;msg.grasp_region_ref=m.grasp_region_ref; msg.geometry_valid=true;msg.calibration_id=m.calibration_id;msg.shelf_id=r.shelf;target_pub_->publish(msg);} - return readonly_result(code,ok,out);});break; + auto result=readonly_result(code,ok,out);result.error_code=m.status==Localize::Result::NOT_FOUND?"NOT_FOUND":m.status==Localize::Result::AMBIGUOUS?"AMBIGUOUS":m.error_code;result.detail=m.error_code+": "+m.message;result.execution_record_ref=m.record_ref;return result;});break; } case Skill::EVALUATE_GRASP: { if(!r.target||!robot_state_)throw std::runtime_error("assess missing binding/state"); @@ -263,7 +278,7 @@ void RosDriver::send(const GoalRequest& r) { switch(m.decision){case 0:out.admission=Admission::DIRECT;break;case 1:out.admission=Admission::ADJUST_POSTURE;break; case 2:out.admission=Admission::NOT_REACHABLE;break;default:out.admission=Admission::UNKNOWN;} bool ok=m.decision<=3&&m.geometry_epoch==r.geometry_epoch&&!m.evidence_ref.empty(); - return readonly_result(code,ok,out);});break; + auto result=readonly_result(code,ok,out);result.error_code=m.error_code;result.detail=m.message;result.execution_record_ref=m.evidence_ref;return result;});break; } case Skill::ADJUST_POSTURE:case Skill::TRANSPORT_POSTURE: { Posture::Goal g;g.trace=trace_msg(r.trace);g.posture_id=r.posture_id; @@ -299,7 +314,7 @@ void RosDriver::send(const GoalRequest& r) { else if(r.skill==Skill::VERIFY_EMPTY)out.verified=out.verified&&e.hand_empty_valid&&e.hand_empty&&out.holding==Holding::EMPTY; else out.verified=out.verified&&e.destination_ref==r.destination_id&&e.hand_empty_valid&&e.hand_empty&& out.holding==Holding::EMPTY&&out.in_destination; - return readonly_result(code,ok&&e.status<=2,out);});break; + auto result=readonly_result(code,ok&&e.status<=2,out);result.error_code=e.error_code;result.detail=e.message;result.execution_record_ref=e.evidence_ref;return result;});break; } case Skill::CHECK_FREE_SPACE: { Space::Goal g;g.task_id=r.trace.task_id;g.subtask_id=r.trace.subtask_id;g.destination_ref=r.destination_id; @@ -319,7 +334,7 @@ void RosDriver::send(const GoalRequest& r) { msg.placement_region_ref=m.placement_region_ref;msg.placement_point=m.placement_point; msg.placement_point_valid=m.placement_point_valid;msg.placement_pose=m.placement_pose; msg.placement_pose_valid=m.placement_pose_valid;msg.geometry_valid=true;placement_pub_->publish(msg);} - return readonly_result(code,ok,out);});break; + auto result=readonly_result(code,ok,out);result.error_code=m.error_code;result.detail=m.message;result.execution_record_ref=m.record_ref;return result;});break; } } } diff --git a/ros2/bt_executor/tools/build_humble.sh b/ros2/bt_executor/tools/build_humble.sh index fc462d3..9459b1a 100755 --- a/ros2/bt_executor/tools/build_humble.sh +++ b/ros2/bt_executor/tools/build_humble.sh @@ -4,7 +4,10 @@ if [[ ! -f /opt/ros/humble/setup.bash ]]; then echo 'NOT RUN: ROS2 Humble is not installed; no ROS compilation claim.' >&2 exit 2 fi +# Humble's generated environment hooks inspect optional unset variables. +set +u source /opt/ros/humble/setup.bash +set -u command -v colcon >/dev/null || { echo 'colcon is required' >&2; exit 2; } bt_repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)" cd "$bt_repo_root" diff --git a/ros2/bt_executor/tools/test_recovery_policy.cpp b/ros2/bt_executor/tools/test_recovery_policy.cpp new file mode 100644 index 0000000..e106938 --- /dev/null +++ b/ros2/bt_executor/tools/test_recovery_policy.cpp @@ -0,0 +1,16 @@ +#include +#include +#include +int main() { + using namespace bt_executor; + assert(!recovery_authorized("", "")); + assert(!recovery_authorized("short", "short")); + assert(recovery_authorized("sixteen-byte-key!", "sixteen-byte-key!")); + assert(!recovery_authorized("sixteen-byte-key!", "sixteen-byte-key?")); + assert(!recovery_authorized("sixteen-byte-key!", "sixteen-byte-key!x")); + assert(!recovery_authorized(std::string(1025,'x'),std::string(1025,'x'))); + assert(!recovery_resume_permitted(false,false,false)); + assert(!recovery_resume_permitted(true,true,false)); + assert(recovery_resume_permitted(true,false,false)); + assert(recovery_resume_permitted(false,true,true)); +} diff --git a/ros2/bt_executor/tools/test_ros_backend.py b/ros2/bt_executor/tools/test_ros_backend.py index cd38607..0b54755 100644 --- a/ros2/bt_executor/tools/test_ros_backend.py +++ b/ros2/bt_executor/tools/test_ros_backend.py @@ -6,6 +6,7 @@ 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 @@ -112,6 +113,105 @@ class BackendLifecycle(unittest.TestCase): 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() diff --git a/ros2/bt_mock_servers/package.xml b/ros2/bt_mock_servers/package.xml index cee81d6..4371701 100644 --- a/ros2/bt_mock_servers/package.xml +++ b/ros2/bt_mock_servers/package.xml @@ -1,4 +1,4 @@ - + bt_mock_servers 1.2.0 diff --git a/ros2/bt_skill_interfaces/CMakeLists.txt b/ros2/bt_skill_interfaces/CMakeLists.txt index a8bd431..27af1b0 100644 --- a/ros2/bt_skill_interfaces/CMakeLists.txt +++ b/ros2/bt_skill_interfaces/CMakeLists.txt @@ -33,6 +33,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/ExecuteTask.action" "srv/GetRobotState.srv" "srv/ReconcileGoal.srv" + "srv/ReconcileTask.srv" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/ros2/bt_skill_interfaces/package.xml b/ros2/bt_skill_interfaces/package.xml index 107cae4..bcc3349 100644 --- a/ros2/bt_skill_interfaces/package.xml +++ b/ros2/bt_skill_interfaces/package.xml @@ -1,4 +1,4 @@ - + bt_skill_interfaces 1.2.0 diff --git a/ros2/bt_skill_interfaces/srv/ReconcileTask.srv b/ros2/bt_skill_interfaces/srv/ReconcileTask.srv new file mode 100644 index 0000000..4223d83 --- /dev/null +++ b/ros2/bt_skill_interfaces/srv/ReconcileTask.srv @@ -0,0 +1,11 @@ +# Task recovery requires independent physical verification by the executor. +bt_skill_interfaces/TaskTrace trace +string operator_id +string authorization +string evidence_ref +string resolution +--- +bool accepted +string error_code +string message +string state_json diff --git a/ros2/robobrain_services/package.xml b/ros2/robobrain_services/package.xml index 409eb8c..9c75819 100644 --- a/ros2/robobrain_services/package.xml +++ b/ros2/robobrain_services/package.xml @@ -1,2 +1,2 @@ - + robobrain_services1.2.0RoboBrain planning/spatial and RoboDopamine advisory serviceswangfeiyuProprietaryament_pythonrclpystd_msgsbt_skill_interfacesament_python diff --git a/ros2/robobrain_services/robobrain_services/nodes.py b/ros2/robobrain_services/robobrain_services/nodes.py index 7dee40d..7a95048 100644 --- a/ros2/robobrain_services/robobrain_services/nodes.py +++ b/ros2/robobrain_services/robobrain_services/nodes.py @@ -10,6 +10,32 @@ from robot_robobrain.observations import Observation,ObservationCache def ns(t):return t.sec*1_000_000_000+t.nanosec def assign_time(t,n):t.sec=int(n)//1_000_000_000;t.nanosec=int(n)%1_000_000_000 +def request_snapshot(value): + """Keep malformed JSON strings intact when recording a failed ROS request.""" + if value is None or isinstance(value,(str,int,float,bool)): + return value + if isinstance(value,dict): + return {str(key):request_snapshot(item) for key,item in value.items()} + if isinstance(value,(list,tuple)): + return [request_snapshot(item) for item in value] + fields=getattr(value,'get_fields_and_field_types',None) + if callable(fields): + return {key:request_snapshot(getattr(value,key)) for key in fields()} + if hasattr(value,'__dict__'): + return request_snapshot(vars(value)) + return repr(value) + +def guarded_work(service,kind,request,operation): + """Always return a terminal failure, including when the archive is unavailable.""" + try:return operation() + except Exception as ex: + result=dict(status='FAILED',state='UNKNOWN',error_code='SERVICE_ERROR',message=str(ex)) + try:result['record_ref']=service._record(kind,request_snapshot(request),'',result) + except Exception as archive_error: + result['record_ref']='' + result['message']+='; diagnostic archive unavailable: '+str(archive_error) + return result + def perception_goal(g,kind): q=dict(task_id=g.task_id,subtask_id=g.subtask_id,target_ref=g.target_ref, target_description=g.target_description,capture_after=ns(g.capture_after), @@ -84,8 +110,7 @@ def run_node(dense=False): def execute(self,h,action,kind): cancel=threading.Event();completed=queue.Queue(maxsize=1);deadline=time.monotonic()+h.request.timeout.sec+h.request.timeout.nanosec/1e9 def worker(): - try:completed.put(self.work(h.request,kind,cancel)) - except Exception as ex:completed.put(dict(status='FAILED',state='UNKNOWN',error_code='SERVICE_ERROR',message=str(ex))) + completed.put(guarded_work(self.service,kind,h.request,lambda:self.work(h.request,kind,cancel))) thread=threading.Thread(target=worker,daemon=True);thread.start();sequence=0;expired=False while thread.is_alive(): if h.is_cancel_requested or time.monotonic()>=deadline:cancel.set();expired=time.monotonic()>=deadline @@ -105,6 +130,7 @@ def run_node(dense=False): r.confidence=float(data.get('confidence',0));assign_time(r.observed_at,data.get('observed_at',0)) else: r.target_ref=h.request.target_ref;r.geometry_valid=False;r.grasp_point_valid=False;r.position_error_bound_valid=False;r.measurement_source=1;r.quality_code=data.get('quality_code','INVALID');r.observation_id=data.get('observation_id','');r.calibration_id=data.get('calibration_id','');r.geometry_epoch=data.get('geometry_epoch',0) + assign_time(r.rgb_stamp,data.get('observed_at',0)) if 'target_point' in data: p=data['target_point'];r.target_point.header.frame_id=p['frame_id'];assign_time(r.target_point.header.stamp,p['stamp_ns']);assign_time(r.rgb_stamp,p['stamp_ns']);r.target_point.point.x,r.target_point.point.y,r.target_point.point.z=map(float,p['point']) if h.is_cancel_requested:h.canceled() diff --git a/tests/helpers/native_coordinator_regression.py b/tests/helpers/native_coordinator_regression.py new file mode 100644 index 0000000..e6e5817 --- /dev/null +++ b/tests/helpers/native_coordinator_regression.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +"""ROS2 Coordinator -> PlanTask -> native BT -> receipt recovery regression. + +Run in a built Humble overlay. Every endpoint is under /sim/robot_01 and all +physical skills are fixtures; this is orchestration evidence, not robot/model +acceptance. The second run drops only the already-decoded execution result at +the coordinator event boundary, then reopens the same durable coordinator DB. +""" +import argparse +import copy +import json +import os +from pathlib import Path +import signal +import subprocess +import sys +import tempfile +import threading +import time + +from native_workflow_regression import eventually, journal_records + + +def run(root, binary, output): + import rclpy + from rclpy.executors import MultiThreadedExecutor + from rclpy.node import Node + from robot_bt_coordinator.plan_v2 import make_plan + from robot_bt_coordinator.ros_backend import RosBackend + from robot_bt_coordinator.service import Coordinator + from bt_mock_servers.mock_skills import MockSkills, ACTION_TYPES + + site_path = root/'config/sim_site_object_table.json' + site = json.loads(site_path.read_text()) + instruction = '把货架A的一瓶水放到周转箱A' + slots = {'items':[{'target_name':'water','quantity':1,'source_location':'shelf_A'}], 'destination':'tote_A'} + plan = make_plan(instruction, slots, 'OBJECT_TABLE') + scenarios = { + 'plan_task':[{'kind':'normal', 'plan':plan}], + 'verify_state':[{'kind':'passed'}], + 'navigate_semantic':[{'kind':'normal','final_pose':site['locations'][name]} + for name in ['shelf_A_stop','tote_A_stop']*2], + } + actions = [name for name in ACTION_TYPES if name != 'execute_task'] + rclpy.init(args=['--ros-args','-p','initial_holding_state:=EMPTY', + '-p','scenarios_json:='+json.dumps(json.dumps(scenarios)), + '-p','enabled_actions:='+json.dumps(actions), + '-p','enabled_topics:='+json.dumps(['robot_state','safety_state','visual_observation','dense_progress'])]) + server = MockSkills() + bridge = Node('native_coordinator_regression',namespace='/sim/robot_01') + executor = MultiThreadedExecutor(num_threads=8) + executor.add_node(server);executor.add_node(bridge) + thread = threading.Thread(target=executor.spin,daemon=True);thread.start() + token = 'native-coordinator-test-recovery-token' + operator = 'native-coordinator-regression' + config = {'planning_context':site, 'planning_timeout':10, 'execution_timeout':30, + 'recovery_token':token, 'recovery_operator_id':operator, 'recovery_timeout':10} + report = {'scope':'native ROS2 coordinator/executor with simulated physical skills','route':'OBJECT_TABLE'} + coordinator = None + process = None + log = (output/'native-coordinator.log').open('w') + + class DropFinal(RosBackend): + """Fault injection after genuine ROS deserialization, before DB commit.""" + def __init__(self): + super().__init__(node=bridge,config=config) + self.drop_task_id = None + self.dropped = [] + def poll(self): + events = super().poll() + kept = [] + for event in events: + if event.get('type') == 'execution_result' and event.get('task_id') == self.drop_task_id: + self.dropped.append(copy.deepcopy(event)) + else: + kept.append(event) + return kept + + def counts(): + with server.lock: + return {name:server.counts[name] for name in ('plan_task','navigate_semantic','execute_manipulation','execute_posture')} + + with tempfile.TemporaryDirectory(prefix='native-coordinator-regression-') as state: + state = Path(state) + journal = state/'executor';journal.mkdir() + database = state/'coordinator.sqlite' + try: + args = [str(binary),'--ros-args','-r','__ns:=/sim/robot_01', + '-p','robot_id:=robot_01','-p','allowed_robots:=[robot_01]', + '-p','site_config_file:='+str(site_path),'-p','journal_directory:='+str(journal), + '-p','execution_enabled:=true','-p','skill_timeout_ms:=5000', + '-p','recovery_token:='+token,'-p','recovery_operator_id:='+operator] + process = subprocess.Popen(args,stdout=log,stderr=subprocess.STDOUT,start_new_session=True) + backend = DropFinal() + assert backend._planner.wait_for_server(timeout_sec=10), 'Planner Action not discovered' + assert backend._execution.wait_for_server(timeout_sec=10), 'native ExecuteTask Action not discovered' + time.sleep(.6) + assert process.poll() is None, 'native executor failed at startup' + coordinator = Coordinator(str(database),backend,['robot_01'],site=site) + + def submit(request_id): + return coordinator.submit({'client_request_id':request_id,'robot_id':'robot_01', + 'instruction':instruction,'known_info':copy.deepcopy(slots)}) + + def pump_until(condition, seconds=45): + def check(): + assert process.poll() is None, 'native executor exited' + coordinator.tick() + return condition() + eventually(check,seconds) + + first = submit('normal-result') + pump_until(lambda:coordinator.get(first['task_id'])['status'] in ('SUCCEEDED','FAILED','INTERVENTION_REQUIRED','NEEDS_CLARIFICATION')) + first = coordinator.get(first['task_id']) + assert first['status'] == 'SUCCEEDED', first + assert first['completed_quantity'] == 1 and first['stop_confirmed'] is True + assert counts()['execute_manipulation'] == 2 + events = coordinator.events(first['task_id'],limit=500) + planning = [event for event in events if event['kind']=='planning_result'] + assert planning and planning[-1]['payload']['planning_record_ref'].startswith('sim://plan_task/') + assert any(event['kind']=='delivery_committed' for event in events) + report['normal'] = {'task_id':first['task_id'],'run_id':first['run_id'], + 'status':first['status'],'completed_quantity':first['completed_quantity'], + 'planning_record_ref':planning[-1]['payload']['planning_record_ref'], + 'counts':counts()} + + second = submit('lost-final-result') + backend.drop_task_id = second['task_id'] + pump_until(lambda:bool(backend.dropped)) + assert len(backend.dropped) == 1 + dropped = backend.dropped[0] + assert dropped['status']=='SUCCEEDED' and dropped['completed_quantity']==1, dropped + assert dropped['stop_confirmed'] is True and dropped['evidence']['safe_to_release'] is True + before_restart = coordinator.get(second['task_id']) + assert before_restart['status']=='EXECUTING' and before_restart['completed_quantity']==0 + assert coordinator.store.db.execute('SELECT COUNT(*) FROM deliveries WHERE task_id=?',(second['task_id'],)).fetchone()[0]==0 + native_receipts = [json.loads(line) for line in (journal/'deliveries.jsonl').read_text().splitlines()] + assert len([row for row in native_receipts if row['task_id']==second['task_id']])==1 + before_recovery = counts() + assert before_recovery['execute_manipulation']==4 + coordinator.close();coordinator=None + + backend = RosBackend(node=bridge,config=config) + coordinator = Coordinator(str(database),backend,['robot_01'],site=site) + restarted = coordinator.get(second['task_id']) + assert restarted['status']=='INTERVENTION_REQUIRED' + assert restarted['run_id']==before_restart['run_id'] + assert backend._recovery.wait_for_service(timeout_sec=10), 'native recovery service not discovered' + recovered = coordinator.intervene(second['task_id'],{ + 'run_id':restarted['run_id'], 'evidence_ref':'regression/operator-observed-stop', 'resolution':'resume_task'}) + assert recovered['status']=='SUCCEEDED' and recovered['completed_quantity']==1, recovered + for _ in range(10): + coordinator.tick();time.sleep(.03) + assert counts()==before_recovery, 'receipt recovery redispatched planning or robot motion' + deliveries = coordinator.store.db.execute('SELECT * FROM deliveries WHERE task_id=?',(second['task_id'],)).fetchall() + assert len(deliveries)==1 + evidence = json.loads(deliveries[0]['evidence']) + assert evidence['evidence_id']==dropped['evidence']['evidence_id'] + assert all(evidence[key] is True for key in ('passed','empty_hand','in_destination','valid')) + rows = journal_records(journal/'goal_registry.log') + second_motion = [row for row in rows if row['run_id']==restarted['run_id'] and 'ExecuteManipulation' in row['type']] + assert len(second_motion)==2, 'duplicate pick/place goals in recovered run' + report['lost_result_recovery'] = {'task_id':second['task_id'],'run_id':restarted['run_id'], + 'status_before_restart':before_restart['status'],'status_after_restart':restarted['status'], + 'status_after_recovery':recovered['status'],'completed_quantity':recovered['completed_quantity'], + 'ledger_rows':len(deliveries),'manipulation_goal_count':len(second_motion), + 'counts_before':before_recovery,'counts_after':counts(),'receipt_evidence':evidence} + report['passed']=True + except Exception as exc: + report['passed']=False;report['failure']=repr(exc) + finally: + if coordinator is not None: + coordinator.close() + if process is not None and process.poll() is None: + os.killpg(process.pid,signal.SIGINT) + try:process.wait(timeout=5) + except subprocess.TimeoutExpired: + os.killpg(process.pid,signal.SIGKILL);process.wait() + for path in journal.glob('*'): + if path.is_file():(output/('coordinator-'+path.name)).write_bytes(path.read_bytes()) + if database.is_file():(output/'native-coordinator.sqlite').write_bytes(database.read_bytes()) + executor.shutdown(timeout_sec=2);thread.join(timeout=2) + server.destroy_node();bridge.destroy_node();rclpy.shutdown();log.close() + return report + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--repo-root',type=Path,default=Path(__file__).resolve().parents[2]) + parser.add_argument('--executor',type=Path) + parser.add_argument('--output-dir',type=Path,required=True) + args = parser.parse_args() + root = args.repo_root.resolve() + sys.path[:0] = [str(root/'coordinator'),str(root/'ros2/bt_mock_servers')] + binary = (args.executor or root/'install/bt_executor/lib/bt_executor/bt_executor_node').resolve() + args.output_dir.mkdir(parents=True,exist_ok=True) + report = run(root,binary,args.output_dir) + (args.output_dir/'native-coordinator-regression.json').write_text(json.dumps(report,indent=2)) + print(json.dumps({key:report[key] for key in ('passed','failure') if key in report}),flush=True) + if not report['passed']:raise SystemExit(1) + + +if __name__=='__main__':main() diff --git a/tests/helpers/native_workflow_regression.py b/tests/helpers/native_workflow_regression.py new file mode 100644 index 0000000..69111b5 --- /dev/null +++ b/tests/helpers/native_workflow_regression.py @@ -0,0 +1,326 @@ +#!/usr/bin/env python3 +"""Actual ROS2 + native executor regression; run inside a built Humble overlay. + +Uses only /sim/robot_01 fixture servers, never robot endpoints. Includes durable +CDR evidence checks, executor restart receipt recovery, and cancellation. This +is transport/orchestration evidence, not physical NAV/VLA acceptance. +""" +import argparse +import copy +import json +import os +from pathlib import Path +import shlex +import signal +import subprocess +import sys +import tempfile +import threading +import time + + +def wait(future, seconds=45): + end = time.monotonic() + seconds + while not future.done() and time.monotonic() < end: + time.sleep(0.01) + if not future.done(): + raise TimeoutError('ROS future deadline exceeded') + return future.result() + + +def eventually(predicate, seconds=8): + end = time.monotonic() + seconds + while time.monotonic() < end: + if predicate(): + return + time.sleep(0.02) + raise AssertionError('expected native state transition did not occur') + + +def journal_records(path): + """Independent decoder for durable registry v4 (does not call production).""" + latest = {} + for line in path.read_text().splitlines(): + row = shlex.split(line) + assert row[0] in ('3', '4') and len(row) == (25 if row[0] == '4' else 23), 'missing durable evidence fields' + decode = lambda index: bytes.fromhex(row[index]).decode('utf-8') + latest[row[1]] = dict(goal_id=row[1], task_id=row[3], run_id=row[5], + skill=int(row[10]), state=int(row[11]), sequence=int(row[14]), + type=decode(18), wire=decode(19), feedback=decode(20), + error_code=decode(21), execution_record_ref=decode(22), result_type=decode(23) if row[0] == '4' else '', + result_wire=decode(24) if row[0] == '4' else '') + return list(latest.values()) + + +def assert_manipulation_evidence(records, run_id, action_type, deserialize): + selected = [row for row in records if row['run_id'] == run_id and + 'ExecuteManipulation' in row['type']] + assert len(selected) == 2, 'one pick and one place wire snapshot required' + decoded = [] + for row in selected: + goal = deserialize(bytes.fromhex(row['wire']), action_type.Goal) + assert goal.skill in ('pick', 'place') + assert goal.trace.run_id == run_id and goal.trace.attempt == 1 + assert goal.target.object_ref == 'water' and goal.target.description + assert goal.instruction == goal.skill + ' registered target water' + assert goal.timeout.sec == 5 and goal.timeout.nanosec == 0 + if goal.skill == 'pick': + assert goal.destination.region_ref == goal.destination.description == '' + else: + assert goal.destination.region_ref == 'tote_A' and goal.destination.description + payload = json.loads(row['feedback']) + feedback = deserialize(bytes.fromhex(payload['cdr_hex']), action_type.Feedback) + assert feedback.sequence == row['sequence'] == payload['sequence'] > 0 + assert feedback.phase == payload['phase'] and feedback.message == payload['message'] + assert feedback.progress_valid == payload['progress_valid'] + assert feedback.message and payload['elapsed_time_ns'] >= 0 + assert row['execution_record_ref'].startswith('sim://execute_manipulation/') + assert row['state'] == 4 + assert 'ExecuteManipulation' in row['result_type'] and row['result_wire'] + result = deserialize(bytes.fromhex(row['result_wire']), action_type.Result) + assert result.result.status == 0 and result.result.stop_state == 1 + assert result.result.stop_evidence_ref and result.result.stopped_at.sec > 0 + assert result.execution_record_ref == row['execution_record_ref'] + decoded.append(dict(skill=goal.skill, sequence=feedback.sequence, + phase=feedback.phase, execution_record_ref=row['execution_record_ref'])) + assert {row['skill'] for row in decoded} == {'pick', 'place'} + return decoded + + +def run_route(root, binary, output, suffix, recovery_enabled): + import rclpy + from rclpy.action import ActionClient + from rclpy.executors import MultiThreadedExecutor + from rclpy.node import Node + from rclpy.serialization import deserialize_message + from bt_skill_interfaces.action import ExecuteTask, ExecuteManipulation + from bt_skill_interfaces.msg import RobotState + from bt_skill_interfaces.srv import ReconcileTask + from robot_bt_coordinator.plan_v2 import item_plan + from bt_mock_servers.mock_skills import MockSkills, ACTION_TYPES + + site_path = root / 'config' / ('sim_site_' + suffix + '.json') + site = json.loads(site_path.read_text()) + plan = item_plan(json.loads((root / 'config' / ('demo_plan_' + suffix + '.json')).read_text()), 0) + stops = ['shelf_A_stop', 'tote_A_stop'] if suffix == 'object_table' else ['observe_A', 'shelf_A_stop', 'tote_A_stop'] + scenarios = {'verify_state': [{'kind': 'passed'}], + 'locate_shelf_column': [{'kind': 'normal', 'shelf_id': 'shelf_A', 'side_id': 'FRONT', 'column_id': '1', 'tier_id': '2'}], + 'navigate_semantic': [{'kind': 'normal', 'final_pose': site['locations'][name]} for name in stops]} + actions = [name for name in ACTION_TYPES if name != 'execute_task'] + rclpy.init(args=['--ros-args', '-p', 'initial_holding_state:=EMPTY', + '-p', 'scenarios_json:=' + json.dumps(json.dumps(scenarios)), + '-p', 'enabled_actions:=' + json.dumps(actions), '-p', + 'enabled_topics:=' + json.dumps(['robot_state', 'safety_state', 'visual_observation', 'dense_progress'])]) + server = MockSkills() + node = Node('native_workflow_regression', namespace='/sim/robot_01') + executor = MultiThreadedExecutor(num_threads=8) + executor.add_node(server) + executor.add_node(node) + thread = threading.Thread(target=executor.spin, daemon=True) + thread.start() + client = ActionClient(node, ExecuteTask, 'tasks/execute') + service = node.create_client(ReconcileTask, 'tasks/reconcile') + feedback = [] + row = dict(route=plan['route'], scope='native ROS2 executor with simulated physical skills') + process = None + log = (output / ('native-' + suffix + '.log')).open('w') + token = 'native-regression-recovery-token' + operator = 'native-regression-operator' + with tempfile.TemporaryDirectory(prefix='native-bt-regression-') as journal: + def stop(): + nonlocal process + if process is not None and process.poll() is None: + os.killpg(process.pid, signal.SIGINT) + try: + process.wait(timeout=5) + except subprocess.TimeoutExpired: + os.killpg(process.pid, signal.SIGKILL) + process.wait() + process = None + + def start(expect_failure=False): + nonlocal process + args = [str(binary), '--ros-args', '-r', '__ns:=/sim/robot_01', + '-p', 'robot_id:=robot_01', '-p', 'allowed_robots:=[robot_01]', + '-p', 'site_config_file:=' + str(site_path), '-p', 'journal_directory:=' + journal, + '-p', 'execution_enabled:=true', '-p', 'skill_timeout_ms:=5000', + '-p', 'recovery_token:=' + token, '-p', 'recovery_operator_id:=' + operator] + process = subprocess.Popen(args, stdout=log, stderr=subprocess.STDOUT, start_new_session=True) + if expect_failure: + code = process.wait(timeout=5) + assert code != 0, 'corrupt task journal accepted at startup' + return + assert client.wait_for_server(timeout_sec=12), 'native executor not discovered' + time.sleep(0.6) + assert process.poll() is None, 'native executor exited during startup' + + def task_goal(tag): + goal = ExecuteTask.Goal() + goal.trace.task_id = 'native-' + suffix + '-' + tag + goal.trace.run_id = 'run-' + suffix + '-' + tag + goal.trace.subtask_id = 'execute_task' + goal.trace.attempt = goal.trace.task_revision = goal.trace.plan_version = goal.trace.execution_generation = 1 + goal.approved_plan_json = json.dumps(plan) + goal.context_json = json.dumps(dict(schema_version=1, registry_version=1, robot_id='robot_01', target_id='water', source_shelf='shelf_A', destination_id='tote_A', observe_location='observe_A', destination_location='tote_A_stop', task_revision=1, route=plan['route'], item_index=0)) + goal.timeout.sec = 30 + return goal + + def reconcile(goal, authorization=token, stale=False, resolution='resume_task'): + assert service.wait_for_service(timeout_sec=10), 'recovery service not discovered' + request = ReconcileTask.Request() + request.trace = copy.deepcopy(goal.trace) + if stale: + request.trace.execution_generation += 1 + request.operator_id, request.authorization = operator, authorization + request.evidence_ref = 'regression/operator-observed-stop' + request.resolution = resolution + return wait(service.call_async(request), 15) + + def motion_counts(): + with server.lock: + return {name: server.counts[name] for name in ('navigate_semantic', 'execute_manipulation', 'execute_posture')} + + try: + start() + goal = task_goal('complete') + handle = wait(client.send_goal_async(goal, feedback_callback=lambda msg: feedback.append(msg.feedback.stage)), 8) + assert handle.accepted, 'valid stage-one route was rejected' + wrapped = wait(handle.get_result_async()) + result = wrapped.result + row.update(native_status=wrapped.status, status=result.result.status, + completed_quantity=result.completed_quantity, evidence=json.loads(result.evidence_json)) + assert wrapped.status == 4 and result.result.status == 0, result.result.message + assert result.completed_quantity == 1 and result.result.stop_state == 1 + assert row['evidence']['safe_to_release'] is True + row['decoded_manipulation'] = assert_manipulation_evidence( + journal_records(Path(journal) / 'goal_registry.log'), goal.trace.run_id, + ExecuteManipulation, deserialize_message) + assert motion_counts()['execute_manipulation'] == 2 + if recovery_enabled: + stop() + time.sleep(0.8) + start() + before = motion_counts() + wrong = reconcile(goal, authorization='wrong-token') + assert not wrong.accepted, 'wrong authorization accepted' + stale = reconcile(goal, stale=True) + assert not stale.accepted, 'stale execution generation accepted' + with server.lock: + server.scenarios['verify_state'] = [{'kind': 'unknown'}] + server.holding_state = RobotState.HOLDING_UNKNOWN + time.sleep(0.25) + unknown = reconcile(goal) + assert not unknown.accepted, 'unknown holding was treated as safe recovery' + with server.lock: + server.scenarios['verify_state'] = [{'kind': 'passed'}] + server.holding_state = RobotState.EMPTY + time.sleep(0.25) + recovered = reconcile(goal) + assert recovered.accepted, recovered.error_code + ': ' + recovered.message + state = json.loads(recovered.state_json) + assert state['verified'] is True and state['stop_confirmed'] is True + assert state['holding_state'] == 'EMPTY' + receipts = state['receipts'] + assert len(receipts) == 1 and receipts[0]['task_id'] == goal.trace.task_id + assert receipts[0]['completed_quantity'] == 1 + assert motion_counts() == before, 'receipt recovery dispatched motion' + row['recovery'] = dict(wrong_token=wrong.error_code, stale_trace=stale.error_code, + unknown_holding=unknown.error_code, recovered=state) + if suffix == 'object_table': + with server.lock: + server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'duration_seconds': 3., 'final_pose': site['locations']['shelf_A_stop']}] + canceled_goal = task_goal('cancel') + canceled_handle = wait(client.send_goal_async(canceled_goal), 8) + assert canceled_handle.accepted + eventually(lambda: motion_counts()['navigate_semantic'] > before['navigate_semantic']) + wait(canceled_handle.cancel_goal_async(), 5) + canceled_result = wait(canceled_handle.get_result_async(), 15) + assert canceled_result.status == 5 and canceled_result.result.result.stop_state == 1 + assert motion_counts()['execute_manipulation'] == before['execute_manipulation'] + stop() + time.sleep(0.8) + start() + # Absence of physical-goal history cannot prove no dispatch. + stop() + goal_log = Path(journal) / 'goal_registry.log' + intact_goals = goal_log.read_bytes() + goal_log.write_bytes(b'') + start() + incomplete = reconcile(canceled_goal, resolution='resume_task') + assert not incomplete.accepted, 'missing dispatch history authorized an item replay' + stop() + goal_log.write_bytes(intact_goals) + start() + canceled_recovery = reconcile(canceled_goal, resolution='cancel_task') + assert canceled_recovery.accepted, canceled_recovery.error_code + row['cancel_recovery'] = json.loads(canceled_recovery.state_json) + row['missing_history_rejected'] = incomplete.error_code + stop() + task_log = Path(journal) / 'task_runs.jsonl' + intact_tasks = task_log.read_bytes() + assert intact_tasks.endswith(b'\n') + task_log.write_bytes(intact_tasks[:-1]) + start(expect_failure=True) + row['unterminated_task_journal_rejected'] = True + stop() + task_log.write_bytes(intact_tasks) + if suffix == 'shelf_cell': + with server.lock: + server.scenarios['locate_shelf_column'] = [{'kind': 'ambiguous'}] + server.scenarios['navigate_semantic'] = [{'kind': 'normal', 'final_pose': site['locations']['observe_A']}] + before_ask = motion_counts() + ask_handle = wait(client.send_goal_async(task_goal('ambiguous')), 8) + assert ask_handle.accepted + ask_wrapped = wait(ask_handle.get_result_async(), 20) + ask = ask_wrapped.result + ask_evidence = json.loads(ask.evidence_json) + assert ask_wrapped.status == 6 and ask.completed_quantity == 0 + assert ask_evidence['safe_to_release'] is True + assert ask_evidence['needs_clarification'] is True + assert ask_evidence['safe_to_retry'] is True + assert motion_counts()['execute_manipulation'] == before_ask['execute_manipulation'] + row['runtime_clarification'] = ask_evidence + row['mock_calls'] = dict(server.counts) + row['feedback_stages'] = feedback + row['passed'] = True + except Exception as exc: + row['passed'] = False + row['failure'] = repr(exc) + finally: + stop() + for path in Path(journal).glob('*'): + if path.is_file(): + (output / ('native-' + suffix + '-' + path.name)).write_bytes(path.read_bytes()) + client.destroy() + executor.shutdown(timeout_sec=2) + thread.join(timeout=2) + server.destroy_node() + node.destroy_node() + rclpy.shutdown() + log.close() + return row + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--repo-root', type=Path, default=Path(__file__).resolve().parents[2]) + parser.add_argument('--executor', type=Path) + parser.add_argument('--output-dir', type=Path, required=True) + parser.add_argument('--skip-recovery', action='store_true', help='Run workflow/evidence subset before recovery service is built') + args = parser.parse_args() + root = args.repo_root.resolve() + sys.path[:0] = [str(root / 'coordinator'), str(root / 'ros2/bt_mock_servers')] + binary = args.executor or root / 'install/bt_executor/lib/bt_executor/bt_executor_node' + args.output_dir.mkdir(parents=True, exist_ok=True) + rows = [] + for suffix in ('object_table', 'shelf_cell'): + row = run_route(root, binary, args.output_dir, suffix, not args.skip_recovery) + rows.append(row) + (args.output_dir / 'native-workflow-regression.json').write_text(json.dumps(rows, indent=2)) + print(json.dumps({k: row[k] for k in ('route', 'passed', 'failure') if k in row}), flush=True) + if not all(row['passed'] for row in rows): + raise SystemExit(1) + + +if __name__ == '__main__': + main() diff --git a/tests/test_coordinator_recovery.py b/tests/test_coordinator_recovery.py new file mode 100644 index 0000000..9716e19 --- /dev/null +++ b/tests/test_coordinator_recovery.py @@ -0,0 +1,190 @@ +import copy +import json +from pathlib import Path +import sqlite3 +import sys +import tempfile +import unittest +from unittest.mock import patch + +ROOT=Path(__file__).resolve().parents[1] +sys.path[:0]=[str(ROOT/'coordinator'),str(ROOT/'robobrain')] +from robot_bt_coordinator.service import Coordinator,demo_site +from robot_bt_coordinator.backends import ManualBackend,demo_plan +from robot_bt_coordinator.errors import ApiError +from robot_bt_coordinator.replay import replay_events +from robot_bt_coordinator.store import Store +from robot_bt_coordinator.plan_v2 import make_plan + + +class TrustedBackend(ManualBackend): + def reconcile(self,task,request): + return dict(verified=True,stop_confirmed=True,holding_state='EMPTY',run_id=task['run_id'],evidence_ref=request['evidence_ref'],receipts=copy.deepcopy(self.receipts),safe_to_retry=self.retry) + + +class RecoveryTest(unittest.TestCase): + def setUp(self): + self.tmp=tempfile.TemporaryDirectory();self.backend=TrustedBackend();self.backend.receipts=[];self.backend.retry=False + self.site=demo_site();self.site['object_aliases']={'water':['water'],'doll':['doll']} + 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): + task=self.c.submit(dict(client_request_id='q',robot_id='r',instruction='move item',known_info={'target_name':'water','source_location':'shelf_A','destination':'tote_A'})) + self.c.tick();task=self.c.get(task['task_id']) + self.backend.emit(dict(type='plan',task_id=task['task_id'],task_revision=task['task_revision'],planning_generation=task['planning_generation'],status='PLAN_READY',plan=demo_plan(task['request']['known_info']))) + self.c.tick();return self.c.get(task['task_id']) + def quarantine(self,task): + self.backend.emit(dict(type='execution_result',task_id=task['task_id'],run_id=task['run_id'],status='INTERVENTION_REQUIRED',completed_quantity=0,stop_confirmed=False)) + self.c.tick() + def request(self,task,resolution='resume_task'): + return dict(run_id=task['run_id'],evidence_ref='trusted-proof',resolution=resolution) + def receipt(self,task): + return dict(task_id=task['task_id'],item_index=0,completed_quantity=1,evidence=dict(evidence_id='delivery-proof',target_ref='water',destination_ref='tote_A',passed=True,empty_hand=True,in_destination=True,valid=True)) + def test_unsafe_resume_cannot_replay_an_uncertain_manipulation(self): + task=self.start();self.quarantine(task) + with self.assertRaises(ApiError) as error:self.c.intervene(task['task_id'],self.request(task)) + self.assertEqual(error.exception.code,'RECONCILIATION_REJECTED') + self.assertEqual(len(self.backend.executions),1) + def test_trusted_receipt_recovers_lost_result_without_redispatch(self): + task=self.start();self.quarantine(task);self.backend.receipts=[self.receipt(task)] + out=self.c.intervene(task['task_id'],self.request(task)) + self.assertEqual(out['status'],'SUCCEEDED');self.assertEqual(out['completed_quantity'],1) + self.assertEqual(len(self.backend.executions),1) + replay=replay_events(self.c.events(task['task_id'],limit=500)) + self.assertEqual(replay['completed_quantity'],1) + def test_empty_hand_alone_cannot_credit_or_resume(self): + task=self.start();self.quarantine(task) + with self.assertRaises(ApiError):self.c.intervene(task['task_id'],self.request(task)) + out=self.c.intervene(task['task_id'],self.request(task,'cancel_task')) + self.assertEqual(out['status'],'CANCELED');self.assertEqual(out['completed_quantity'],0) + def clarification_result(self,task,**changes): + evidence=dict(needs_clarification=True,safe_to_retry=True,empty_hand=True,valid=True,safe_to_release=True,questions=['Which registered shelf should be used?']) + evidence.update(changes) + self.backend.emit(dict(type='execution_result',task_id=task['task_id'],run_id=task['run_id'],status='FAILED',completed_quantity=0,stop_confirmed=True,evidence=evidence)) + self.c.tick();return self.c.get(task['task_id']) + def test_verified_runtime_question_returns_to_planning_after_answer(self): + task=self.start();out=self.clarification_result(task) + self.assertEqual(out['status'],'NEEDS_CLARIFICATION');self.assertFalse(out['motion_dispatched']) + self.assertEqual(out['question']['source_run_id'],task['run_id']) + out=self.c.clarify(task['task_id'],dict(question_id=out['question']['question_id'],task_revision=out['task_revision'],known_info={'source_location':'shelf_A'})) + self.c.tick();self.assertEqual(self.c.get(task['task_id'])['status'],'PLANNING') + self.assertTrue(self.backend.plans[-1]['clarification_confirmed']) + self.assertEqual(len(self.backend.executions),1) + def test_runtime_question_without_no_manipulation_proof_is_quarantined(self): + task=self.start();out=self.clarification_result(task,safe_to_retry=False) + self.assertEqual(out['status'],'INTERVENTION_REQUIRED') + self.assertIsNone(out['question']) + def test_ordinary_failure_does_not_fabricate_question(self): + task=self.start();out=self.clarification_result(task,needs_clarification=False) + self.assertEqual(out['status'],'FAILED');self.assertIsNone(out['question']) + def test_operator_replan_requires_no_manipulation_and_no_delivery(self): + task=self.start();self.quarantine(task) + with self.assertRaises(ApiError):self.c.intervene(task['task_id'],self.request(task,'replan_task')) + self.backend.retry=True + out=self.c.intervene(task['task_id'],self.request(task,'replan_task')) + self.assertEqual(out['status'],'NEEDS_CLARIFICATION');self.assertFalse(out['motion_dispatched']) + self.assertEqual(len(self.backend.executions),1) + def test_operator_replan_cannot_rewrite_already_delivered_scope(self): + task=self.start();self.quarantine(task);self.backend.retry=True;self.backend.receipts=[self.receipt(task)] + with self.assertRaises(ApiError):self.c.intervene(task['task_id'],self.request(task,'replan_task')) + self.assertEqual(self.c.get(task['task_id'])['status'],'INTERVENTION_REQUIRED') + def test_question_cannot_hide_a_verified_physical_delivery(self): + task=self.start();proof=self.receipt(task)['evidence'] + proof.update(needs_clarification=True,safe_to_retry=True,safe_to_release=True,questions=['Which shelf?']) + self.backend.emit(dict(type='execution_result',task_id=task['task_id'],run_id=task['run_id'],status='FAILED',completed_quantity=1,stop_confirmed=True,evidence=proof));self.c.tick() + out=self.c.get(task['task_id']) + self.assertEqual(out['completed_quantity'],1) + self.assertEqual(out['status'],'INTERVENTION_REQUIRED') + def test_proven_pre_manipulation_resume_uses_new_run(self): + task=self.start();self.quarantine(task);self.backend.retry=True + out=self.c.intervene(task['task_id'],self.request(task)) + self.assertEqual(out['status'],'EXECUTING');self.assertNotEqual(out['run_id'],task['run_id']) + self.assertEqual(len(self.backend.executions),2) + def test_paused_execution_accepts_authenticated_reconciliation(self): + task=self.start();self.c.control(task['task_id'],'pause') + self.backend.emit(dict(type='execution_result',task_id=task['task_id'],run_id=task['run_id'],status='CANCELED',completed_quantity=0,stop_confirmed=True)) + self.c.tick();self.assertEqual(self.c.get(task['task_id'])['status'],'PAUSED') + self.backend.retry=True + self.assertEqual(self.c.intervene(task['task_id'],self.request(task))['status'],'EXECUTING') + def test_unverified_resume_preserves_paused_state(self): + task=self.start();self.c.control(task['task_id'],'pause') + self.backend.emit(dict(type='execution_result',task_id=task['task_id'],run_id=task['run_id'],status='CANCELED',completed_quantity=0,stop_confirmed=True)) + self.c.tick() + with self.assertRaises(ApiError) as error:self.c.control(task['task_id'],'resume') + self.assertEqual(error.exception.code,'RECONCILIATION_REQUIRED') + self.assertEqual(self.c.get(task['task_id'])['status'],'PAUSED') + self.assertEqual(len(self.backend.executions),1) + def test_completed_planning_releases_timer_state(self): + self.start();self.assertEqual(self.c._planning_started,{}) + def test_duplicate_recovered_receipts_credit_once_then_start_next_item(self): + self.site['execution_route']='OBJECT_TABLE' + slots=dict(items=[dict(target_name='water',quantity=2,source_location='shelf_A')],destination='tote_A') + task=self.c.submit(dict(client_request_id='multi',robot_id='r',instruction='move two',known_info=slots)) + self.c.tick();task=self.c.get(task['task_id']) + self.backend.emit(dict(type='plan',task_id=task['task_id'],task_revision=1,planning_generation=task['planning_generation'],status='PLAN_READY',plan=make_plan('move two',slots,'OBJECT_TABLE'))) + self.c.tick();task=self.c.get(task['task_id']);self.quarantine(task) + receipt=self.receipt(task);self.backend.receipts=[receipt,copy.deepcopy(receipt)] + out=self.c.intervene(task['task_id'],self.request(task)) + self.assertEqual(out['status'],'EXECUTING');self.assertEqual(out['completed_quantity'],1) + self.assertEqual(out['active_item_index'],1);self.assertNotEqual(out['run_id'],task['run_id']) + self.assertEqual([e['item_index'] for e in self.c.events(task['task_id'],limit=500) if e['kind']=='delivery_committed'],[0]) + def test_empty_clarification_cannot_mark_intent_as_confirmed(self): + task=self.c.submit(dict(client_request_id='intent',robot_id='r',instruction='1 water shelf_A tote_A',known_info=dict(target_name='doll'))) + with self.assertRaises(ApiError): + self.c.clarify(task['task_id'],dict(question_id=task['question']['question_id'],task_revision=task['task_revision'],known_info={})) + self.assertFalse(self.c.get(task['task_id'])['clarification_confirmed']) + def test_invalid_receipts_are_atomic_and_do_not_release_quarantine(self): + for mutate in [lambda r:r.update(task_id='other'),lambda r:r.update(item_index=True),lambda r:r.update(completed_quantity=2),lambda r:r['evidence'].update(destination_ref='wrong')]: + task=self.start() if not hasattr(self,'current') else self.current + self.current=task;self.quarantine(task) + receipt=self.receipt(task);mutate(receipt);self.backend.receipts=[receipt] + with self.assertRaises(ApiError):self.c.intervene(task['task_id'],self.request(task,'cancel_task')) + self.assertEqual(self.c.get(task['task_id'])['completed_quantity'],0) + self.assertEqual(self.c.get(task['task_id'])['status'],'INTERVENTION_REQUIRED') + def test_tick_avoids_historical_task_scan(self): + for n in range(30): + t=self.c.submit(dict(client_request_id=str(n),robot_id='r',instruction='move')) + self.c.control(t['task_id'],'cancel') + with patch.object(self.c.store,'all',side_effect=AssertionError('historical scan')): + self.c.tick() + def test_progress_replay_preserves_stage_sequence_and_detail(self): + task=self.start() + self.backend.emit(dict(type='progress',task_id=task['task_id'],run_id=task['run_id'],sequence=1,stage='NavigateSource',detail='waiting obstacle')) + self.c.tick() + events=self.c.events(task['task_id'],limit=500) + progress=next(e for e in events if e['kind']=='progress') + self.assertEqual(progress['stage'],'NavigateSource');self.assertEqual(progress['sequence'],1) + self.assertEqual(progress['detail'],'waiting obstacle') + self.assertEqual(replay_events(events)['progress'][-1]['stage'],'NavigateSource') + def test_supplied_slots_conflicting_with_instruction_require_clarification(self): + task=self.c.submit(dict(client_request_id='intent',robot_id='r',instruction='1 water shelf_A tote_A',known_info=dict(target_name='doll',quantity=1,source_location='shelf_A',destination='tote_A'))) + self.assertEqual(task['status'],'NEEDS_CLARIFICATION');self.assertFalse(task['clarification_confirmed']) + self.c.tick();self.assertEqual(self.backend.plans,[]) + task=self.c.clarify(task['task_id'],dict(question_id=task['question']['question_id'],task_revision=task['task_revision'],known_info=dict(target_name='doll'))) + self.assertTrue(task['clarification_confirmed']) + self.c.close();self.c=Coordinator(self.tmp.name+'/tasks.db',self.backend,['r'],self.site) + self.assertTrue(self.c.get(task['task_id'])['clarification_confirmed']) + self.c.tick();self.assertTrue(self.backend.plans[-1]['clarification_confirmed']) + def test_approved_plan_is_independently_checked_against_instruction(self): + task=self.c.submit(dict(client_request_id='intent',robot_id='r',instruction='1 water shelf_A tote_A',known_info={})) + self.c.tick();task=self.c.get(task['task_id']) + plan=demo_plan(dict(target_name='doll',quantity=1,source_location='shelf_A',destination='tote_A')) + self.backend.emit(dict(type='plan',task_id=task['task_id'],task_revision=1,planning_generation=task['planning_generation'],status='PLAN_READY',plan=plan));self.c.tick() + self.assertEqual(self.c.get(task['task_id'])['status'],'NEEDS_CLARIFICATION');self.assertEqual(self.backend.executions,[]) + + +class MigrationTest(unittest.TestCase): + def test_legacy_database_migrates_active_status_without_losing_history(self): + with tempfile.TemporaryDirectory() as folder: + path=folder+'/old.db';db=sqlite3.connect(path) + db.execute('CREATE TABLE tasks(seq INTEGER PRIMARY KEY AUTOINCREMENT,task_id TEXT UNIQUE NOT NULL,robot_id TEXT NOT NULL,request_id TEXT NOT NULL,request_hash TEXT NOT NULL,data TEXT NOT NULL,UNIQUE(robot_id,request_id))') + for n,status in enumerate(('SUCCEEDED','QUEUED','PAUSED')): + db.execute('INSERT INTO tasks(task_id,robot_id,request_id,request_hash,data) VALUES(?,?,?,?,?)',(str(n),'r',str(n),'hash',json.dumps(dict(task_id=str(n),status=status)))) + db.commit();db.close();store=Store(path) + try: + self.assertEqual([t['status'] for t in store.active()],['QUEUED','PAUSED']) + self.assertEqual(len(store.all()),3) + finally:store.close() + + +if __name__=='__main__':unittest.main() diff --git a/tests/test_http_api.py b/tests/test_http_api.py index b6fa084..25b6bba 100644 --- a/tests/test_http_api.py +++ b/tests/test_http_api.py @@ -3,6 +3,9 @@ import json import sys import tempfile import threading +import socket +from concurrent.futures import ThreadPoolExecutor +from unittest.mock import patch import unittest from pathlib import Path sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'coordinator')) @@ -35,4 +38,54 @@ class HttpTest(unittest.TestCase): self.assertEqual(self.request('POST','/v1/tasks',payload)[0],400) self.assertEqual(self.request('GET','/not-found')[0],404) + def test_listener_can_queue_ten_fresh_connections_before_accept(self): + # Model a burst while the accept loop is busy. This is a capacity check, + # not a throughput or latency assertion on the host running the suite. + listener=make_server(self.c,'127.0.0.1',0,'test-secret') + clients=[] + try: + for _ in range(10): + try: + clients.append(socket.create_connection(('127.0.0.1',listener.server_port),timeout=2)) + except OSError as error: + self.fail('listener could not queue ten clients: '+str(error)) + finally: + for client in clients:client.close() + listener.server_close() + + def test_ten_concurrent_submissions_are_independently_accepted_and_deduplicated(self): + gate=threading.Barrier(10) + def submit(index): + body=json.dumps(dict(client_request_id='burst-'+str(index),robot_id='robot_01',instruction='move water',known_info={})) + gate.wait(timeout=10) + return self.request('POST','/v1/tasks',body) + with ThreadPoolExecutor(max_workers=10) as pool: + results=list(pool.map(submit,range(10))) + self.assertEqual([code for code,_ in results],[202]*10) + self.assertEqual(len({task['task_id'] for _,task in results}),10) + self.assertTrue(all(task['status']=='QUEUED' for _,task in results)) + with ThreadPoolExecutor(max_workers=10) as pool: + repeats=list(pool.map(submit,range(10))) + self.assertEqual([task['task_id'] for _,task in repeats],[task['task_id'] for _,task in results]) + self.assertTrue(all(code==202 and task['deduplicated'] for code,task in repeats)) + self.assertEqual(len(self.c.store.all()),10) + + def test_recovery_backend_failure_is_sanitized_service_unavailable(self): + with patch.object(self.c,'intervene',side_effect=RuntimeError('private-token-123 from ROS')): + code,body=self.request('POST','/v1/tasks/task/interventions','{}',token='operator-secret') + self.assertEqual(code,503);self.assertEqual(body['error_code'],'RECONCILIATION_UNAVAILABLE') + self.assertNotIn('private-token',json.dumps(body)) + + def test_recovery_timeout_returns_json_instead_of_dropping_connection(self): + with patch.object(self.c,'intervene',side_effect=TimeoutError('private-token-123 outcome unknown')): + try:code,body=self.request('POST','/v1/tasks/task/interventions','{}',token='operator-secret') + except http.client.RemoteDisconnected:self.fail('recovery timeout dropped the HTTP connection without a JSON result') + self.assertEqual(code,504);self.assertEqual(body['error_code'],'RECONCILIATION_TIMEOUT') + self.assertNotIn('private-token',json.dumps(body)) + + def test_recovery_backend_is_not_called_with_ordinary_api_token(self): + with patch.object(self.c,'intervene') as reconcile: + self.assertEqual(self.request('POST','/v1/tasks/task/interventions','{}')[0],401) + reconcile.assert_not_called() + if __name__=='__main__':unittest.main() diff --git a/tests/test_robobrain_consistency.py b/tests/test_robobrain_consistency.py new file mode 100644 index 0000000..a3bd782 --- /dev/null +++ b/tests/test_robobrain_consistency.py @@ -0,0 +1,158 @@ +import copy +import json +import sys +import tempfile +import types +import unittest +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +sys.path[:0] = [str(ROOT/'coordinator'), str(ROOT/'robobrain'), str(ROOT/'ros2/robobrain_services')] +from robot_robobrain.backends import FixtureBackend +from robot_robobrain.service import BrainService +from robot_robobrain.observations import Observation +from robot_bt_coordinator.plan_v2 import make_plan + +class ConsistencyTests(unittest.TestCase): + def setUp(self): + self.tmp = tempfile.TemporaryDirectory() + self.addCleanup(self.tmp.cleanup) + self.site = json.loads((ROOT/'config/sim_site_object_table.json').read_text(encoding='utf-8')) + self.slots = {'items': [{'target_name':'water', 'quantity':2, 'source_location':'shelf_A'}, {'target_name':'doll', 'quantity':1, 'source_location':'shelf_A'}], 'destination':'tote_A'} + self.instruction = '把货架A的两瓶水和一个玩偶放到周转箱A' + + def run_plan(self, slots, revision=1, confirmed=False, instruction=None): + text = instruction or self.instruction + service = BrainService(FixtureBackend(json.dumps(make_plan(text, slots, 'OBJECT_TABLE'))), self.tmp.name) + return service.plan(dict(task_id='t', task_revision=revision, planning_generation=1, + instruction=text, known_info=slots, context=self.site, + constraints={'route':'OBJECT_TABLE', 'clarification_confirmed':confirmed}, timeout=1)) + + def test_initial_explicit_quantity_order_item_destination_conflicts_fail(self): + variants=[] + p=copy.deepcopy(self.slots);p['items'][0]['quantity']=1;variants.append(p) + p=copy.deepcopy(self.slots);p['items'].reverse();variants.append(p) + p=copy.deepcopy(self.slots);p['items'][0]['target_name']='doll';variants.append(p) + p=copy.deepcopy(self.slots);p['destination']='tote_B';variants.append(p) + for slots in variants: + with self.subTest(slots=slots): + result=self.run_plan(slots) + self.assertEqual(result['status'], 'FAILED') + self.assertEqual(result['error_code'], 'SEMANTIC_MISMATCH') + self.assertTrue(Path(result['record_ref']).is_file()) + + def test_revision_alone_and_initial_confirmation_marker_do_not_override(self): + slots=copy.deepcopy(self.slots);slots['items'][0]['quantity']=1 + for revision,confirmed in [(2,False),(1,True),(2,'true')]: + with self.subTest(revision=revision,confirmed=confirmed): + self.assertEqual(self.run_plan(slots,revision,confirmed)['status'],'FAILED') + + def test_confirmed_clarification_can_override_original(self): + slots=copy.deepcopy(self.slots);slots['items'][0]['quantity']=1 + self.assertEqual(self.run_plan(slots,2,True)['status'],'PLAN_READY') + + def test_matching_and_unrecognized_language_are_not_false_proofs(self): + self.assertEqual(self.run_plan(self.slots)['status'],'PLAN_READY') + self.assertEqual(self.run_plan(self.slots,instruction='carry out the supplied item list')['status'],'PLAN_READY') + + def test_shelf_non_success_retains_observation_identity(self): + path=Path(self.tmp.name)/'image.png';path.write_bytes(b'fixture') + obs=Observation('observation-7',100,'camera',str(path),'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) + for status in ['NOT_FOUND','AMBIGUOUS']: + result=BrainService(FixtureBackend(json.dumps({'status':status})),self.tmp.name).shelf(goal,obs,110) + self.assertEqual(result.get('observation_id'),'observation-7') + self.assertEqual(result.get('observed_at'),100) + self.assertNotIn('column_id',result) + + def test_unexpected_backend_failure_is_archived(self): + def broken(_): raise RuntimeError('model driver crashed') + service=BrainService(FixtureBackend(broken),self.tmp.name) + goal=dict(task_id='t',task_revision=1,planning_generation=1,instruction=self.instruction,known_info=self.slots,context=self.site,constraints={'route':'OBJECT_TABLE'},timeout=1) + try: + result=service.plan(goal) + except RuntimeError: + self.fail('unexpected backend exception escaped without diagnostic result') + self.assertEqual(result['status'],'FAILED') + self.assertEqual(result['error_code'],'INFERENCE_FAILED') + self.assertIn('model driver crashed',json.loads(Path(result['record_ref']).read_text())['result']['message']) + +class AdapterConfigTests(unittest.TestCase): + def test_missing_loader_configuration_has_actionable_error(self): + from robot_robobrain.model_adapter import create + for config in [{}, {'loader':'missing-separator','model':{},'task_mapping':{}}]: + try: + create(config) + except Exception as ex: + self.assertIsInstance(ex,ValueError) + self.assertIn('loader',str(ex)) + else:self.fail('invalid loader configuration accepted') + + def test_single_image_adapter_rejects_dense_frames_before_loader_import(self): + from robot_robobrain.model_adapter import create + config={'loader':'deployment_models:create_robobrain','model':{'checkpoint':'/not/present'},'task_mapping':{'dense_feedback':'general'}} + try:create(config) + except Exception as ex: + self.assertIsInstance(ex,ValueError) + self.assertIn('dense',str(ex)) + else:self.fail('single-image adapter accepted dense task mapping') + +class ServiceBoundaryTests(unittest.TestCase): + def test_malformed_ros_request_is_archived_without_parsing_it(self): + from robobrain_services import nodes + self.assertTrue(callable(getattr(nodes,'guarded_work',None)), 'ROS worker needs a failure-archiving boundary') + with tempfile.TemporaryDirectory() as tmp: + service=BrainService(FixtureBackend('{}'),tmp) + request=types.SimpleNamespace(task_id='t',instruction='original text',known_info_json='{malformed',timeout=types.SimpleNamespace(sec=1,nanosec=0)) + def bad_request(): raise ValueError('invalid JSON input') + result=nodes.guarded_work(service,'plan',request,bad_request) + self.assertEqual(result['error_code'],'SERVICE_ERROR') + record=json.loads(Path(result['record_ref']).read_text()) + self.assertEqual(record['input']['known_info_json'],'{malformed') + self.assertEqual(record['input']['timeout']['sec'],1) + self.assertIn('invalid JSON input',record['result']['message']) + + def test_archive_failure_still_returns_terminal_failure(self): + from robobrain_services import nodes + self.assertTrue(callable(getattr(nodes,'guarded_work',None)), 'ROS worker needs a failure-archiving boundary') + class UnavailableArchive: + def _record(self,*args):raise OSError('disk full') + def failure():raise RuntimeError('service crashed') + result=nodes.guarded_work(UnavailableArchive(),'shelf',{'task_id':'t'},failure) + self.assertEqual(result['status'],'FAILED') + self.assertEqual(result.get('record_ref'),'') + self.assertIn('archive',result['message']) + self.assertIn('disk full',result['message']) + + def test_success_result_is_not_rewritten(self): + from robobrain_services import nodes + self.assertTrue(callable(getattr(nodes,'guarded_work',None)), 'ROS worker needs a failure-archiving boundary') + expected={'status':'PLAN_READY','record_ref':'existing'} + self.assertIs(nodes.guarded_work(None,'plan',{},lambda:expected),expected) + +class LocalizeDiagnosticTests(unittest.TestCase): + def test_non_success_keeps_observation_without_inventing_point(self): + with tempfile.TemporaryDirectory() as tmp: + path=Path(tmp)/'image.png';path.write_bytes(b'fixture') + obs=Observation('obs3d',100,'camera',str(path),'observe_A',1,'shelf_A','cal-1',7) + goal=dict(task_id='t',subtask_id='s',target_ref='water',capture_after=90,timeout=1) + for status in ('NOT_FOUND','AMBIGUOUS'): + result=BrainService(FixtureBackend(json.dumps({'status':status})),tmp).localize(goal,obs,110) + self.assertEqual(result.get('observation_id'),'obs3d') + self.assertEqual(result.get('observed_at'),100) + self.assertEqual(result.get('calibration_id'),'cal-1') + self.assertEqual(result.get('geometry_epoch'),7) + self.assertFalse(result['geometry_valid']) + self.assertNotIn('target_point',result) + +class DemoClarificationTests(unittest.TestCase): + def test_demo_propagates_trusted_clarification_marker(self): + from robot_robobrain.demo_backend import BrainDemoBackend + backend=BrainDemoBackend.__new__(BrainDemoBackend) + backend.plans=[];backend.site={'execution_route':'OBJECT_TABLE'} + goals=[] + backend.brain=types.SimpleNamespace(plan=lambda goal:(goals.append(goal) or {'status':'FAILED','record_ref':'r'})) + backend.emit=lambda event:None + task={'task_id':'t','task_revision':2,'planning_generation':3,'clarification_confirmed':True,'request':{'instruction':'original','known_info':{}}} + backend.start_planning(task) + self.assertIs(goals[0]['constraints'].get('clarification_confirmed'),True) diff --git a/tests/test_tooling.py b/tests/test_tooling.py new file mode 100644 index 0000000..e00a647 --- /dev/null +++ b/tests/test_tooling.py @@ -0,0 +1,25 @@ +import importlib.util +from pathlib import Path +import unittest +from unittest.mock import patch + +ROOT=Path(__file__).resolve().parents[1] +spec=importlib.util.spec_from_file_location('coverage_report',ROOT/'tools/coverage_report.py') +coverage=importlib.util.module_from_spec(spec) +spec.loader.exec_module(coverage) + + +class CoverageCompatibilityTest(unittest.TestCase): + def test_non_source_line_markers_do_not_enter_coverage_counts(self): + # Python 3.14 trace may expose None for artificial bytecode positions. + with patch.object(coverage.trace,'_find_executable_linenos',return_value={None:1,0:1,1:1,7:1}): + actual=coverage.executable_lines('example.py') + self.assertEqual(actual,{1,7}) + + def test_real_source_lines_remain_available(self): + lines=coverage.executable_lines(__file__) + self.assertTrue(lines) + self.assertTrue(all(type(line) is int and line>0 for line in lines)) + + +if __name__=='__main__':unittest.main() diff --git a/tools/build_portable.sh b/tools/build_portable.sh index 040348c..9114640 100755 --- a/tools/build_portable.sh +++ b/tools/build_portable.sh @@ -9,7 +9,8 @@ flags=(-std=c++17 -O2 -Wall -Wextra -Werror -pthread -Icore/include) "$cxx" "${flags[@]}" -c core/src/workflow.cpp -o build/workflow.o "$cxx" "${flags[@]}" -c core/src/sim_driver.cpp -o build/sim_driver.o "$cxx" "${flags[@]}" core/examples/demo.cpp build/core.o build/workflow.o build/sim_driver.o -o build/bt_demo -for test_name in core_test workflow_test journal_failure_test preflight_test settlement_test proof_regression_test readiness_regression_test scenario_test lifecycle_test; do - "$cxx" "${flags[@]}" "core/tests/$test_name.cpp" build/core.o build/workflow.o -o "build/$test_name" +for test_source in core/tests/*_test.cpp; do + test_name="$(basename "$test_source" .cpp)" + "$cxx" "${flags[@]}" -UNDEBUG "$test_source" build/core.o build/workflow.o -o "build/$test_name" "./build/$test_name" done diff --git a/tools/coverage_report.py b/tools/coverage_report.py index 78b9063..fedc463 100644 --- a/tools/coverage_report.py +++ b/tools/coverage_report.py @@ -16,8 +16,14 @@ import trace import unittest ROOT = Path(__file__).resolve().parents[1] -CORE_TESTS = ('core_test', 'workflow_test', 'journal_failure_test', 'preflight_test', - 'settlement_test', 'proof_regression_test', 'readiness_regression_test', 'scenario_test', 'lifecycle_test') +CORE_TESTS = tuple(path.stem for path in sorted((ROOT / 'core/tests').glob('*_test.cpp'))) + + +def executable_lines(path): + # Python 3.14 can include artificial bytecode positions without a source + # line; these are not executable source statements and cannot be sorted. + return {line for line in trace._find_executable_linenos(str(path)) + if type(line) is int and line > 0} def production(path): @@ -35,13 +41,20 @@ def python_lines(destination): # are deliberately not counted as covered without an observed trace event. result = tracer.runfunc(unittest.TextTestRunner(verbosity=1).run, suite) counts = tracer.results().counts + observed = {} + resolved = {} + for (filename, line), count in counts.items(): + if count <= 0: + continue + if filename not in resolved: + resolved[filename] = Path(filename).resolve() + observed.setdefault(resolved[filename], set()).add(line) files = [] for path in sorted(ROOT.rglob('*.py')): if not production(path): continue - statements = set(trace._find_executable_linenos(str(path))) - hit = {line for (filename, line), count in counts.items() - if Path(filename).resolve() == path and count > 0} & statements + statements = executable_lines(path) + hit = observed.get(path, set()) & statements files.append(dict(path=str(path.relative_to(ROOT)), statements=len(statements), covered=len(hit), missing=sorted(statements-hit))) data = dict(metric='Python executable source lines; main test thread only', diff --git a/tools/http_concurrency.py b/tools/http_concurrency.py new file mode 100644 index 0000000..fc29b9a --- /dev/null +++ b/tools/http_concurrency.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Measure fresh-connection task admission bursts against an isolated local API. + +The planner is deliberately idle: receipts must not wait for planning. Latency +is reported as evidence, never asserted as a machine-independent unit-test SLA. +""" +import argparse +from concurrent.futures import ThreadPoolExecutor +import http.client +import json +import math +from pathlib import Path +import sys +import tempfile +import threading +import time + +ROOT=Path(__file__).resolve().parents[1] +sys.path.insert(0,str(ROOT/'coordinator')) +from robot_bt_coordinator.backends import ManualBackend +from robot_bt_coordinator.http_api import make_server +from robot_bt_coordinator.service import Coordinator + + +def probe(clients=10,rounds=3): + if not 1<=clients<=128 or not 1<=rounds<=100: + raise ValueError('clients must be 1..128 and rounds 1..100') + with tempfile.TemporaryDirectory(prefix='bt-http-probe-') as folder: + coordinator=Coordinator(str(Path(folder)/'tasks.db'),ManualBackend(),{'probe_robot'}) + server=make_server(coordinator,'127.0.0.1',0,'local-probe-token') + thread=threading.Thread(target=server.serve_forever,daemon=True) + thread.start() + samples=[] + try: + for iteration in range(rounds): + gate=threading.Barrier(clients) + def submit(index): + connection=http.client.HTTPConnection('127.0.0.1',server.server_port,timeout=10) + request=dict(client_request_id=f'probe-{iteration}-{index}',robot_id='probe_robot',instruction='local admission probe',known_info={}) + gate.wait(timeout=15) + started=time.perf_counter() + try: + connection.request('POST','/v1/tasks',json.dumps(request),{'Authorization':'Bearer local-probe-token','Content-Type':'application/json'}) + response=connection.getresponse() + body=json.loads(response.read()) + return dict(elapsed_ms=(time.perf_counter()-started)*1000,http_status=response.status,task_id=body.get('task_id'),status=body.get('status')) + finally:connection.close() + with ThreadPoolExecutor(max_workers=clients) as pool: + samples.extend(pool.map(submit,range(clients))) + successful=all(s['http_status']==202 and s['status']=='QUEUED' for s in samples) + successful=successful and len({s['task_id'] for s in samples})==clients*rounds + durations=sorted(s['elapsed_ms'] for s in samples) + return dict(clients=clients,rounds=rounds,requests=len(samples),all_accepted=successful, + p50_ms=durations[math.ceil(len(durations)*.5)-1], + p95_ms=durations[math.ceil(len(durations)*.95)-1],max_ms=max(durations), + listen_backlog=server.request_queue_size,planner_started=bool(coordinator.backend.plans),samples=samples) + finally: + server.shutdown();server.server_close();thread.join();coordinator.close() + + +def main(): + parser=argparse.ArgumentParser() + parser.add_argument('--clients',type=int,default=10) + parser.add_argument('--rounds',type=int,default=3) + args=parser.parse_args() + result=probe(args.clients,args.rounds) + print(json.dumps(result,indent=2)) + if not result['all_accepted'] or result['planner_started']:raise SystemExit(1) + + +if __name__=='__main__':main() diff --git a/tools/test_all.sh b/tools/test_all.sh index bd0b766..da59554 100755 --- a/tools/test_all.sh +++ b/tools/test_all.sh @@ -3,6 +3,7 @@ set -euo pipefail project_root="$(cd "$(dirname "$0")/.." && pwd)" cd "$project_root" bash tools/build_portable.sh +bash tools/test_recovery_policy.sh python3 -m unittest discover -s tests -p 'test_*.py' -v python3 ros2/bt_executor/tools/check_static.py python3 ros2/bt_executor/tools/test_ros_backend.py diff --git a/tools/test_recovery_policy.sh b/tools/test_recovery_policy.sh new file mode 100644 index 0000000..09d8482 --- /dev/null +++ b/tools/test_recovery_policy.sh @@ -0,0 +1,9 @@ +#!/usr/bin/env bash +set -euo pipefail +project_root="$(cd "$(dirname "$0")/.." && pwd)" +cd "$project_root" +mkdir -p build +"${CXX:-g++}" -std=c++17 -O2 -Wall -Wextra -Werror -UNDEBUG \ + -Iros2/bt_executor/include ros2/bt_executor/tools/test_recovery_policy.cpp \ + -o build/recovery_policy_test +./build/recovery_policy_test