实现行为树执行器、任务协调和技能接口

2026-09-20 12:18:52 +08:00
commit 492676344a
143 changed files with 13010 additions and 0 deletions
@@ -0,0 +1,29 @@
+import sys, unittest, copy
+from pathlib import Path
+sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'coordinator'))
+from robot_bt_coordinator.plan import validate_plan, validate_known
+from robot_bt_coordinator.errors import ApiError
+
+def sample(route='OBJECT_TABLE'):
+    items=[{'target_name':'water','quantity':2,'source_location':'shelf_A'},{'target_name':'doll','quantity':1,'source_location':'shelf_A'}]
+    p=dict(schema_version=2,plan_version=1,task_type='multi_item_pick_transport_place',goal='two waters then one doll',route=route,slots={'items':items,'destination':'tote_A'},missing_information=[],subtasks=[])
+    for idx,target in enumerate(['water','water','doll']):
+        rows=[('NAVIGATE',{'target':target}),('PICK',{'target':target}),('NAVIGATE',{'destination':'tote_A'}),('PLACE',{'target':target,'destination':'tote_A'})]
+        if route=='SHELF_CELL': rows=[('NAVIGATE',{'source_location':'shelf_A','mode':'observation'}),('ROBOBRAIN_SHELF_LOCALIZE',{'target':target}),('NAVIGATE',{'source_location':'shelf_A','mode':'shelf_cell'})]+rows[1:]
+        for skill,args in rows:
+            n=len(p['subtasks']);p['subtasks'].append(dict(id=f'S{n+1}',skill=skill,arguments=dict(args,item_index=idx),depends_on=[] if not n else [f'S{n}']))
+    return p
+class PlanV2Tests(unittest.TestCase):
+    def test_accept_two_routes_and_multi_items(self):
+        for route in ['OBJECT_TABLE','SHELF_CELL']:
+            try:p=validate_plan(sample(route))
+            except ApiError as e:self.fail('new DR plan rejected: '+str(e))
+            self.assertEqual(p['slots']['items'][0]['quantity'],2)
+    def test_known_multi(self):
+        try:result=validate_known(sample()['slots'])
+        except ApiError as e:self.fail(str(e))
+        self.assertEqual(result['items'][1]['target_name'],'doll')
+    def test_reject_broken_dependency_unknown_skill_quantity_order(self):
+        for edit in [lambda p:p['subtasks'][4].update(depends_on=[]),lambda p:p['subtasks'][1].update(skill='<PICK>'),lambda p:p['slots']['items'][0].update(quantity=True),lambda p:p['slots']['items'][0].update(quantity=21),lambda p:p['subtasks'][1]['arguments'].update(target='doll')]:
+            p=sample();edit(p)
+            with self.assertRaises(ApiError):validate_plan(p)