fix: align navigation contract and readiness handling
This commit is contained in:
@@ -489,4 +489,158 @@ class ProxyTests(unittest.TestCase):
|
||||
self.assertTrue(any(e.error for e in events))
|
||||
|
||||
|
||||
|
||||
|
||||
class NavigationWireTests(unittest.TestCase):
|
||||
def test_blocked_unknown_is_not_reported_as_clear(self):
|
||||
s = module.GatewaySnapshot.parse(snapshot(), ID)
|
||||
self.assertIsNone(getattr(s, 'blocked', 'missing'))
|
||||
with self.assertRaises(module.GatewayError):
|
||||
module.assign_navigation_feedback(object(), s)
|
||||
|
||||
def test_explicit_blocked_and_error_code_survive_parse(self):
|
||||
s = module.GatewaySnapshot.parse(snapshot(blocked=True, error_code='INPUTS_UNHEALTHY'), ID)
|
||||
self.assertEqual((getattr(s, 'blocked', None), getattr(s, 'error_code', None)), (True, 'INPUTS_UNHEALTHY'))
|
||||
for invalid in (0, 'false'):
|
||||
with self.assertRaises(module.GatewayError):
|
||||
module.GatewaySnapshot.parse(snapshot(blocked=invalid), ID)
|
||||
|
||||
def test_new_feedback_maps_yaw_and_blocked_phase(self):
|
||||
from types import SimpleNamespace as NS
|
||||
f = NS(STOPPING=4, CHECKING=1, NAVIGATING=2, BLOCKED=3, elapsed_time=NS(sec=0, nanosec=0))
|
||||
s = module.GatewaySnapshot.parse(snapshot(blocked=True, position_error=.5, yaw_error=-.2), ID)
|
||||
self.assertTrue(hasattr(module, 'assign_navigation_feedback'))
|
||||
module.assign_navigation_feedback(f, s)
|
||||
self.assertEqual((f.phase, f.blocked, f.error_valid, f.yaw_error), (3, True, True, -.2))
|
||||
self.assertFalse(f.current_pose_valid)
|
||||
|
||||
def test_goal_uses_yaw_tolerance(self):
|
||||
from types import SimpleNamespace as NS
|
||||
goal = NS(trace=NS(**{k: 't' if k in ('task_id', 'subtask_id', 'run_id') else 1 for k in module._TRACE_FIELDS}), timeout=NS(sec=2, nanosec=0), position_tolerance=.1, yaw_tolerance=.23,
|
||||
target_pose=NS(header=NS(frame_id='map'), pose=NS(position=NS(x=1., y=2., z=0.), orientation=NS(x=0., y=0., z=0., w=1.))))
|
||||
self.assertEqual(module.build_goal_body(goal, ID, 'map-a')['yaw_tolerance'], .23)
|
||||
|
||||
class GatewayTelemetryTests(unittest.TestCase):
|
||||
setUp = GatewayTests.setUp
|
||||
def test_only_fresh_explicit_blocked_sample_is_published(self):
|
||||
self.gateway.submit(request())
|
||||
self.backend.set_snapshot(request()['goal_id'], 'ACTIVE', pose=request()['target_pose'])
|
||||
sample = self.backend.samples[request()['goal_id']]
|
||||
sample['blocked'] = dict(self.backend.health_value, value=True)
|
||||
self.assertIs(self.gateway.poll(request()['goal_id']).get('blocked'), True)
|
||||
self.clock.advance(1)
|
||||
self.assertIsNone(self.gateway.poll(request()['goal_id']).get('blocked'))
|
||||
|
||||
def test_readiness_code_requires_fresh_explicit_backend_diagnostic(self):
|
||||
self.gateway.submit(request())
|
||||
self.backend.health_sample(False)
|
||||
self.backend.health_value['error_code'] = 'INPUTS_UNHEALTHY'
|
||||
out = self.gateway.poll(request()['goal_id'])
|
||||
self.assertEqual(out.get('error_code'), 'INPUTS_UNHEALTHY')
|
||||
self.assertEqual(out['stop_state'], 'UNKNOWN')
|
||||
|
||||
class NavigationOutcomeTests(unittest.TestCase):
|
||||
def test_terminal_status_mapping_distinguishes_timeout_blocked_and_not_ready(self):
|
||||
from types import SimpleNamespace as NS
|
||||
enum = NS(SUCCEEDED=0, CANCELED=1, TIMEOUT=2, BLOCKED=3, NOT_READY=4, FAILED=5)
|
||||
for outcome, expected in [('COMPLETED',0),('CANCELED',1),('TIMED_OUT',2),('BLOCKED',3),('NOT_READY',4),('REJECTED',5),('FAILED',5)]:
|
||||
value = module.GatewaySnapshot.parse(snapshot(outcome=outcome), ID)
|
||||
self.assertEqual(module.navigation_status(value, enum), expected)
|
||||
value = module.GatewaySnapshot.parse(snapshot(outcome='FAILED', error_code='ROBOT_STATE_UNAVAILABLE'), ID)
|
||||
self.assertEqual(module.navigation_status(value, enum), 4)
|
||||
|
||||
class GoalValidationTests(unittest.TestCase):
|
||||
def test_trace_ranges_quaternion_and_yaw_are_checked_before_network(self):
|
||||
from types import SimpleNamespace as NS
|
||||
goal = NS(trace=NS(task_id='t', subtask_id='s', run_id='r', attempt=1, task_revision=1, plan_version=1, execution_generation=1), timeout=NS(sec=2,nanosec=0), position_tolerance=.1, yaw_tolerance=.2, target_pose=NS(header=NS(frame_id='map'), pose=NS(position=NS(x=0.,y=0.,z=0.),orientation=NS(x=0.,y=0.,z=0.,w=1.))))
|
||||
for field,value in [('run_id',''),('attempt',True),('attempt',2**32),('task_revision',0),('plan_version',True),('execution_generation',2**64)]:
|
||||
bad=copy.deepcopy(goal);setattr(bad.trace,field,value)
|
||||
with self.subTest(field=field), self.assertRaises(ValueError):module.build_goal_body(bad,ID,'map-a')
|
||||
goal.yaw_tolerance=4.
|
||||
goal.target_pose.pose.orientation.w=2.
|
||||
body=module.build_goal_body(goal,ID,'map-a')
|
||||
self.assertEqual(body['yaw_tolerance'],4.)
|
||||
self.assertEqual(body['target_pose']['orientation']['w'],1.)
|
||||
self.assertEqual(validate_goal(body)['yaw_tolerance'],4.)
|
||||
goal.target_pose.pose.orientation.w=0.
|
||||
with self.assertRaises(ValueError):module.build_goal_body(goal,ID,'map-a')
|
||||
|
||||
class BackendDiagnosticTests(unittest.TestCase):
|
||||
def test_health_callback_keeps_explicit_machine_readable_cause(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from navigation_gateway.backends import Ros1MoveBaseBackend
|
||||
backend = Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend)
|
||||
backend.map_id='map-a'; backend.lock=threading.RLock()
|
||||
backend._source_metadata=lambda stamp: {'source_fresh':True,'source_stamp':stamp}
|
||||
backend._health(NS(data=json.dumps({'ready':False,'map_id':'map-a','stamp':12.,'error_code':'INPUTS_UNHEALTHY'})))
|
||||
self.assertEqual(backend.health_value.get('error_code'), 'INPUTS_UNHEALTHY')
|
||||
backend._health(NS(data=json.dumps({'ready':False,'map_id':'map-a','stamp':12.,'reason':'some prose'})))
|
||||
self.assertEqual(backend.health_value.get('error_code', ''), '')
|
||||
|
||||
class LocalNotReadyTests(unittest.IsolatedAsyncioTestCase):
|
||||
async def test_valid_unready_goal_returns_not_ready_without_http_submit(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from unittest.mock import patch, Mock
|
||||
class Node:
|
||||
def __init__(self,*args):pass
|
||||
def create_timer(self,*args):pass
|
||||
nav=NS(SUCCEEDED=0,CANCELED=1,TIMEOUT=2,BLOCKED=3,NOT_READY=4,FAILED=5,UNKNOWN=0,CONFIRMED=1)
|
||||
imports={'rclpy':NS(), 'rclpy.action':NS(ActionServer=lambda *a,**k:NS(),CancelResponse=NS(ACCEPT=1),GoalResponse=NS(ACCEPT=1,REJECT=2)),
|
||||
'rclpy.node':NS(Node=Node),'rclpy.task':NS(Future=lambda:object()),'rclpy.callback_groups':NS(ReentrantCallbackGroup=lambda:object()),
|
||||
'bt_skill_interfaces':NS(), 'bt_skill_interfaces.action':NS(Navigate=NS(Result=lambda:NS(result=NS()))),'bt_skill_interfaces.msg':NS(NavigationResult=nav)}
|
||||
goal=NS(trace=NS(task_id='t',subtask_id='s',run_id='r',attempt=1,task_revision=1,plan_version=1,execution_generation=1),timeout=NS(sec=2,nanosec=0),position_tolerance=.1,yaw_tolerance=.2,target_pose=NS(header=NS(frame_id='map'),pose=NS(position=NS(x=0.,y=0.,z=0.),orientation=NS(x=0.,y=0.,z=0.,w=1.))))
|
||||
config=module.ProxyConfig(token='a-long-test-token',connect_timeout_sec=.1,read_timeout_sec=.1,request_timeout_sec=.2,poll_interval_sec=.01,readiness_max_age_sec=.5,feedback_silence_timeout_sec=.5)
|
||||
client=Mock()
|
||||
with patch.dict(sys.modules,imports), patch.object(module,'GatewayClient',return_value=client), patch.object(module.threading,'Thread'):
|
||||
node=module.create_ros_node(config,map_id='map-a',action_name='skills/navigate')
|
||||
node._health_at=time.monotonic();node._health_error_code='INPUTS_UNHEALTHY'
|
||||
self.assertEqual(node._accept(goal),1)
|
||||
handle=NS(request=goal,goal_id=NS(uuid=list(__import__('uuid').UUID(ID).bytes)),abort=Mock())
|
||||
result=await node._execute(handle)
|
||||
self.assertEqual((result.result.status,result.result.error_code,result.result.stop_state),(4,'INPUTS_UNHEALTHY',0))
|
||||
self.assertFalse(node._reserved)
|
||||
client.submit.assert_not_called()
|
||||
node._health_at=0
|
||||
self.assertEqual(node._accept(goal),1)
|
||||
result=await node._execute(handle)
|
||||
self.assertEqual(result.result.error_code,'NAV_NOT_READY')
|
||||
client.submit.assert_not_called()
|
||||
|
||||
class BlockedAdmissionTests(unittest.TestCase):
|
||||
setUp = GatewayTests.setUp
|
||||
def test_missing_or_stale_blocked_rejects_before_send(self):
|
||||
for blocked in (None, {'value':False,'received_at':0.,'source_fresh':False}):
|
||||
self.backend.health_sample(True)
|
||||
self.backend.health_value['blocked']=blocked
|
||||
self.assertEqual(self.gateway.health()['error_code'],'INPUTS_UNHEALTHY')
|
||||
with self.assertRaises(GatewayError):self.gateway.submit(request())
|
||||
self.assertEqual(self.backend.send_count,0)
|
||||
|
||||
def test_explicit_simulation_blocked_can_progress_then_loss_cancels(self):
|
||||
self.assertTrue(self.gateway.health()['ready'])
|
||||
self.gateway.submit(request())
|
||||
for blocked in (False,True):
|
||||
self.backend.set_snapshot(request()['goal_id'],'ACTIVE',pose=request()['target_pose'],blocked=blocked)
|
||||
out=self.gateway.poll(request()['goal_id'])
|
||||
self.assertIs(out['blocked'],blocked)
|
||||
self.assertEqual(self.backend.cancel_count,0)
|
||||
self.backend.set_snapshot(request()['goal_id'],'ACTIVE',pose=request()['target_pose'],blocked=None)
|
||||
out=self.gateway.poll(request()['goal_id'])
|
||||
self.assertEqual((self.backend.cancel_count,out['error_code'],out['stop_state']),(1,'INPUTS_UNHEALTHY','UNKNOWN'))
|
||||
|
||||
def test_ros_health_carries_only_explicit_blocked_with_original_source_time(self):
|
||||
from types import SimpleNamespace as NS
|
||||
from navigation_gateway.backends import Ros1MoveBaseBackend
|
||||
backend=Ros1MoveBaseBackend.__new__(Ros1MoveBaseBackend)
|
||||
backend.map_id='sim-map';backend.lock=threading.RLock();backend.connected=True
|
||||
backend._source_metadata=lambda stamp:{'source_stamp':stamp,'source_fresh':True}
|
||||
backend.source_is_fresh=lambda sample:sample.get('source_fresh') is True
|
||||
for blocked in (False,True,None,0):
|
||||
payload={'ready':True,'map_id':'sim-map','stamp':100.,'blocked':blocked}
|
||||
backend._health(NS(data=json.dumps(payload)))
|
||||
sample=backend.health().get('blocked')
|
||||
if type(blocked) is bool:
|
||||
self.assertEqual((sample['value'],sample['source_stamp']),(blocked,100.))
|
||||
else:self.assertIsNone(sample)
|
||||
|
||||
if __name__ == "__main__": unittest.main()
|
||||
|
||||
Reference in New Issue
Block a user