327 lines
17 KiB
Python
327 lines
17 KiB
Python
#!/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()
|