fix: harden task recovery and DR contract handling
This commit is contained in:
@@ -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