实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1,16 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=61"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "robot-bt-coordinator"
|
||||
version = "1.2.0"
|
||||
description = "Robot behavior-tree task coordinator"
|
||||
requires-python = ">=3.10"
|
||||
|
||||
[project.scripts]
|
||||
robot-bt-coordinator = "robot_bt_coordinator.cli:main"
|
||||
|
||||
[tool.setuptools.packages.find]
|
||||
where = ["."]
|
||||
include = ["robot_bt_coordinator*"]
|
||||
@@ -0,0 +1,2 @@
|
||||
"""Robot task coordination; no motor control or learned model code."""
|
||||
__version__ = '1.0.0'
|
||||
@@ -0,0 +1,90 @@
|
||||
"""Transport boundary; simulation executes the actual portable C++ core."""
|
||||
import json
|
||||
from copy import deepcopy
|
||||
import queue
|
||||
import subprocess
|
||||
import threading
|
||||
from pathlib import Path
|
||||
from .errors import ApiError
|
||||
from .plan import validate_known
|
||||
|
||||
def demo_plan(info):
|
||||
slots=dict(info);slots.setdefault('quantity',1)
|
||||
target=slots.get('target_name','water');source=slots.get('source_location','shelf_A');dest=slots.get('destination','tote_A')
|
||||
args=[('NAVIGATE',{'destination':source}),('GROUND_TARGET',{'target':target}),('PICK',{'target':target}),('NAVIGATE',{'destination':dest}),('CHECK_FREE_SPACE',{'destination':dest}),('PLACE',{'target':target,'destination':dest})]
|
||||
return dict(schema_version=1,plan_version=1,task_type='pick_transport_place',goal=f'Move {target} from {source} to {dest}',slots=slots,missing_information=[],subtasks=[dict(id=f'S{i+1}',skill=s,arguments=a,depends_on=[] if i==0 else [f'S{i}']) for i,(s,a) in enumerate(args)])
|
||||
|
||||
class ManualBackend:
|
||||
"""Explicit test transport. Does not synthesize success or connect to hardware."""
|
||||
def __init__(self):self.events=queue.Queue();self.plans=[];self.executions=[];self.cancellations=[]
|
||||
def emit(self,event):self.events.put(event)
|
||||
def poll(self):
|
||||
out=[]
|
||||
for _ in range(1000):
|
||||
try:out.append(self.events.get_nowait())
|
||||
except queue.Empty:break
|
||||
return out
|
||||
def start_planning(self,task):self.plans.append(task)
|
||||
def start_execution(self,task):self.executions.append(deepcopy(task))
|
||||
def cancel(self,tid,run_id):self.cancellations.append((tid,run_id))
|
||||
def close(self):pass
|
||||
|
||||
class DemoBackend(ManualBackend):
|
||||
"""Mock planner + real C++ StageRunner with simulated skill endpoints only."""
|
||||
def __init__(self,executable,state_dir):
|
||||
super().__init__();self.executable=str(Path(executable).resolve());self.state_dir=Path(state_dir)
|
||||
if not Path(self.executable).is_file():raise ApiError('DEMO_NOT_BUILT','run tools/build_portable.sh first',503)
|
||||
self.state_dir.mkdir(parents=True,exist_ok=True);self.processes={};self.workers=[];self.lock=threading.Lock()
|
||||
def start_planning(self,t):
|
||||
super().start_planning(t)
|
||||
info=validate_known(t['request']['known_info'])
|
||||
if 'items' in info:
|
||||
from .plan_v2 import make_plan
|
||||
self.emit(dict(type='plan',task_id=t['task_id'],task_revision=t['task_revision'],planning_generation=t['planning_generation'],status='PLAN_READY',plan=make_plan(t['request']['instruction'],info,getattr(self,'route','OBJECT_TABLE'))));return
|
||||
missing=[k for k in ('target_name','source_location','destination') if not info.get(k)]
|
||||
event=dict(type='plan',task_id=t['task_id'],task_revision=t['task_revision'],planning_generation=t['planning_generation'])
|
||||
if missing:event.update(status='NEEDS_CLARIFICATION',questions=missing)
|
||||
else:event.update(status='PLAN_READY',plan=demo_plan(info))
|
||||
self.emit(event)
|
||||
def start_execution(self,t):
|
||||
# Plan construction uses only registered demonstration geometry.
|
||||
super().start_execution(t)
|
||||
ctx=t['context']
|
||||
if ctx.get('simulation') is not True:raise ApiError('SIMULATION_ONLY','demo backend requires a simulation site')
|
||||
command=[self.executable,'--events-jsonl','--target-ref',ctx['target_id'],'--destination-ref',ctx['destination_id'],'--journal',str(self.state_dir/(t['run_id']+'.journal')),'--task-id',t['task_id'],'--run-id',t['run_id'],'--robot-id',t['robot_id']]
|
||||
command+=['--route',ctx.get('route','LEGACY'),'--item-index',str(ctx.get('item_index',0))]
|
||||
p=subprocess.Popen(command,stdout=subprocess.PIPE,stderr=subprocess.PIPE,text=True)
|
||||
with self.lock:self.processes[t['run_id']]=p
|
||||
def collect():
|
||||
emitted=False
|
||||
try:
|
||||
# communicate drains both streams and has an overall simulation budget.
|
||||
out,err=p.communicate(timeout=60)
|
||||
sequence=0
|
||||
for line in out.splitlines():
|
||||
try:e=json.loads(line)
|
||||
except ValueError:continue
|
||||
if e.get('type')=='result':
|
||||
emitted=True
|
||||
evidence=e.get('evidence',{})
|
||||
self.emit(dict(e,type='execution_result',task_id=t['task_id'],run_id=t['run_id'],evidence=evidence,stop_confirmed=e.get('stop_confirmed',False)))
|
||||
elif e.get('type') in ('stage','progress'):
|
||||
sequence+=1;self.emit(dict(type='progress',task_id=t['task_id'],run_id=t['run_id'],sequence=sequence,stage=e.get('stage','')))
|
||||
if not emitted:self.emit(dict(type='execution_result',task_id=t['task_id'],run_id=t['run_id'],status='INTERVENTION_REQUIRED',stop_confirmed=False,error_code='SIMULATOR_NO_FINAL_RESULT'))
|
||||
except subprocess.TimeoutExpired:
|
||||
p.kill();p.communicate()
|
||||
self.emit(dict(type='execution_result',task_id=t['task_id'],run_id=t['run_id'],status='INTERVENTION_REQUIRED',stop_confirmed=False,error_code='SIMULATOR_TIMEOUT'))
|
||||
finally:
|
||||
with self.lock:self.processes.pop(t['run_id'],None)
|
||||
worker=threading.Thread(target=collect,daemon=True);worker.start();self.workers.append(worker)
|
||||
self.workers=[w for w in self.workers if w.is_alive()]
|
||||
def cancel(self,tid,run_id):
|
||||
super().cancel(tid,run_id)
|
||||
with self.lock:p=self.processes.get(run_id)
|
||||
if p is not None and p.poll() is None:p.terminate()
|
||||
# Killing a client is never manufactured into a confirmed physical result.
|
||||
def close(self):
|
||||
with self.lock:ps=list(self.processes.values())
|
||||
for p in ps:
|
||||
if p.poll() is None:p.terminate()
|
||||
for w in self.workers:w.join(timeout=2)
|
||||
@@ -0,0 +1,53 @@
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import signal
|
||||
import threading
|
||||
from pathlib import Path
|
||||
from .backends import DemoBackend
|
||||
from .http_api import make_server
|
||||
from .service import Coordinator
|
||||
from .plan import strict_json
|
||||
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description='Robot BT coordinator; explicit simulated or ROS2 backend')
|
||||
p.add_argument('--backend',choices=['mock','brain-mock','ros2'],default='mock')
|
||||
p.add_argument('--host',default='127.0.0.1');p.add_argument('--port',type=int,default=8088)
|
||||
p.add_argument('--state-dir',default='var');p.add_argument('--site',required=True)
|
||||
p.add_argument('--robot-id',default='robot_01');p.add_argument('--demo-executable',default='build/bt_demo')
|
||||
p.add_argument('--dense-progress',action='store_true');p.add_argument('--ros-namespace',default='/sim/robot_01');args=p.parse_args()
|
||||
token=os.environ.get('ROBOT_BT_API_TOKEN','')
|
||||
if not token:p.error('set ROBOT_BT_API_TOKEN before starting')
|
||||
site=strict_json(Path(args.site).read_text())
|
||||
state=Path(args.state_dir).resolve();state.mkdir(parents=True,exist_ok=True)
|
||||
if args.backend=='mock':backend=DemoBackend(args.demo_executable,state/'simulator')
|
||||
elif args.backend=='brain-mock':
|
||||
from robot_robobrain.demo_backend import BrainDemoBackend
|
||||
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})
|
||||
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',''))
|
||||
stopped=threading.Event()
|
||||
def worker():
|
||||
while not stopped.is_set():
|
||||
try:coordinator.tick()
|
||||
except Exception as e:
|
||||
# Stop scheduling after unexpected coordinator errors; never invent a terminal state.
|
||||
import sys
|
||||
print('Coordinator tick halted: '+type(e).__name__,file=sys.stderr)
|
||||
stopped.set();return
|
||||
stopped.wait(.05)
|
||||
thread=threading.Thread(target=worker,daemon=True);thread.start()
|
||||
def stop(*_):
|
||||
stopped.set();threading.Thread(target=server.shutdown,daemon=True).start()
|
||||
signal.signal(signal.SIGINT,stop);signal.signal(signal.SIGTERM,stop)
|
||||
print(json.dumps({'listening':f'http://{args.host}:{args.port}','backend':args.backend,'robot_id':args.robot_id}),flush=True)
|
||||
try:server.serve_forever(poll_interval=.1)
|
||||
finally:
|
||||
stopped.set();thread.join(timeout=2);server.server_close();coordinator.close()
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,4 @@
|
||||
class ApiError(Exception):
|
||||
def __init__(self, code, message, status=422):
|
||||
super().__init__(message)
|
||||
self.code, self.message, self.status = code, message, status
|
||||
@@ -0,0 +1,74 @@
|
||||
"""Small local/LAN JSON API. Put a TLS reverse proxy in front for remote clients."""
|
||||
import hmac
|
||||
import json
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
from urllib.parse import urlsplit, parse_qs
|
||||
from .errors import ApiError
|
||||
from .plan import strict_json
|
||||
|
||||
MAX_BODY=65536
|
||||
|
||||
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):
|
||||
server_version='RobotBT/1.0'
|
||||
def setup(self):
|
||||
super().setup();self.connection.settimeout(5)
|
||||
def log_message(self,*args):pass
|
||||
def reply(self,status,value):
|
||||
raw=json.dumps(value,ensure_ascii=False,allow_nan=False,separators=(',',':')).encode()
|
||||
self.send_response(status);self.send_header('Content-Type','application/json; charset=utf-8');self.send_header('Content-Length',str(len(raw)))
|
||||
self.send_header('Cache-Control','no-store');self.send_header('X-Content-Type-Options','nosniff');self.end_headers();self.wfile.write(raw)
|
||||
def body(self):
|
||||
if self.headers.get('Transfer-Encoding'):raise ApiError('INVALID_BODY','chunked requests are not supported',400)
|
||||
try:size=int(self.headers.get('Content-Length','0'))
|
||||
except ValueError:raise ApiError('INVALID_BODY','invalid Content-Length',400)
|
||||
if size<0 or size>MAX_BODY:raise ApiError('BODY_TOO_LARGE','maximum request body is 64 KiB',413)
|
||||
if size and self.headers.get_content_type()!='application/json':raise ApiError('CONTENT_TYPE','application/json required',415)
|
||||
try:
|
||||
raw=self.rfile.read(size)
|
||||
if len(raw)!=size:raise ValueError('truncated body')
|
||||
value=strict_json(raw.decode('utf-8')) if size else {}
|
||||
if not isinstance(value,dict):raise ValueError('expected JSON object')
|
||||
return value
|
||||
except (ValueError,UnicodeError):raise ApiError('INVALID_JSON','invalid or ambiguous JSON object',400)
|
||||
def auth(self,operator=False):
|
||||
expected=operator_token if operator else api_token
|
||||
supplied=self.headers.get('Authorization','')
|
||||
if not expected or not hmac.compare_digest(supplied,'Bearer '+expected):raise ApiError('UNAUTHORIZED','valid bearer token required',401)
|
||||
def dispatch(self):
|
||||
url=urlsplit(self.path);parts=url.path.strip('/').split('/');q=parse_qs(url.query)
|
||||
if self.command=='GET' and url.path=='/healthz':return 200,{'status':'ok','mode':type(coordinator.backend).__name__}
|
||||
operator=parts[-1:] == ['interventions']
|
||||
self.auth(operator)
|
||||
if self.command=='GET' and url.path=='/v1/capabilities':
|
||||
if coordinator.site.get('execution_route') in ('OBJECT_TABLE','SHELF_CELL'):
|
||||
return 200,{'schema_version':2,'robot_ids':sorted(coordinator.robots),'task_types':['pick_transport_place','multi_item_pick_transport_place'],'quantity_max':20,'route':coordinator.site['execution_route'],'skills':['NAVIGATE','ROBOBRAIN_SHELF_LOCALIZE','PICK','PLACE'],'event_delivery':'cursor_poll','tick_hz':20}
|
||||
return 200,{'schema_version':1,'robot_ids':sorted(coordinator.robots),'task_types':['pick_transport_place'],'quantity_max':1,'skills':['NAVIGATE','GROUND_TARGET','PICK','CHECK_FREE_SPACE','PLACE','ASK_USER'],'event_delivery':'cursor_poll','tick_hz':20}
|
||||
if self.command=='POST' and parts==['v1','tasks']:return 202,coordinator.submit(self.body())
|
||||
if len(parts)<3 or parts[:2]!=['v1','tasks']:raise ApiError('NOT_FOUND','endpoint not found',404)
|
||||
tid=parts[2]
|
||||
if self.command=='GET' and len(parts)==3:return 200,coordinator.get(tid)
|
||||
if self.command=='GET' and len(parts)==4 and parts[3]=='events':
|
||||
try:after=int(q.get('after',['0'])[0]);limit=int(q.get('limit',['100'])[0])
|
||||
except ValueError:raise ApiError('INVALID_CURSOR','integer cursor and limit required',400)
|
||||
events=coordinator.events(tid,after,limit)
|
||||
return 200,{'events':events,'next_cursor':events[-1]['event_id'] if events else after}
|
||||
if self.command=='POST' and len(parts)==4:
|
||||
data=self.body();action=parts[3]
|
||||
if action in ('cancel','pause','resume'):
|
||||
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)
|
||||
raise ApiError('NOT_FOUND','endpoint not found',404)
|
||||
def handle_request(self):
|
||||
try:status,payload=self.dispatch();self.reply(status,payload)
|
||||
except ApiError as e:self.reply(e.status,{'error_code':e.code,'message':e.message})
|
||||
except (BrokenPipeError,ConnectionResetError,TimeoutError):pass
|
||||
except Exception:
|
||||
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
|
||||
return server
|
||||
@@ -0,0 +1,86 @@
|
||||
"""Strict semantic-plan admission. Models select semantics, never executable code."""
|
||||
import json
|
||||
from .errors import ApiError
|
||||
|
||||
SKILLS = ('NAVIGATE', 'GROUND_TARGET', 'PICK', 'NAVIGATE', 'CHECK_FREE_SPACE', 'PLACE')
|
||||
SLOTS = {'target_name', 'quantity', 'source_location', 'destination'}
|
||||
|
||||
def fail(message):
|
||||
raise ApiError('INVALID_PLAN', message)
|
||||
|
||||
def strict_json(raw):
|
||||
def pairs(items):
|
||||
out = {}
|
||||
for k, v in items:
|
||||
if k in out: raise ValueError('duplicate JSON key: ' + k)
|
||||
out[k] = v
|
||||
return out
|
||||
return json.loads(raw, object_pairs_hook=pairs, parse_constant=lambda x: (_ for _ in ()).throw(ValueError('non-finite JSON')))
|
||||
|
||||
def canonical(value):
|
||||
return json.dumps(value, ensure_ascii=False, sort_keys=True, separators=(',', ':'), allow_nan=False)
|
||||
|
||||
def text(value, name, maximum=200):
|
||||
if not isinstance(value, str) or not value.strip() or len(value) > maximum:
|
||||
fail(name + ' must be nonempty text of bounded length')
|
||||
return value
|
||||
|
||||
def validate_known(info, complete=False):
|
||||
if isinstance(info,dict) and 'items' in info:
|
||||
from .plan_v2 import validate_slots
|
||||
return validate_slots(info,complete)
|
||||
if not isinstance(info, dict) or set(info) - SLOTS: fail('unsupported known_info/slots fields')
|
||||
for k,v in info.items():
|
||||
if k == 'quantity':
|
||||
if type(v) is not int or v != 1: fail('P0 supports exactly one item')
|
||||
else: text(v,k)
|
||||
if complete:
|
||||
for k in ('target_name','source_location','destination'):
|
||||
if k not in info: fail('missing slot: '+k)
|
||||
return dict(info)
|
||||
|
||||
def validate_plan(plan):
|
||||
if isinstance(plan,dict) and plan.get('schema_version')==2:
|
||||
from .plan_v2 import validate_v2
|
||||
return validate_v2(plan)
|
||||
if not isinstance(plan,dict): fail('plan must be an object')
|
||||
allowed={'schema_version','plan_version','task_type','goal','slots','missing_information','subtasks'}
|
||||
required={'task_type','goal','slots','missing_information','subtasks'}
|
||||
if set(plan)-allowed or required-set(plan): fail('unknown or missing plan fields')
|
||||
if plan.get('schema_version',1)!=1 or type(plan.get('schema_version',1)) is not int: fail('unsupported schema_version')
|
||||
if type(plan.get('plan_version',1)) is not int or not 1<=plan.get('plan_version',1)<=4294967295: fail('invalid plan_version')
|
||||
if plan['task_type']!='pick_transport_place': fail('unsupported task_type')
|
||||
text(plan['goal'],'goal',1000)
|
||||
if isinstance(plan['slots'],dict) and 'items' in plan['slots']:fail('items requires schema_version=2')
|
||||
validate_known(plan['slots'],complete=True)
|
||||
if not isinstance(plan['missing_information'],list) or plan['missing_information']: fail('plan has unresolved information')
|
||||
tasks=plan['subtasks']
|
||||
# The sole non-motion plan variant asks one question, then requires replanning.
|
||||
if isinstance(tasks,list) and len(tasks)==1 and isinstance(tasks[0],dict) and tasks[0].get('skill')=='ASK_USER':
|
||||
q=tasks[0]
|
||||
if set(q)!={'id','skill','arguments','depends_on'} or q['depends_on']!=[] or not isinstance(q['arguments'],dict) or set(q['arguments'])!={'question'}:
|
||||
fail('invalid ASK_USER plan')
|
||||
text(q['id'],'subtask.id',80);text(q['arguments']['question'],'question',500)
|
||||
result=json.loads(canonical(plan));result.setdefault('schema_version',1);result.setdefault('plan_version',1)
|
||||
return result
|
||||
if not isinstance(tasks,list) or len(tasks)!=6: fail('P0 requires the six-step fixed workflow')
|
||||
ids=set()
|
||||
for s in tasks:
|
||||
if not isinstance(s,dict) or set(s)!={'id','skill','arguments','depends_on'}: fail('invalid subtask structure')
|
||||
text(s['id'],'subtask.id',80)
|
||||
if s['id'] in ids: fail('duplicate subtask id')
|
||||
ids.add(s['id'])
|
||||
if not isinstance(s['depends_on'],list) or any(not isinstance(d,str) for d in s['depends_on']):fail('invalid dependencies')
|
||||
if len(set(s['depends_on']))!=len(s['depends_on']): fail('duplicate dependency')
|
||||
# A supported fixed chain is a stricter acceptance set than an arbitrary DAG.
|
||||
slots=plan['slots']
|
||||
expected_args=[{'destination':slots['source_location']},{'target':slots['target_name']},{'target':slots['target_name']},{'destination':slots['destination']},{'destination':slots['destination']},{'target':slots['target_name'],'destination':slots['destination']}]
|
||||
for i,s in enumerate(tasks):
|
||||
if any(d not in ids for d in s['depends_on']): fail('missing dependency')
|
||||
if s['depends_on'] != ([] if i==0 else [tasks[i-1]['id']]): fail('unsupported topology or dependency cycle')
|
||||
if s['skill']!=SKILLS[i]: fail('unknown skill or unsupported skill ordering')
|
||||
if s['arguments']!=expected_args[i]: fail('arguments must exactly match approved task slots')
|
||||
result=json.loads(canonical(plan))
|
||||
result.setdefault('schema_version',1);result.setdefault('plan_version',1)
|
||||
result['slots'].setdefault('quantity',1)
|
||||
return result
|
||||
@@ -0,0 +1,69 @@
|
||||
"""Frozen September DR schema. Generated plan never owns route or motion policy."""
|
||||
import copy
|
||||
from .plan import fail, text
|
||||
MAX_ITEMS=20
|
||||
ROUTES=('OBJECT_TABLE','SHELF_CELL')
|
||||
|
||||
def validate_slots(s, complete=True):
|
||||
if not isinstance(s,dict) or set(s)-{'items','destination'}:fail('v2 slots must be items/destination object')
|
||||
if complete and set(s)!={'items','destination'}:fail('missing items or destination')
|
||||
if 'destination' in s:text(s['destination'],'destination')
|
||||
if 'items' in s:
|
||||
if not isinstance(s['items'],list) or not 1<=len(s['items'])<=MAX_ITEMS:fail('items must be a bounded ordered list')
|
||||
for item in s['items']:
|
||||
if not isinstance(item,dict) or set(item)!={'target_name','quantity','source_location'}:fail('invalid item fields')
|
||||
text(item['target_name'],'target_name');text(item['source_location'],'source_location')
|
||||
if type(item['quantity']) is not int or not 1<=item['quantity']<=MAX_ITEMS:fail('invalid quantity')
|
||||
if sum(i['quantity'] for i in s['items'])>MAX_ITEMS:fail('too many physical items')
|
||||
return copy.deepcopy(s)
|
||||
|
||||
def instances(plan):
|
||||
if plan['schema_version']==1:return [dict(plan['slots'],item_index=0)]
|
||||
result=[]
|
||||
for item in plan['slots']['items']:
|
||||
for n in range(item['quantity']):result.append(dict(item,quantity=1,destination=plan['slots']['destination'],item_index=len(result),instance_index=n+1))
|
||||
return result
|
||||
|
||||
def steps(route,target,source,dest,item_index):
|
||||
rows=[('NAVIGATE',{'target':target}),('PICK',{'target':target}),('NAVIGATE',{'destination':dest}),('PLACE',{'target':target,'destination':dest})]
|
||||
if route=='SHELF_CELL':rows=[('NAVIGATE',{'source_location':source,'mode':'observation'}),('ROBOBRAIN_SHELF_LOCALIZE',{'target':target}),('NAVIGATE',{'source_location':source,'mode':'shelf_cell'})]+rows[1:]
|
||||
return [(skill,dict(args,item_index=item_index)) for skill,args in rows]
|
||||
|
||||
def make_plan(instruction,slots,route,version=1):
|
||||
validate_slots(slots)
|
||||
if route not in ROUTES:fail('unsupported route')
|
||||
count=sum(i['quantity'] for i in slots['items'])
|
||||
p=dict(schema_version=2,plan_version=version,task_type='pick_transport_place' if count==1 else 'multi_item_pick_transport_place',goal=instruction,route=route,slots=copy.deepcopy(slots),missing_information=[],subtasks=[])
|
||||
for item in instances(p):
|
||||
for skill,args in steps(route,item['target_name'],item['source_location'],item['destination'],item['item_index']):
|
||||
n=len(p['subtasks']);p['subtasks'].append(dict(id=f'S{n+1}',skill=skill,arguments=args,depends_on=[] if n==0 else [f'S{n}']))
|
||||
return p
|
||||
|
||||
def validate_v2(p):
|
||||
if set(p)!={'schema_version','plan_version','task_type','goal','route','slots','missing_information','subtasks'}:fail('invalid v2 plan fields')
|
||||
if type(p['schema_version']) is not int or p['schema_version']!=2:fail('invalid schema version')
|
||||
if type(p['plan_version']) is not int or not 1<=p['plan_version']<=4294967295:fail('invalid plan version')
|
||||
text(p['goal'],'goal',1000)
|
||||
expected=make_plan(p['goal'],p['slots'],p['route'],p['plan_version'])
|
||||
if p['task_type']!=expected['task_type'] or p['missing_information']!=[]:fail('unresolved or wrong task type')
|
||||
tasks=p['subtasks']
|
||||
if not isinstance(tasks,list) or len(tasks)!=len(expected['subtasks']):fail('quantity does not match complete item chains')
|
||||
ids=set()
|
||||
for n,(s,want) in enumerate(zip(tasks,expected['subtasks'])):
|
||||
if not isinstance(s,dict) or set(s)!={'id','skill','arguments','depends_on'}:fail('invalid subtask fields')
|
||||
text(s['id'],'id',80)
|
||||
if s['id'] in ids:fail('duplicate subtask')
|
||||
ids.add(s['id'])
|
||||
if s['depends_on']!=([] if n==0 else [tasks[n-1]['id']]):fail('broken ordered dependency')
|
||||
if not isinstance(s['arguments'],dict) or type(s['arguments'].get('item_index')) is not int:fail('invalid item identity')
|
||||
if s['skill']!=want['skill'] or s['arguments']!=want['arguments']:fail('skill/arguments/order mismatch')
|
||||
return copy.deepcopy(p)
|
||||
|
||||
def item_plan(p,index):
|
||||
"""Single-item executor plan keeps parent-wide item_index (including after reboot)."""
|
||||
if p['schema_version']==1:return copy.deepcopy(p)
|
||||
item=instances(p)[index]
|
||||
q=make_plan(p['goal'],{'items':[{k:item[k] for k in ('target_name','quantity','source_location')}],'destination':item['destination']},p['route'],p['plan_version'])
|
||||
# Executor receives a one-item plan with canonical local step index=0.
|
||||
# Business item_index travels independently in authenticated context.
|
||||
return q
|
||||
@@ -0,0 +1,41 @@
|
||||
"""Read-only event replay: reconstruct approvals, decisions and committed deliveries.
|
||||
|
||||
No ROS import or execution transport exists in this module. Sensor/model inference
|
||||
is not rerun, and this result must never be used as live robot state.
|
||||
"""
|
||||
import argparse
|
||||
import json
|
||||
import sqlite3
|
||||
from pathlib import Path
|
||||
from .plan import validate_plan
|
||||
from .plan_v2 import instances
|
||||
|
||||
def replay_events(events):
|
||||
status='RECEIVED';version=0;cursor=0;delivered={};stages=[];plans=[];errors=[]
|
||||
for e in events:
|
||||
if type(e['event_id']) is not int or e['event_id']<=cursor:raise ValueError('non-monotonic event cursor')
|
||||
cursor=e['event_id']
|
||||
if e['kind'] in ('state','progress'):
|
||||
if e['status_version']!=version+1:raise ValueError('missing or non-monotonic status version')
|
||||
version=e['status_version'];status=e['status']
|
||||
if e.get('error_code'):errors.append(e['error_code'])
|
||||
elif e['kind']=='plan_approved':
|
||||
plans.append({'run_id':e['run_id'],'plan':validate_plan(e['plan'])})
|
||||
elif e['kind']=='delivery_committed':
|
||||
key=e['item_index']
|
||||
if key in delivered and delivered[key]!=e['evidence']:raise ValueError('conflicting delivery evidence')
|
||||
delivered[key]=e['evidence']
|
||||
elif e['kind']=='execution_result':
|
||||
payload=e['payload'];stages.append({'run_id':payload.get('run_id'),'status':payload.get('status'),'stop_confirmed':payload.get('stop_confirmed')})
|
||||
if status=='SUCCEEDED' and (not plans or set(delivered)!=set(range(len(instances(plans[-1]['plan']))))):raise ValueError('success without committed delivery')
|
||||
return {'mode':'offline_audit_replay','status':status,'status_version':version,'last_event_id':cursor,'completed_quantity':len(delivered),'plans':plans,'results':stages,'errors':errors,'connects_to_robot':False}
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser();p.add_argument('--db',required=True);p.add_argument('--task-id',required=True);args=p.parse_args()
|
||||
path=Path(args.db).resolve()
|
||||
with sqlite3.connect(path.as_uri()+'?mode=ro',uri=True) as db:
|
||||
rows=db.execute('SELECT event_id,kind,created_at,data FROM events WHERE task_id=? ORDER BY event_id',(args.task_id,)).fetchall()
|
||||
if not rows:raise SystemExit('no matching task events')
|
||||
events=[dict(event_id=r[0],kind=r[1],created_at=r[2],**json.loads(r[3])) for r in rows]
|
||||
print(json.dumps(replay_events(events),ensure_ascii=False,indent=2))
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,396 @@
|
||||
"""Nonblocking ROS2 Humble adapter. Importing this module does not require ROS.
|
||||
|
||||
The coordinator persists dispatch intent before calling this adapter. A lost goal
|
||||
response therefore reports stop unknown; it never fabricates a clean rejection.
|
||||
An externally supplied node must be spun by its owner. An owned node is spun by
|
||||
a dedicated single-threaded executor, independently of coordinator polling.
|
||||
"""
|
||||
from collections import deque
|
||||
from copy import deepcopy
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from uuid import uuid4
|
||||
|
||||
from .plan import canonical, strict_json
|
||||
|
||||
CONTEXT_FIELDS = ('schema_version', 'registry_version', 'robot_id', 'target_id',
|
||||
'source_shelf', 'destination_id', 'observe_location',
|
||||
'destination_location', 'task_revision')
|
||||
|
||||
|
||||
class RosBackend:
|
||||
def __init__(self, node=None, config=None, namespace=None):
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.context import Context
|
||||
from rclpy.executors import SingleThreadedExecutor
|
||||
from bt_skill_interfaces.action import PlanTask, ExecuteTask
|
||||
|
||||
self.config = dict(config or {})
|
||||
if namespace is not None:
|
||||
self.config['namespace'] = namespace
|
||||
self._rclpy = rclpy
|
||||
self._own_node = node is None
|
||||
self._context = None
|
||||
self._executor = None
|
||||
self._thread = None
|
||||
if node is None:
|
||||
ns = self.config.get('namespace', '')
|
||||
if not isinstance(ns, str) or not ns.startswith('/') or ns == '/':
|
||||
raise ValueError('ROS backend requires explicit robot namespace')
|
||||
self._context = Context()
|
||||
rclpy.init(context=self._context)
|
||||
node = rclpy.create_node('bt_coordinator_bridge', namespace=ns,
|
||||
context=self._context)
|
||||
self._executor = SingleThreadedExecutor(context=self._context)
|
||||
self._executor.add_node(node)
|
||||
self.node = node
|
||||
self._PlanTask, self._ExecuteTask = PlanTask, ExecuteTask
|
||||
self._planner = ActionClient(node, PlanTask, self.config.get('plan_action', 'tasks/plan'))
|
||||
self._execution = ActionClient(node, ExecuteTask, self.config.get('execute_action', 'tasks/execute'))
|
||||
self._events = deque()
|
||||
self._planning = {}
|
||||
self._runs = {}
|
||||
self._lock = threading.RLock()
|
||||
self._closed = False
|
||||
self._acceptance_timeout = float(self.config.get('acceptance_timeout', 3.0))
|
||||
self._feedback_timeout = float(self.config.get('feedback_timeout', 5.0))
|
||||
self._stop_timeout = float(self.config.get('stop_timeout', 7.0))
|
||||
for value in (self._acceptance_timeout, self._feedback_timeout, self._stop_timeout):
|
||||
if not math.isfinite(value) or value <= 0:
|
||||
raise ValueError('backend timeout must be finite and positive')
|
||||
self._dense_subscription=None
|
||||
if self.config.get('dense_progress_enabled',False):
|
||||
from bt_skill_interfaces.msg import DenseProgress
|
||||
self._dense_subscription=node.create_subscription(DenseProgress,'monitor/dense_progress',self._dense_feedback,10)
|
||||
if self._executor is not None:
|
||||
self._thread = threading.Thread(target=self._executor.spin,
|
||||
name='bt-ros-callbacks', daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
@staticmethod
|
||||
def _duration(value, seconds):
|
||||
seconds = float(seconds)
|
||||
if not math.isfinite(seconds) or not 0 < seconds <= 3600:
|
||||
raise ValueError('invalid action timeout')
|
||||
value.sec = int(seconds)
|
||||
value.nanosec = int((seconds - int(seconds)) * 1_000_000_000)
|
||||
|
||||
def _emit(self, event):
|
||||
with self._lock:
|
||||
self._events.append(event)
|
||||
|
||||
@staticmethod
|
||||
def _planning_event(rec, status, **extra):
|
||||
return dict(type='plan', task_id=rec['task_id'],
|
||||
task_revision=rec['task_revision'],
|
||||
planning_generation=rec['planning_generation'], status=status, **extra)
|
||||
|
||||
@staticmethod
|
||||
def _execution_event(rec, status, stopped, quantity=0, evidence=None, error=''):
|
||||
return dict(type='execution_result', task_id=rec['task_id'], run_id=rec['run_id'],
|
||||
status=status, stop_confirmed=stopped, completed_quantity=quantity,
|
||||
evidence=evidence or {}, error_code=error)
|
||||
|
||||
def start_planning(self, task_dict):
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
raise RuntimeError('ROS backend is closed')
|
||||
task = deepcopy(task_dict)
|
||||
key = (task['task_id'], task['task_revision'], task['planning_generation'])
|
||||
if key in self._planning:
|
||||
return
|
||||
rec = {k: task[k] for k in ('task_id', 'task_revision', 'planning_generation')}
|
||||
rec.update(sent_at=time.monotonic(), handle=None, done=False)
|
||||
self._planning[key] = rec
|
||||
if not self._planner.server_is_ready():
|
||||
rec['done'] = True
|
||||
self._emit(self._planning_event(rec, 'FAILED', error_code='PLANNER_UNAVAILABLE'))
|
||||
return
|
||||
goal = self._PlanTask.Goal()
|
||||
goal.task_id = task['task_id']
|
||||
goal.task_revision = task['task_revision']
|
||||
goal.planning_generation = task['planning_generation']
|
||||
# Preserve the exact user instruction/known_info, never reconstruct
|
||||
# the original request from a previous or partial plan.
|
||||
goal.instruction = task['request']['instruction']
|
||||
goal.known_info_json = canonical(task['request'].get('known_info', {}))
|
||||
goal.context_snapshot_json = canonical(task.get('context') or self.config.get('planning_context', {}))
|
||||
goal.constraints_json = canonical({'schema_version': 1, 'quantity': 1,
|
||||
'supported_skills': ['NAVIGATE', 'GROUND_TARGET', 'PICK', 'CHECK_FREE_SPACE', 'PLACE', 'ASK_USER'],
|
||||
'robot_id': task['robot_id']})
|
||||
route=self.config.get('planning_context',{}).get('execution_route')
|
||||
if route in ('OBJECT_TABLE','SHELF_CELL'):
|
||||
goal.constraints_json=canonical({'schema_version':2,'route':route,'max_items':20,'execution':'sequential','robot_id':task['robot_id']})
|
||||
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]
|
||||
try:
|
||||
handle = future.result()
|
||||
if not handle or not handle.accepted:
|
||||
raise RuntimeError('planner rejected goal')
|
||||
rec['handle'] = handle
|
||||
if rec['done']:
|
||||
handle.cancel_goal_async()
|
||||
return
|
||||
handle.get_result_async().add_done_callback(lambda completed: self._plan_result(key, completed))
|
||||
except Exception as exc:
|
||||
if not rec['done']:
|
||||
rec['done'] = True
|
||||
self._emit(self._planning_event(rec, 'FAILED', error_code='PLANNER_TRANSPORT_ERROR', error=str(exc)))
|
||||
|
||||
def _plan_result(self, key, future):
|
||||
with self._lock:
|
||||
rec = self._planning[key]
|
||||
if rec['done']:
|
||||
return
|
||||
rec['done'] = True
|
||||
try:
|
||||
wrapped = future.result()
|
||||
if wrapped.status != 4:
|
||||
raise ValueError('planning native result was not SUCCEEDED')
|
||||
result = wrapped.result
|
||||
if result.status == 0:
|
||||
plan = strict_json(result.task_plan_json)
|
||||
self._emit(self._planning_event(rec, 'PLAN_READY', plan=plan, planning_record_ref=getattr(result,'planning_record_ref','')))
|
||||
elif result.status == 1:
|
||||
data = strict_json(result.task_plan_json)
|
||||
missing = data.get('missing_information', []) if isinstance(data, dict) else []
|
||||
questions = [v if isinstance(v, str) else v.get('question', '')
|
||||
for v in missing if isinstance(v, (str, dict))]
|
||||
if not questions:
|
||||
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))
|
||||
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)))
|
||||
|
||||
def start_execution(self, task_dict):
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
raise RuntimeError('ROS backend is closed')
|
||||
task = deepcopy(task_dict)
|
||||
key = (task['task_id'], task['run_id'])
|
||||
if key in self._runs:
|
||||
return
|
||||
now = time.monotonic()
|
||||
rec = dict(task_id=key[0], run_id=key[1], sent_at=now, last_feedback=now,
|
||||
sent_ros_ns=self.node.get_clock().now().nanoseconds,
|
||||
sequence=0, handle=None, cancel_intent=False, cancel_at=None,
|
||||
unknown_emitted=False, done=False, wire_uuid=None)
|
||||
self._runs[key] = rec
|
||||
goal = self._ExecuteTask.Goal()
|
||||
trace = goal.trace
|
||||
trace.task_id, trace.run_id, trace.subtask_id = key[0], key[1], 'execute_task'
|
||||
trace.attempt = 1
|
||||
trace.task_revision = task['task_revision']
|
||||
trace.plan_version = task['plan']['plan_version']
|
||||
trace.execution_generation = task.get('execution_generation', task['planning_generation'])
|
||||
goal.approved_plan_json = canonical(task.get('execution_plan') or task['plan'])
|
||||
# Only symbolic keys cross the boundary. The C++ node reads trusted
|
||||
# registered coordinates/postures from its own site_config_file.
|
||||
context={k:task['context'][k] for k in CONTEXT_FIELDS}
|
||||
if task['plan']['schema_version']==2:context.update(route=task['context']['route'],item_index=task['context']['item_index'])
|
||||
goal.context_json=canonical(context)
|
||||
self._duration(goal.timeout, self.config.get('execution_timeout', 600))
|
||||
if not self._execution.server_is_ready():
|
||||
rec['done'] = True
|
||||
self._emit(self._execution_event(rec, 'FAILED', True, error='EXECUTOR_UNAVAILABLE'))
|
||||
return
|
||||
try:
|
||||
# rclpy allows an explicit UUID: capture it before sending and
|
||||
# expose it in events; coordinator durable run intent already exists.
|
||||
from unique_identifier_msgs.msg import UUID
|
||||
wire = uuid4()
|
||||
wire_id = UUID(uuid=list(wire.bytes))
|
||||
rec['wire_uuid'] = str(wire)
|
||||
future = self._execution.send_goal_async(goal, goal_uuid=wire_id,
|
||||
feedback_callback=lambda feedback: self._feedback(key, feedback))
|
||||
future.add_done_callback(lambda completed: self._execution_accepted(key, completed))
|
||||
except Exception as exc:
|
||||
self._unknown(rec, 'DISPATCH_ACCEPTANCE_UNKNOWN: ' + str(exc))
|
||||
|
||||
def _dense_feedback(self,m):
|
||||
with self._lock:
|
||||
rec=self._runs.get((m.trace.task_id,m.trace.run_id))
|
||||
if not rec or rec['done'] or not m.progress_valid or m.state not in ('RUNNING','STALLED','REGRESSED'):return
|
||||
at=m.observed_at.sec*1_000_000_000+m.observed_at.nanosec
|
||||
now=self.node.get_clock().now().nanoseconds
|
||||
if not rec['sent_ros_ns']<=at<=now or now-at>2_000_000_000 or not math.isfinite(m.progress) or not 0<=m.progress<=1:return
|
||||
seen=rec.setdefault('dense_sequence',{})
|
||||
if m.sequence<=seen.get(m.trace.subtask_id,0):return
|
||||
seen[m.trace.subtask_id]=m.sequence
|
||||
self._emit(dict(type='advisory',task_id=m.trace.task_id,run_id=m.trace.run_id,subtask_id=m.trace.subtask_id,state=m.state,progress=m.progress,record_ref=m.record_ref,completion_authority=False))
|
||||
|
||||
def _unknown(self, rec, reason):
|
||||
if rec['done'] or rec['unknown_emitted']:
|
||||
return
|
||||
rec['unknown_emitted'] = True
|
||||
rec['cancel_intent'] = True
|
||||
rec['cancel_at'] = rec['cancel_at'] or time.monotonic()
|
||||
self._emit(self._execution_event(rec, 'INTERVENTION_REQUIRED', False,
|
||||
evidence={'ros_goal_uuid': rec['wire_uuid']}, error=reason))
|
||||
if rec['handle'] is not None:
|
||||
self._send_cancel(rec)
|
||||
|
||||
def _execution_accepted(self, key, future):
|
||||
with self._lock:
|
||||
rec = self._runs[key]
|
||||
try:
|
||||
handle = future.result()
|
||||
if not handle or not handle.accepted:
|
||||
rec['done'] = True
|
||||
self._emit(self._execution_event(rec, 'FAILED', True,
|
||||
evidence={'ros_goal_uuid': rec['wire_uuid']}, error='EXECUTOR_REJECTED'))
|
||||
return
|
||||
rec['handle'] = handle
|
||||
handle.get_result_async().add_done_callback(lambda completed: self._execution_result(key, completed))
|
||||
if rec['cancel_intent']:
|
||||
self._send_cancel(rec)
|
||||
except Exception as exc:
|
||||
self._unknown(rec, 'ACCEPTANCE_UNKNOWN: ' + str(exc))
|
||||
|
||||
def _feedback(self, key, wrapped):
|
||||
with self._lock:
|
||||
rec = self._runs[key]
|
||||
if rec['done']:
|
||||
return
|
||||
msg = wrapped.feedback
|
||||
at = msg.stamp.sec * 1_000_000_000 + msg.stamp.nanosec
|
||||
now = self.node.get_clock().now().nanoseconds
|
||||
if (msg.sequence <= rec['sequence'] or at <= 0 or at > now or
|
||||
now - at > int(self._feedback_timeout * 1e9) or not msg.stage):
|
||||
return
|
||||
try:
|
||||
status = strict_json(msg.status_json)
|
||||
if not isinstance(status, dict):
|
||||
return
|
||||
except (ValueError, TypeError):
|
||||
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))
|
||||
|
||||
def _execution_result(self, key, future):
|
||||
with self._lock:
|
||||
rec = self._runs[key]
|
||||
if rec['done']:
|
||||
return
|
||||
try:
|
||||
wrapped = future.result()
|
||||
result = wrapped.result
|
||||
business = int(result.result.status)
|
||||
native = int(wrapped.status)
|
||||
consistent = ((native == 4 and business == 0) or
|
||||
(native == 5 and business == 2) or
|
||||
(native == 6 and business in (1, 3, 4)))
|
||||
if not consistent:
|
||||
raise ValueError('native/business result mismatch')
|
||||
evidence = strict_json(result.evidence_json)
|
||||
if not isinstance(evidence, dict):
|
||||
raise ValueError('result evidence must be object')
|
||||
stopped_at = result.result.stopped_at.sec * 1_000_000_000 + result.result.stopped_at.nanosec
|
||||
ros_now = self.node.get_clock().now().nanoseconds
|
||||
stopped = (result.result.stop_state == 1 and
|
||||
max(0, rec.get('sent_ros_ns', 0)) < stopped_at <= ros_now and
|
||||
ros_now - stopped_at <= int(self._feedback_timeout * 1e9) and
|
||||
bool(result.result.stop_evidence_ref) and evidence.get('stop_confirmed') is True)
|
||||
quantity = int(result.completed_quantity)
|
||||
if quantity not in (0, 1):
|
||||
raise ValueError('invalid completed quantity')
|
||||
status = ('SUCCEEDED' if business == 0 else 'CANCELED' if business == 2 else 'FAILED')
|
||||
if not stopped or evidence.get('status') == 'INTERVENTION_REQUIRED':
|
||||
status = 'INTERVENTION_REQUIRED'
|
||||
evidence['ros_goal_uuid'] = rec['wire_uuid']
|
||||
rec['done'] = True
|
||||
self._emit(self._execution_event(rec, status, stopped, quantity, evidence, result.result.error_code))
|
||||
except Exception as exc:
|
||||
self._unknown(rec, 'RESULT_PROTOCOL_ERROR: ' + str(exc))
|
||||
|
||||
def _send_cancel(self, rec):
|
||||
try:
|
||||
# Cancel response is deliberately not translated into stop evidence.
|
||||
rec['handle'].cancel_goal_async()
|
||||
except Exception:
|
||||
pass # deadline below remains responsible for STOP_UNKNOWN
|
||||
|
||||
def cancel(self, task_id, run_id):
|
||||
with self._lock:
|
||||
canceled_planning = False
|
||||
for planning in self._planning.values():
|
||||
if planning['task_id'] == task_id and not planning['done']:
|
||||
planning['done'] = True
|
||||
canceled_planning = True
|
||||
if planning['handle'] is not None:
|
||||
planning['handle'].cancel_goal_async()
|
||||
rec = self._runs.get((task_id, run_id))
|
||||
if rec is None:
|
||||
if canceled_planning or not run_id:
|
||||
return
|
||||
self._emit(dict(type='execution_result', task_id=task_id, run_id=run_id,
|
||||
status='INTERVENTION_REQUIRED', stop_confirmed=False,
|
||||
completed_quantity=0, evidence={}, error_code='UNKNOWN_RUN'))
|
||||
return
|
||||
if rec['done']:
|
||||
return
|
||||
rec['cancel_intent'] = True
|
||||
rec['cancel_at'] = rec['cancel_at'] or time.monotonic()
|
||||
if rec['handle'] is not None:
|
||||
self._send_cancel(rec)
|
||||
|
||||
pause = cancel
|
||||
|
||||
def poll(self):
|
||||
with self._lock:
|
||||
now = time.monotonic()
|
||||
for rec in self._planning.values():
|
||||
if not rec['done'] and now - rec['sent_at'] > float(self.config.get('planning_timeout', 25)):
|
||||
rec['done'] = True
|
||||
if rec['handle'] is not None:
|
||||
rec['handle'].cancel_goal_async()
|
||||
self._emit(self._planning_event(rec, 'FAILED', error_code='PLANNING_TIMEOUT'))
|
||||
for rec in self._runs.values():
|
||||
if rec['done']:
|
||||
continue
|
||||
if rec['handle'] is None and now - rec['sent_at'] > self._acceptance_timeout:
|
||||
self._unknown(rec, 'ACCEPTANCE_TIMEOUT')
|
||||
elif rec['handle'] is not None and now - rec['last_feedback'] > self._feedback_timeout:
|
||||
self._unknown(rec, 'FEEDBACK_TIMEOUT')
|
||||
if rec['cancel_at'] is not None and now - rec['cancel_at'] > self._stop_timeout:
|
||||
self._unknown(rec, 'STOP_TIMEOUT')
|
||||
events = list(self._events)
|
||||
self._events.clear()
|
||||
return events
|
||||
|
||||
def close(self):
|
||||
with self._lock:
|
||||
if self._closed:
|
||||
return
|
||||
for rec in self._runs.values():
|
||||
if not rec['done']:
|
||||
rec['cancel_intent'] = True
|
||||
if rec['handle'] is not None:
|
||||
self._send_cancel(rec)
|
||||
# Closing does not assert physical stopping. Coordinator restart keeps
|
||||
# all dispatched unfinished runs quarantined for reconciliation.
|
||||
self._closed = True
|
||||
# Do not hold _lock while joining a callback that might be waiting for it.
|
||||
if self._executor is not None:
|
||||
self._executor.shutdown(timeout_sec=2)
|
||||
if self._thread is not None:
|
||||
self._thread.join(timeout=2)
|
||||
self._executor.remove_node(self.node)
|
||||
self._planner.destroy()
|
||||
self._execution.destroy()
|
||||
if self._own_node:
|
||||
self.node.destroy_node()
|
||||
self._context.shutdown()
|
||||
@@ -0,0 +1,269 @@
|
||||
"""Task state machine. Serialized transitions; persist intent before external calls."""
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
from .errors import ApiError
|
||||
from .plan import canonical, text, validate_known, validate_plan
|
||||
from .store import Store
|
||||
from .plan_v2 import instances, item_plan
|
||||
|
||||
TERMINAL = {'SUCCEEDED','FAILED','CANCELED','EXPIRED'}
|
||||
EXECUTION = {'READY','EXECUTING','PAUSING','CANCELING','INTERVENTION_REQUIRED'}
|
||||
|
||||
def demo_site():
|
||||
return {'schema_version':1,'registry_version':1,'simulation':True,
|
||||
'locations':{'observe_A':{'frame_id':'map','x':0.,'y':0.,'z':0.,'qx':0.,'qy':0.,'qz':0.,'qw':1.},
|
||||
'shelf_A_stop':{'frame_id':'map','x':1.,'y':0.,'z':0.,'qx':0.,'qy':0.,'qz':0.,'qw':1.},
|
||||
'tote_A_stop':{'frame_id':'map','x':2.,'y':0.,'z':0.,'qx':0.,'qy':0.,'qz':0.,'qw':1.}},
|
||||
'sources':{'shelf_A':{'observe_location':'observe_A','shelf_id':'shelf_A'}},
|
||||
'destinations':{'tote_A':{'location':'tote_A_stop','region_ref':'tote_A'}},
|
||||
'parking_locations':{'shelf_A/FRONT/1':'shelf_A_stop'},
|
||||
'allowed_postures':['pregrasp','transport'],'transport_posture':'transport'}
|
||||
|
||||
class Coordinator:
|
||||
def __init__(self,path,backend,robots,site=None,clock=time.time,queue_timeout=3600,steady=time.monotonic,planning_timeout=30):
|
||||
self.lock=threading.RLock();self.store=Store(path);self.backend=backend
|
||||
self.robots=set(robots);self.site=site if site is not None else demo_site()
|
||||
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():
|
||||
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':
|
||||
t['planning_generation']+=1
|
||||
self._set(t,'QUEUED','PLANNER_RESTART')
|
||||
def close(self):
|
||||
with self.lock:
|
||||
if not self.closed:
|
||||
self.backend.close();self.store.close();self.closed=True
|
||||
def _task(self,tid):
|
||||
t=self.store.get(tid)
|
||||
if t is None:raise ApiError('NOT_FOUND','task not found',404)
|
||||
return t
|
||||
def _set(self,t,status,error='',kind='state'):
|
||||
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())
|
||||
def get(self,tid):
|
||||
with self.lock:return self._task(tid)
|
||||
def events(self,tid,after=0,limit=100):
|
||||
if type(after) is not int or after<0 or type(limit) is not int or not 1<=limit<=500:
|
||||
raise ApiError('INVALID_CURSOR','after>=0, limit=1..500 required')
|
||||
with self.lock:
|
||||
self._task(tid)
|
||||
return self.store.events(tid,after,limit)
|
||||
def submit(self,request):
|
||||
if not isinstance(request,dict) or set(request)-{'client_request_id','robot_id','instruction','known_info','client_created_at'}:
|
||||
raise ApiError('INVALID_REQUEST','unknown request fields')
|
||||
for k,m in [('client_request_id',120),('robot_id',80),('instruction',1000)]:text(request.get(k),k,m)
|
||||
if request['robot_id'] not in self.robots:raise ApiError('UNAUTHORIZED_ROBOT','robot is not registered',403)
|
||||
info=validate_known(request.get('known_info',{}))
|
||||
if 'client_created_at' in request and (type(request['client_created_at']) not in (int,float) or not math.isfinite(request['client_created_at']) or request['client_created_at']<0):
|
||||
raise ApiError('INVALID_REQUEST','invalid client_created_at')
|
||||
# A timestamp supplied by a retry must not change the identity of the intent.
|
||||
body={k:request[k] for k in ('client_request_id','robot_id','instruction')};body['known_info']=info
|
||||
fingerprint=hashlib.sha256(canonical(body).encode()).hexdigest()
|
||||
with self.lock,self.store.db:
|
||||
existing=self.store.db.execute('SELECT request_hash,data FROM tasks WHERE robot_id=? AND request_id=?',(body['robot_id'],body['client_request_id'])).fetchone()
|
||||
if existing:
|
||||
if existing[0]!=fingerprint:raise ApiError('IDEMPOTENCY_CONFLICT','same request key has different content',409)
|
||||
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)
|
||||
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)
|
||||
def control(self,tid,action):
|
||||
if action not in {'cancel','pause','resume'}:raise ApiError('INVALID_CONTROL','unsupported control')
|
||||
with self.lock:
|
||||
t=self._task(tid)
|
||||
if t['status'] in TERMINAL:return t
|
||||
if action=='resume':
|
||||
if t['status']!='PAUSED':raise ApiError('INVALID_STATE','task is not paused',409)
|
||||
with self.store.db:
|
||||
if t['motion_dispatched']:
|
||||
self._set(t,'INTERVENTION_REQUIRED','RESUME_REQUIRES_PHYSICAL_RECHECK')
|
||||
else:self._set(t,'QUEUED')
|
||||
return t
|
||||
if t['status']=='INTERVENTION_REQUIRED':
|
||||
# A second cancel is useful, but never removes the physical quarantine.
|
||||
if t['run_id']:
|
||||
try:self.backend.cancel(tid,t['run_id'])
|
||||
except Exception:pass
|
||||
return t
|
||||
if not t['motion_dispatched']:
|
||||
with self.store.db:
|
||||
t['planning_generation']+=1
|
||||
self._set(t,'CANCELED' if action=='cancel' else 'PAUSED')
|
||||
try:self.backend.cancel(tid,t['run_id'])
|
||||
except Exception:pass
|
||||
return t
|
||||
desired='CANCELING' if action=='cancel' else 'PAUSING'
|
||||
if t['status']==desired:return t
|
||||
with self.store.db:self._set(t,desired)
|
||||
try:self.backend.cancel(tid,t['run_id'])
|
||||
except Exception:
|
||||
with self.store.db:self._set(t,'INTERVENTION_REQUIRED','CANCEL_TRANSPORT_UNKNOWN')
|
||||
return t
|
||||
def clarify(self,tid,answer):
|
||||
if not isinstance(answer,dict) or set(answer)!={'question_id','task_revision','known_info'}:raise ApiError('INVALID_ANSWER','invalid clarification fields')
|
||||
if type(answer['task_revision']) is not int or not isinstance(answer['question_id'],str):raise ApiError('INVALID_ANSWER','invalid question/revision types')
|
||||
with self.lock,self.store.db:
|
||||
t=self._task(tid)
|
||||
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 '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
|
||||
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')
|
||||
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']:
|
||||
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']:
|
||||
raise ApiError('RECONCILIATION_REJECTED','matching stop and empty-hand evidence required',409)
|
||||
with self.store.db:
|
||||
t['stop_confirmed']=True;self._set(t,'CANCELED','RECONCILED_BY_OPERATOR')
|
||||
return t
|
||||
def tick(self):
|
||||
with self.lock:
|
||||
for e in self.backend.poll():
|
||||
if not isinstance(e,dict) or not isinstance(e.get('task_id'),str):continue
|
||||
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():
|
||||
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()
|
||||
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
|
||||
for t in tasks:
|
||||
if t['status']!='QUEUED':continue
|
||||
if self.clock()-t['accepted_at']>self.queue_timeout:
|
||||
with self.store.db:self._set(t,'EXPIRED','QUEUE_EXPIRED')
|
||||
continue
|
||||
with self.store.db:
|
||||
t['planning_generation']+=1;t['planning_attempts']+=1
|
||||
self._set(t,'PLANNING')
|
||||
with self.store.db:self.store.event(t,'planning_request',{'request':t['request'],'task_revision':t['task_revision'],'planning_generation':t['planning_generation']},self.clock())
|
||||
self._planning_started[t['task_id']]=self.steady()
|
||||
try:self.backend.start_planning(t)
|
||||
except Exception:
|
||||
with self.store.db:self._planning_failure(t,'PLANNER_TRANSPORT_ERROR')
|
||||
break
|
||||
def _planning_failure(self,t,error):
|
||||
self._set(t,'QUEUED' if t['planning_attempts']<2 else 'FAILED',error)
|
||||
def _event(self,t,e):
|
||||
typ=e.get('type')
|
||||
if typ=='plan':
|
||||
self.store.event(t,'planning_result',{'payload':e},self.clock())
|
||||
if t['status']!='PLANNING' or e.get('task_revision')!=t['task_revision'] or e.get('planning_generation')!=t['planning_generation']:return
|
||||
if self.steady()-self._planning_started.get(t['task_id'],self.steady())>=self.planning_timeout:
|
||||
self._planning_failure(t,'PLANNING_TIMEOUT');return
|
||||
if e.get('status')=='NEEDS_CLARIFICATION':
|
||||
questions=e.get('questions',[])
|
||||
if not isinstance(questions,list) or not 1<=len(questions)<=8 or any(not isinstance(x,str) or not x.strip() or len(x)>500 for x in questions):
|
||||
self._planning_failure(t,'INVALID_CLARIFICATION');return
|
||||
t['question']={'question_id':str(uuid.uuid4()),'task_revision':t['task_revision'],'questions':questions}
|
||||
self._set(t,'NEEDS_CLARIFICATION');return
|
||||
if e.get('status')!='PLAN_READY':self._planning_failure(t,e.get('error_code','PLANNING_FAILED'));return
|
||||
try:
|
||||
p=validate_plan(e.get('plan'))
|
||||
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 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:
|
||||
if len(slots['items'])!=1:raise ApiError('INTENT_MISMATCH','expected single known item')
|
||||
compare=dict(slots['items'][0],destination=slots['destination'])
|
||||
if any(compare.get(k)!=v for k,v in known.items()):raise ApiError('INTENT_MISMATCH','planner changed supplied slots')
|
||||
if p['schema_version']==2 and p['route']!=self.site.get('execution_route'):raise ApiError('ROUTE_MISMATCH','route must match deployment')
|
||||
for item in instances(p):
|
||||
if item['source_location'] not in self.site['sources'] or item['destination'] not in self.site['destinations']:raise ApiError('UNREGISTERED_STATION','source/destination not registered')
|
||||
except (ApiError,ValueError,KeyError,TypeError) as ex:
|
||||
self._planning_failure(t,getattr(ex,'code','INVALID_PLAN'));return
|
||||
t['plan']=p;t['plan_version']=p['plan_version'];t['active_item_index']=0;t['requested_quantity']=len(instances(p))
|
||||
t['run_id']=str(uuid.uuid4())
|
||||
self.store.event(t,'plan_approved',{'plan':p,'task_revision':t['task_revision'],'planning_generation':t['planning_generation'],'run_id':t['run_id']},self.clock())
|
||||
self._dispatch_item(t,initial=True)
|
||||
elif typ in {'execution_result','progress','cancel_ack','advisory'}:
|
||||
if e.get('run_id')!=t['run_id'] or t['status'] in TERMINAL or not t['motion_dispatched']:return
|
||||
if typ=='cancel_ack':return
|
||||
if typ=='advisory':
|
||||
self.store.event(t,'advisory',{'payload':e},self.clock());return
|
||||
if typ=='progress':
|
||||
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]
|
||||
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)
|
||||
if type(quantity) is not int or quantity not in (0,1):
|
||||
self._set(t,'INTERVENTION_REQUIRED','INVALID_COMPLETED_QUANTITY');return
|
||||
if quantity==1:
|
||||
slots=instances(t['plan'])[t.get('active_item_index',0)];expected_dest=t['context']['destination_id'];item_index=t.get('active_item_index',0)
|
||||
if not isinstance(ev,dict) or any(ev.get(k) is not True for k in ('passed','empty_hand','in_destination','valid')) or not isinstance(ev.get('evidence_id'),str) or not ev['evidence_id'] or ev.get('target_ref')!=slots['target_name'] or ev.get('destination_ref')!=expected_dest:
|
||||
self._set(t,'INTERVENTION_REQUIRED','DELIVERY_EVIDENCE_INVALID');return
|
||||
# A verified physical delivery survives abnormal cleanup; business
|
||||
# accounting and permission to start another robot task differ.
|
||||
inserted=self.store.db.execute('INSERT OR IGNORE INTO deliveries(task_id,item_index,evidence,created_at) VALUES(?,?,?,?)',(t['task_id'],item_index,canonical(ev),self.clock())).rowcount
|
||||
t['completed_quantity']=self.store.db.execute('SELECT COUNT(*) FROM deliveries WHERE task_id=?',(t['task_id'],)).fetchone()[0]
|
||||
t['delivery_evidence']=json.loads(self.store.db.execute('SELECT evidence FROM deliveries WHERE task_id=? AND item_index=?',(t['task_id'],item_index)).fetchone()[0])
|
||||
if inserted:self.store.event(t,'delivery_committed',{'item_index':item_index,'evidence':ev},self.clock())
|
||||
if e.get('stop_confirmed') is not True:
|
||||
self._set(t,'INTERVENTION_REQUIRED','STOP_UNKNOWN');return
|
||||
t['stop_confirmed']=True
|
||||
if e.get('status')=='SUCCEEDED':
|
||||
if quantity!=1:
|
||||
self._set(t,'INTERVENTION_REQUIRED','DELIVERY_EVIDENCE_INVALID');return
|
||||
if ev.get('safe_to_release') is not True:
|
||||
self._set(t,'INTERVENTION_REQUIRED','CLEANUP_RECHECK_REQUIRED');return
|
||||
if t['status']=='CANCELING':self._set(t,'CANCELED');return
|
||||
if t['status']=='PAUSING':self._set(t,'PAUSED');return
|
||||
if t['status']=='INTERVENTION_REQUIRED':self._set(t,'INTERVENTION_REQUIRED','PHYSICAL_RECHECK_REQUIRED');return
|
||||
if t.get('active_item_index',0)+1<t.get('requested_quantity',1):
|
||||
t['active_item_index']+=1;self._dispatch_item(t);return
|
||||
self._set(t,'SUCCEEDED');return
|
||||
if t['status'] in {'PAUSING','CANCELING'} and e.get('status')=='CANCELED':
|
||||
ev=e.get('evidence',{})
|
||||
if t['status']=='PAUSING':self._set(t,'PAUSED');return
|
||||
if isinstance(ev,dict) and ev.get('safe_to_release') is True and ev.get('empty_hand') is True and ev.get('valid') is True:
|
||||
self._set(t,'CANCELED')
|
||||
else:self._set(t,'INTERVENTION_REQUIRED','CANCELED_HOLDING_UNKNOWN')
|
||||
return
|
||||
if e.get('status')=='INTERVENTION_REQUIRED':self._set(t,'INTERVENTION_REQUIRED',e.get('error_code','PHYSICAL_RECHECK_REQUIRED'));return
|
||||
if e.get('status') in {'FAILED','CANCELED'}:
|
||||
# An execution failure can have left an item in the gripper. Never admit another task without evidence.
|
||||
ev=e.get('evidence',{})
|
||||
if isinstance(ev,dict) and ev.get('safe_to_release') is True and ev.get('empty_hand') is True and ev.get('valid') is True:
|
||||
self._set(t,e['status'],e.get('error_code','EXECUTION_FAILED'))
|
||||
else:self._set(t,'INTERVENTION_REQUIRED',e.get('error_code','PHYSICAL_RECHECK_REQUIRED'))
|
||||
else:self._set(t,'INTERVENTION_REQUIRED','RESULT_PROTOCOL_ERROR')
|
||||
|
||||
def _dispatch_item(self,t,initial=False):
|
||||
index=t.get('active_item_index',0);slots=instances(t['plan'])[index]
|
||||
source=self.site['sources'][slots['source_location']];dest=self.site['destinations'][slots['destination']]
|
||||
if not initial:t['run_id']=str(uuid.uuid4())
|
||||
t['last_sequence']=0;t['execution_plan']=item_plan(t['plan'],index)
|
||||
t['context']={**self.site,'robot_id':t['robot_id'],'target_id':slots['target_name'],'source_shelf':source['shelf_id'],'destination_id':dest['region_ref'],'observe_location':source['observe_location'],'destination_location':dest['location'],'task_revision':t['task_revision']}
|
||||
if t['plan']['schema_version']==2:t['context'].update(item_index=index,route=t['plan']['route'])
|
||||
self._set(t,'READY');t['motion_dispatched']=True;t['stop_confirmed']=False;self._set(t,'EXECUTING')
|
||||
self.store.event(t,'execution_dispatched',{'run_id':t['run_id'],'item_index':index,'context':t['context'],'versions':{'coordinator':'1.2.0','schema':t['plan']['schema_version'],'tree':'1.2.0'}},self.clock())
|
||||
self.store.db.commit()
|
||||
try:self.backend.start_execution(t)
|
||||
except Exception:self._set(t,'INTERVENTION_REQUIRED','DISPATCH_ACCEPTANCE_UNKNOWN')
|
||||
@@ -0,0 +1,41 @@
|
||||
"""Single-process serialized SQLite repository. Durable task and delivery identity."""
|
||||
import json
|
||||
import fcntl
|
||||
from pathlib import Path
|
||||
import sqlite3
|
||||
from .plan import canonical
|
||||
|
||||
class Store:
|
||||
def __init__(self,path):
|
||||
self._lockfile=None
|
||||
if path!=':memory:':
|
||||
self._lockfile=open(str(Path(path).resolve())+'.lock','a+')
|
||||
try:fcntl.flock(self._lockfile.fileno(),fcntl.LOCK_EX|fcntl.LOCK_NB)
|
||||
except BlockingIOError:
|
||||
self._lockfile.close();raise RuntimeError('another coordinator owns this database')
|
||||
self.db=sqlite3.connect(path,check_same_thread=False)
|
||||
self.db.row_factory=sqlite3.Row
|
||||
self.db.execute('PRAGMA journal_mode=WAL')
|
||||
self.db.execute('PRAGMA synchronous=FULL')
|
||||
self.db.execute('PRAGMA foreign_keys=ON')
|
||||
self.db.executescript('''
|
||||
CREATE TABLE IF NOT EXISTS 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));
|
||||
CREATE TABLE IF NOT EXISTS events(event_id INTEGER PRIMARY KEY AUTOINCREMENT,task_id TEXT NOT NULL REFERENCES tasks(task_id),kind TEXT NOT NULL,created_at REAL NOT NULL,data TEXT NOT NULL);
|
||||
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));
|
||||
''')
|
||||
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 put(self,t):
|
||||
self.db.execute('UPDATE tasks SET data=? WHERE task_id=?',(canonical(t),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):
|
||||
return [dict(event_id=r['event_id'],kind=r['kind'],created_at=r['created_at'],**json.loads(r['data'])) for r in self.db.execute('SELECT * FROM events WHERE task_id=? AND event_id>? ORDER BY event_id LIMIT ?',(tid,after,limit))]
|
||||
def close(self):
|
||||
self.db.close()
|
||||
if self._lockfile is not None:
|
||||
fcntl.flock(self._lockfile.fileno(),fcntl.LOCK_UN);self._lockfile.close();self._lockfile=None
|
||||
Reference in New Issue
Block a user