66 lines
4.6 KiB
Python
66 lines
4.6 KiB
Python
|
import sys, threading, tempfile, unittest
|
||
|
|
from pathlib import Path
|
||
|
|
from types import SimpleNamespace as NS
|
||
|
|
from collections import deque
|
||
|
|
ROOT=Path(__file__).resolve().parents[1]
|
||
|
|
sys.path.insert(0,str(ROOT/'coordinator'))
|
||
|
|
from robot_bt_coordinator.ros_backend import RosBackend
|
||
|
|
from robot_bt_coordinator.service import Coordinator
|
||
|
|
from robot_bt_coordinator.backends import ManualBackend
|
||
|
|
from robot_bt_coordinator.replay import replay_events
|
||
|
|
|
||
|
|
class PlannerFeedbackTests(unittest.TestCase):
|
||
|
|
def backend(self):
|
||
|
|
b=RosBackend.__new__(RosBackend);b._lock=threading.RLock();b._events=deque();b._planning={};b._closed=False;b.config={}
|
||
|
|
class Planner:
|
||
|
|
def server_is_ready(self):return True
|
||
|
|
def send_goal_async(self,goal,**kwargs):
|
||
|
|
self.callback=kwargs.get('feedback_callback');return NS(add_done_callback=lambda cb:None)
|
||
|
|
b._planner=Planner();b._PlanTask=NS(Goal=lambda:NS(timeout=NS()))
|
||
|
|
b.start_planning(dict(task_id='t',task_revision=1,planning_generation=2,robot_id='r',request={'instruction':'move item'}))
|
||
|
|
return b
|
||
|
|
def feedback(self,sequence=1,phase=7,message='actual phase text'):
|
||
|
|
return NS(feedback=NS(stamp=NS(sec=12,nanosec=345),sequence=sequence,phase=phase,message=message))
|
||
|
|
def test_real_fields_are_copied_without_phase_invention(self):
|
||
|
|
b=self.backend();self.assertTrue(callable(b._planner.callback))
|
||
|
|
b._planner.callback(self.feedback());e=b._events.popleft()
|
||
|
|
self.assertEqual(e,dict(type='planning_progress',task_id='t',task_revision=1,planning_generation=2,stamp={'sec':12,'nanosec':345},sequence=1,phase=7,message='actual phase text'))
|
||
|
|
def test_duplicate_reordered_and_terminal_feedback_ignored(self):
|
||
|
|
b=self.backend();self.assertTrue(callable(b._planner.callback))
|
||
|
|
for seq in (2,2,1):b._planner.callback(self.feedback(seq))
|
||
|
|
b._planning[('t',1,2)]['done']=True;b._planner.callback(self.feedback(3))
|
||
|
|
self.assertEqual(len(b._events),1)
|
||
|
|
def test_invalid_fields_do_not_kill_callback(self):
|
||
|
|
b=self.backend();self.assertTrue(callable(b._planner.callback))
|
||
|
|
for field,value in [('sequence',True),('sequence',-1),('sequence',2**32),('phase',256),('message',object())]:
|
||
|
|
f=self.feedback();setattr(f.feedback,field,value);b._planner.callback(f)
|
||
|
|
f=self.feedback();f.feedback.stamp.nanosec=1000000000;b._planner.callback(f)
|
||
|
|
self.assertFalse(b._events)
|
||
|
|
def test_feedback_is_persisted_and_stale_generation_rejected(self):
|
||
|
|
with tempfile.TemporaryDirectory() as tmp:
|
||
|
|
path=str(Path(tmp)/'tasks.db');backend=ManualBackend();c=Coordinator(path,backend,{'r'})
|
||
|
|
try:
|
||
|
|
tid=c.submit(dict(client_request_id='req',robot_id='r',instruction='move item',known_info={}))['task_id'];c.tick();t=c.get(tid)
|
||
|
|
event=dict(type='planning_progress',task_id=tid,task_revision=t['task_revision'],planning_generation=t['planning_generation'],stamp={'sec':12,'nanosec':345},sequence=1,phase=7,message='actual phase text')
|
||
|
|
backend.emit(event);backend.emit(event);backend.emit(dict(event,planning_generation=0,sequence=2));c.tick()
|
||
|
|
rows=[e for e in c.events(tid) if e['kind']=='planning_progress'];self.assertEqual(len(rows),1);self.assertEqual(rows[0]['payload'],event)
|
||
|
|
self.assertEqual(c.get(tid)['status'],'PLANNING')
|
||
|
|
# A new generation starts its own sequence; old callbacks cannot
|
||
|
|
# contaminate it, and feedback after leaving planning is ignored.
|
||
|
|
current=c.store.get(tid);current['planning_generation']+=1
|
||
|
|
with c.store.db:c.store.put(current)
|
||
|
|
newer=dict(event,planning_generation=current['planning_generation'],message='next actual phase')
|
||
|
|
backend.emit(dict(event,sequence=99));backend.emit(newer);c.tick()
|
||
|
|
rows=[e['payload'] for e in c.events(tid) if e['kind']=='planning_progress']
|
||
|
|
self.assertEqual(rows,[event,newer])
|
||
|
|
c.control(tid,'cancel');backend.emit(dict(newer,sequence=2));c.tick()
|
||
|
|
self.assertEqual(c.get(tid)['status'],'CANCELED')
|
||
|
|
replay=replay_events(c.events(tid));self.assertEqual(replay['planning_progress'],[event,newer]);self.assertFalse(replay['connects_to_robot'])
|
||
|
|
self.assertEqual([e['payload'] for e in c.events(tid) if e['kind']=='planning_progress'],[event,newer])
|
||
|
|
finally:c.close()
|
||
|
|
c=Coordinator(path,ManualBackend(),{'r'})
|
||
|
|
try:self.assertEqual([e['payload'] for e in c.events(tid) if e['kind']=='planning_progress'],[event,newer])
|
||
|
|
finally:c.close()
|
||
|
|
|
||
|
|
if __name__=='__main__':unittest.main()
|