30 lines
2.2 KiB
Python
30 lines
2.2 KiB
Python
|
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)
|