实现行为树执行器、任务协调和技能接口
This commit is contained in:
Executable
+15
@@ -0,0 +1,15 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
if [[ ! -f /opt/ros/humble/setup.bash ]]; then
|
||||
echo 'NOT RUN: ROS2 Humble is not installed; no ROS compilation claim.' >&2
|
||||
exit 2
|
||||
fi
|
||||
source /opt/ros/humble/setup.bash
|
||||
command -v colcon >/dev/null || { echo 'colcon is required' >&2; exit 2; }
|
||||
bt_repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
|
||||
cd "$bt_repo_root"
|
||||
# Install/build upstream BehaviorTree.CPP 4.10.0 separately and source its prefix.
|
||||
# EXACT REQUIRED intentionally rejects an incompatible system BT.CPP package.
|
||||
colcon build --base-paths ros2 --packages-up-to bt_executor robobrain_services bt_mock_servers --event-handlers console_direct+
|
||||
colcon test --packages-select bt_skill_interfaces bt_executor --event-handlers console_direct+
|
||||
colcon test-result --verbose
|
||||
@@ -0,0 +1,46 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Checks fixed-tree expansion and generated IDL headers without claiming ROS build."""
|
||||
from pathlib import Path
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
REPOSITORY = PACKAGE.parents[1]
|
||||
root = ET.parse(PACKAGE / 'trees/fixed_workflow.xml').getroot()
|
||||
assert root.attrib['BTCPP_format'] == '4'
|
||||
trees = {v.attrib['ID']: v for v in root.findall('BehaviorTree')}
|
||||
assert {'NavigateSkill', 'GroundTargetSkill', 'PickSkill', 'CheckFreeSpaceSkill',
|
||||
'PlaceSkill', 'AskUserSkill'} <= set(trees)
|
||||
assert [v.tag for v in trees['TaskRoot'][0]] == ['ApprovedPlanGate', 'SubTree']
|
||||
|
||||
|
||||
def expand(node, inputs=None):
|
||||
inputs = inputs or {}
|
||||
if node.tag == 'RunStage':
|
||||
stage = node.attrib['stage']
|
||||
return [inputs[stage[1:-1]] if stage.startswith('{') else stage]
|
||||
if node.tag == 'SubTree':
|
||||
return expand(trees[node.attrib['ID']], {**inputs, **node.attrib})
|
||||
return [stage for child in node for stage in expand(child, inputs)]
|
||||
|
||||
|
||||
expected = re.findall(r'case Stage::\w+:return "(\w+)"',
|
||||
(REPOSITORY / 'core/src/workflow.cpp').read_text())
|
||||
actual = expand(trees['TaskRoot'])
|
||||
assert actual == expected, (actual, expected)
|
||||
assert not any(v.tag in {'Script', 'ScriptCondition', 'RetryUntilSuccessful'} for v in root.iter())
|
||||
assert trees['AskUserSkill'][0].tag == 'RequestClarification'
|
||||
|
||||
|
||||
def header_name(name):
|
||||
return re.sub(r'([a-z0-9])([A-Z])', r'\1_\2',
|
||||
re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', name)).lower()
|
||||
|
||||
|
||||
for source in list((PACKAGE / 'src').glob('*.cpp')) + list((PACKAGE / 'include/bt_executor').glob('*.hpp')):
|
||||
for kind, name in re.findall(r'bt_skill_interfaces/(action|msg)/(\w+)\.hpp', source.read_text()):
|
||||
candidates = list((PACKAGE.parent / 'bt_skill_interfaces' / kind).glob('*'))
|
||||
assert any(header_name(v.stem) == name for v in candidates), (source, kind, name)
|
||||
ET.parse(PACKAGE / 'package.xml')
|
||||
print(f'Static checks passed: {len(actual)} ordered core stages, six skill templates, generated IDL include names, package XML.')
|
||||
print('This does not compile C++ or validate ROS2 middleware behavior.')
|
||||
@@ -0,0 +1,117 @@
|
||||
"""Transport lifecycle tests using controlled callback objects; no robot simulation."""
|
||||
import sys
|
||||
from pathlib import Path
|
||||
import threading
|
||||
from collections import deque
|
||||
from types import SimpleNamespace as S
|
||||
import unittest
|
||||
import time
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[3] / 'coordinator'))
|
||||
from robot_bt_coordinator.ros_backend import RosBackend
|
||||
|
||||
|
||||
class Future:
|
||||
def __init__(self, value): self.value = value
|
||||
def result(self): return self.value
|
||||
def add_done_callback(self, cb): self.callback = cb
|
||||
|
||||
|
||||
class Handle:
|
||||
accepted = True
|
||||
def __init__(self): self.cancels = 0; self.future = Future(None)
|
||||
def cancel_goal_async(self): self.cancels += 1
|
||||
def get_result_async(self): return self.future
|
||||
|
||||
|
||||
class BackendLifecycle(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.backend = RosBackend.__new__(RosBackend)
|
||||
self.backend._lock = threading.RLock()
|
||||
self.backend._events = deque()
|
||||
self.backend._feedback_timeout = 5
|
||||
self.backend.node = S(get_clock=lambda: S(now=lambda: S(nanoseconds=10_000_000_000)))
|
||||
self.key = ('task', 'run')
|
||||
self.rec = dict(task_id='task', run_id='run', sent_at=time.monotonic(), last_feedback=0,
|
||||
sequence=0, handle=None, done=False, cancel_intent=False,
|
||||
cancel_at=None, unknown_emitted=False, wire_uuid='wire')
|
||||
self.backend._runs = {self.key: self.rec}
|
||||
self.backend._planning = {}
|
||||
|
||||
def result(self, native=4, business=0, stop=1, evidence='{"stop_confirmed":true}'):
|
||||
return Future(S(status=native, result=S(result=S(status=business, stop_state=stop,
|
||||
stopped_at=S(sec=10, nanosec=0), stop_evidence_ref='proof', error_code=''),
|
||||
completed_quantity=1, evidence_json=evidence)))
|
||||
|
||||
def test_cancel_before_late_accept(self):
|
||||
self.backend.cancel(*self.key)
|
||||
handle = Handle()
|
||||
self.backend._execution_accepted(self.key, Future(handle))
|
||||
self.assertEqual(handle.cancels, 1)
|
||||
self.assertFalse(self.rec['done'])
|
||||
self.assertEqual(len(self.backend._events), 0)
|
||||
|
||||
def test_planning_cancel_cancels_handle(self):
|
||||
handle = Handle()
|
||||
planning = dict(task_id='task', task_revision=1, planning_generation=1, done=False, handle=handle)
|
||||
self.backend._planning = {('task', 1, 1): planning}
|
||||
self.backend.cancel('task', '')
|
||||
self.assertTrue(planning['done'])
|
||||
self.assertEqual(handle.cancels, 1)
|
||||
self.assertFalse(self.backend._events)
|
||||
|
||||
def test_native_business_mismatch_quarantines(self):
|
||||
self.backend._execution_result(self.key, self.result(native=5, business=0))
|
||||
event = self.backend._events.pop()
|
||||
self.assertEqual(event['status'], 'INTERVENTION_REQUIRED')
|
||||
self.assertFalse(event['stop_confirmed'])
|
||||
self.assertFalse(self.rec['done']) # tracking survives unknown result
|
||||
|
||||
def test_unknown_stop_does_not_become_success(self):
|
||||
self.backend._execution_result(self.key, self.result(stop=0))
|
||||
self.assertEqual(self.backend._events.pop()['status'], 'INTERVENTION_REQUIRED')
|
||||
|
||||
def test_stale_stop_proof_does_not_release(self):
|
||||
future = self.result()
|
||||
future.value.result.result.stopped_at.sec = 1
|
||||
self.backend._execution_result(self.key, future)
|
||||
self.assertFalse(self.backend._events.pop()['stop_confirmed'])
|
||||
|
||||
def test_future_stop_proof_does_not_release(self):
|
||||
future = self.result()
|
||||
future.value.result.result.stopped_at.sec = 11
|
||||
self.backend._execution_result(self.key, future)
|
||||
self.assertFalse(self.backend._events.pop()['stop_confirmed'])
|
||||
|
||||
def test_cancel_ack_never_is_stop(self):
|
||||
self.rec['handle'] = Handle()
|
||||
self.backend.cancel(*self.key)
|
||||
self.assertEqual(len(self.backend._events), 0)
|
||||
self.assertFalse(self.rec['done'])
|
||||
|
||||
def test_stale_and_duplicate_feedback_do_not_refresh_liveness(self):
|
||||
def feedback(sec, sequence, stage='Navigate'):
|
||||
self.backend._feedback(self.key, S(feedback=S(stamp=S(sec=sec, nanosec=0),
|
||||
sequence=sequence, stage=stage, status_json='{}')))
|
||||
feedback(1, 1)
|
||||
self.assertEqual(self.rec['last_feedback'], 0)
|
||||
feedback(10, 2)
|
||||
accepted = self.rec['last_feedback']
|
||||
feedback(10, 2)
|
||||
self.assertEqual(self.rec['last_feedback'], accepted)
|
||||
self.assertEqual(len(self.backend._events), 1)
|
||||
|
||||
def test_late_terminal_after_unknown_still_emits_evidence(self):
|
||||
self.backend._unknown(self.rec, 'ACCEPTANCE_TIMEOUT')
|
||||
self.backend._execution_result(self.key, self.result(native=5, business=2))
|
||||
events = list(self.backend._events)
|
||||
self.assertEqual([e['status'] for e in events], ['INTERVENTION_REQUIRED', 'CANCELED'])
|
||||
self.assertTrue(events[-1]['stop_confirmed'])
|
||||
|
||||
def test_duplicate_json_result_fails_closed(self):
|
||||
self.backend._execution_result(self.key, self.result(evidence='{"stop_confirmed":false,"stop_confirmed":true}'))
|
||||
self.assertEqual(self.backend._events.pop()['status'], 'INTERVENTION_REQUIRED')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user