fix: harden task recovery and DR contract handling
This commit is contained in:
@@ -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()
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user