520 lines
27 KiB
Python
520 lines
27 KiB
Python
"""Bounded functional fixtures; no device, socket, capture or load generation."""
|
|
import json
|
|
import threading
|
|
import time
|
|
from queue import Queue, Empty
|
|
from types import SimpleNamespace
|
|
import numpy as np
|
|
import pytest
|
|
from k1link.sessions.live_planning import PlanningLiveEvent
|
|
from k1link.missions.live_buffer import LiveCloudBuffer
|
|
from k1link.missions.live_tests import PlanningLiveTests
|
|
from k1link.missions.registration_colors import query_colors
|
|
from k1link.missions.live_scene import scene_bytes
|
|
from k1link.compute.live_perception import LivePerceptionIngress
|
|
from k1link.device_plugins.xgrids_k1.planning_live import K1PlanningLiveSource
|
|
from test_stream_summary import _pose_payload, _pcl_payload
|
|
|
|
|
|
def event(kind, *, t=1, p=(0,0,0), points=None, sequence=1):
|
|
return PlanningLiveEvent('B',2,sequence,int(t*1e9),int(t*1e9),kind,points,p)
|
|
|
|
|
|
def test_causal_pose_and_bounded_point_window():
|
|
b=LiveCloudBuffer([[10,10,0],[14,10,0]])
|
|
cloud=np.array([[1,1,1],[1.01,1,1],[99,1,1],[1,1,20]])
|
|
b.ingest(event('points',points=cloud));assert len(b.snapshot()['points'])==0
|
|
b.ingest(event('pose',t=2))
|
|
b.ingest(event('points',t=1.9,points=cloud));assert len(b.snapshot()['points'])==0
|
|
b.ingest(event('points',t=2.6,points=cloud));assert len(b.snapshot()['points'])==0
|
|
b.ingest(event('points',t=2.1,points=cloud));assert len(b.snapshot()['points'])==1
|
|
# No double pose transform: query remains in its native local K1 frame.
|
|
assert np.allclose(b.snapshot()['points'][0],[1,1,1])
|
|
for i in range(1,80):
|
|
b.ingest(event('pose',t=3+i,p=(i*.05,0,0)))
|
|
b.ingest(event('points',t=3.1+i,points=cloud,sequence=i))
|
|
assert len(b.chunks)==40 and len(b.events)==40
|
|
assert len(b.snapshot()['points'])<=40000
|
|
with pytest.raises(ValueError,match='Разрыв координат'):b.ingest(event('pose',t=200,p=(100,0,0)))
|
|
|
|
|
|
def test_route_initialization_can_request_an_explicit_wider_k1_scene():
|
|
b=LiveCloudBuffer([[0,0,0],[10,0,0]],point_radius_m=80)
|
|
b.ingest(event('pose',t=1,p=(0,0,0)))
|
|
b.ingest(event('points',t=1.1,points=np.array([[79.9,0,1],[80.1,0,1]])))
|
|
sample=b.snapshot()
|
|
assert sample['point_radius_m']==80
|
|
assert len(sample['points'])==1
|
|
|
|
|
|
def test_only_accepted_correspondences_are_green():
|
|
p=np.array([[0,0,0],[0,0,1],[0,0,2]])
|
|
green=np.array([154,235,75])
|
|
assert not (query_colors(p)==green).all(axis=1).any()
|
|
assert not (query_colors(p,{'status':'rejected','matched_query_indices':[0,1,2]})==green).all(axis=1).any()
|
|
c=query_colors(p,{'status':'candidate','matched_query_indices':[1]})
|
|
assert (c==green).all(axis=1).tolist()==[False,True,False]
|
|
|
|
|
|
def test_plugin_adapter_reads_existing_committed_ingress_only():
|
|
ingress=LivePerceptionIngress();adapter=K1PlanningLiveSource(ingress)
|
|
adapter.open('test');ingress.begin_session('B')
|
|
assert adapter.take('test').kind=='session-start'
|
|
ingress.publish(modality='pose',source_id='x/lio_pose',source_sequence=3,
|
|
captured_at_epoch_ns=4,received_monotonic_ns=5,payload=_pose_payload((5,0,0)))
|
|
p=adapter.take('test');assert p.position==(5.,0.,0.) and p.generation==1
|
|
ingress.publish(modality='lidar',source_id='x/lio_pcl',source_sequence=4,
|
|
captured_at_epoch_ns=5,received_monotonic_ns=6,payload=_pcl_payload(scaler=1000,point_count=4))
|
|
frame=adapter.take('test');assert np.allclose(frame.points[0],[1,-2,.5])
|
|
with pytest.raises(RuntimeError):adapter.open('other-profile')
|
|
adapter.close('test');assert not ingress.snapshot()['consumer_connected']
|
|
ingress.close()
|
|
|
|
|
|
class Source:
|
|
def __init__(self):self.state=dict(active=False,session_generation=1,session_id='old');self.queue=Queue();self.owner=None
|
|
def snapshot(self):return dict(self.state)
|
|
def open(self,id):
|
|
if self.owner:raise RuntimeError('busy')
|
|
self.owner=id
|
|
def close(self,id):assert self.owner==id;self.owner=None
|
|
def take(self,id):
|
|
try:return self.queue.get(timeout=.02)
|
|
except Empty:return None
|
|
|
|
|
|
def fixture_service(tmp_path,monkeypatch):
|
|
import k1link.missions.live_tests as module
|
|
draft=dict(id='draft',name='Test route',revision=1,zone=dict(session_id='A',generation='a'),
|
|
route=dict(length_m=20,start_index=0,end_index=20,points=[dict(position=[i,0,0]) for i in range(21)]))
|
|
r=np.random.default_rng(19);points=r.normal(size=(1000,3))
|
|
sources=SimpleNamespace(store=SimpleNamespace(get_session=lambda _:SimpleNamespace(plugin_id='test-plugin')),
|
|
reference_map=lambda *args,**kwargs:(points,dict(session_id='A',source_digests={'raw':'a'*64})))
|
|
drafts=SimpleNamespace(database=tmp_path/'db',sources=sources,get=lambda _:json.loads(json.dumps(draft)))
|
|
source=Source();lock=threading.Lock();service=PlanningLiveTests(drafts,{'test-plugin':source},lock)
|
|
def calculate(directory,ref,query,hint):
|
|
# Real numeric regression is tested separately; this fixture verifies orchestration.
|
|
return dict(status='candidate',T_reference_query=hint.tolist(),matched_query_indices=[0],
|
|
overlap=.9,inlier_rmse_m=.1,reasons=[])
|
|
monkeypatch.setattr(module,'run_registration',calculate)
|
|
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
|
monkeypatch.setattr(module,'run_route_relocalization',
|
|
lambda directory,ref,path,query,anchor:dict(
|
|
status='candidate',T_reference_query=np.eye(4).tolist(),
|
|
matched_query_indices=[0],overlap=.9,inlier_rmse_m=.1,reasons=[],
|
|
initialization=dict(complete=True,scope='selected-route',
|
|
policy=ROUTE_RELOCALIZATION_POLICY,expected_attempts=1,attempts=[{}])))
|
|
return service,source,lock,draft
|
|
|
|
|
|
def until(fn):
|
|
deadline=time.monotonic()+5
|
|
while time.monotonic()<deadline:
|
|
if fn():return
|
|
time.sleep(.01)
|
|
raise AssertionError('condition did not become true')
|
|
|
|
|
|
def test_profile_requires_new_session_free_lease_and_frozen_revision(tmp_path,monkeypatch):
|
|
service,source,lock,draft=fixture_service(tmp_path,monkeypatch)
|
|
with pytest.raises(ValueError,match='изменён'):service.start('draft',2)
|
|
source.state['active']=True
|
|
with pytest.raises(ValueError,match='завершите'):service.start('draft',1)
|
|
source.state['active']=False;source.owner='AI'
|
|
with pytest.raises(ValueError,match='другим'):service.start('draft',1)
|
|
assert not lock.locked()
|
|
source.owner=None
|
|
run=service.start('draft',1)
|
|
try:
|
|
until(lambda:service.get()['state']=='waiting')
|
|
assert run['profile']=='planning' and lock.locked()
|
|
draft['revision']=2
|
|
assert service.get()['draft']['revision']==1
|
|
with pytest.raises(ValueError,match='ещё выполняется'):service.start('draft',1)
|
|
# Old queued acquisition cannot be rebound to reference A or mistaken for B.
|
|
source.queue.put(PlanningLiveEvent('old',1,1,time.monotonic_ns(),time.time_ns(),'pose',position=[0,0,0]))
|
|
time.sleep(.05);assert service.get()['query_session_id'] is None
|
|
source.state.update(active=True,session_id='B',session_generation=2)
|
|
now=time.monotonic()
|
|
source.queue.put(event('pose',t=now,sequence=10))
|
|
source.queue.put(event('points',t=now+.001,sequence=11,
|
|
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))))
|
|
until(lambda:service.get().get('planning_phase')=='collecting')
|
|
assert service.get()['query_session_id']=='B' and service.get()['state']=='running'
|
|
assert service.get()['result'] is None
|
|
assert service.scene(run['id'],True).startswith(b'RRF2')
|
|
source.state.update(session_id='C',session_generation=3)
|
|
until(lambda:service.get()['state']=='error')
|
|
finally:service.close()
|
|
assert not lock.locked() and source.owner is None
|
|
restored=PlanningLiveTests(service.drafts,{'test-plugin':source},lock)
|
|
assert restored.get()['query_session_id']=='B' and restored.get()['state']=='error'
|
|
assert restored.get()['scene_available'] and restored.sample is not None
|
|
assert restored.get()['stale'] and not lock.locked()
|
|
assert restored.history()[0]['query_session_id']=='B'
|
|
assert restored.scene(run['id'],True).startswith(b'RRF2')
|
|
|
|
|
|
def test_cancel_retains_raw_and_releases_only_derived_lease(tmp_path,monkeypatch):
|
|
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
|
run=service.start('draft',1)
|
|
until(lambda:service.get()['state']=='waiting')
|
|
service.stop(run['id']);service.close()
|
|
assert service.get()['state']=='cancelled' and not lock.locked() and source.owner is None
|
|
assert service.get()['vehicle_control'] is False
|
|
with pytest.raises(KeyError):service.stop('wrong-id')
|
|
|
|
|
|
def test_native_rerun_base_and_incremental_update_are_valid():
|
|
reference=np.random.default_rng(1).normal(size=(400,3));path=np.array([[0,0,0],[4,0,0]])
|
|
sample=dict(points=reference,path=path,hint=np.eye(4))
|
|
assert scene_bytes('test',reference,path,sample,base=True).startswith(b'RRF2')
|
|
assert scene_bytes('test',reference,path,sample,base=False).startswith(b'RRF2')
|
|
|
|
|
|
def test_receipt_gap_is_not_an_instantaneous_jump_and_clears_fit_window():
|
|
b=LiveCloudBuffer([[0,0,0],[30,0,0]])
|
|
b.ingest(event('pose',t=1,p=(0,0,0)))
|
|
b.ingest(event('points',t=1.1,points=np.array([[0,0,1]])))
|
|
b.ingest(event('pose',t=11,p=(10.4,0,0)))
|
|
sample=b.snapshot()
|
|
assert sample['segment']==1 and len(sample['points'])==0
|
|
assert sample['gaps'][0]['seconds']==10
|
|
assert sample['distance']==pytest.approx(10.4)
|
|
# The same displacement over 100 ms still rejects; no threshold bypass.
|
|
with pytest.raises(ValueError,match='Разрыв координат'):
|
|
b.ingest(event('pose',t=11.1,p=(21,0,0)))
|
|
|
|
|
|
@pytest.mark.parametrize("loss", ["stale", "reference-coverage", "fit-rejected", "worker-error", "pose-jump", "operator-stop", "operator-stop-pending", "multi-lap"])
|
|
def test_live_stationary_bootstrap_keeps_calibration_separate_and_requires_fresh_windows(tmp_path,monkeypatch,loss):
|
|
import k1link.missions.live_tests as module
|
|
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
|
service,source,lock,draft=fixture_service(tmp_path,monkeypatch)
|
|
if loss == "multi-lap":
|
|
draft['route'].update(length_m=4, end_index=4, points=draft['route']['points'][:5])
|
|
clock=[100.]
|
|
monkeypatch.setattr(module,'time',SimpleNamespace(
|
|
monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
|
calls=[]
|
|
fail_next=[None]
|
|
entry_release=threading.Event()
|
|
fresh_entered, fresh_release = threading.Event(), threading.Event()
|
|
def fit(directory,ref,query,hint):
|
|
failure,fail_next[0]=fail_next[0],None
|
|
if failure=="worker-error":raise ValueError("fixture worker unavailable")
|
|
if failure=="fit-rejected":return dict(status="rejected",reasons=["fixture bad fit"])
|
|
if failure == "operator-stop-pending":
|
|
fresh_entered.set()
|
|
assert fresh_release.wait(5)
|
|
calls.append('fresh')
|
|
return dict(status='candidate',T_reference_query=hint.tolist(),matched_query_indices=[0],
|
|
overlap=.9,inlier_rmse_m=.1,reasons=[])
|
|
def entry(directory,ref,path,query,anchor,**kwargs):
|
|
if calls:
|
|
assert kwargs["reference_position"] == [0.,0.,0.]
|
|
calls.append('entry')
|
|
entry_release.wait(3)
|
|
return dict(status='candidate',T_reference_query=np.eye(4).tolist(),matched_query_indices=[0],
|
|
overlap=.9,inlier_rmse_m=.1,reasons=[],initialization=dict(
|
|
complete=True,scope='selected-route',policy=ROUTE_RELOCALIZATION_POLICY,
|
|
expected_attempts=1,attempts=[{}]))
|
|
monkeypatch.setattr(module,'run_registration',fit)
|
|
monkeypatch.setattr(module,'run_route_relocalization',entry)
|
|
service.start('draft',1)
|
|
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
|
sequence=0
|
|
def frame(t,p=(0,0,0)):
|
|
nonlocal sequence
|
|
sequence+=1;source.queue.put(event('pose',t=t,p=p,sequence=sequence))
|
|
sequence+=1;source.queue.put(event('points',t=t+.001,points=points,sequence=sequence))
|
|
return sequence
|
|
try:
|
|
until(lambda:service.get()['state']=='waiting')
|
|
source.state.update(active=True,session_id='B',session_generation=2)
|
|
# Session opens during hardware calibration. Neither control nor poses
|
|
# start route initialization without a usable cloud.
|
|
source.queue.put(event('session-start',t=100,sequence=0))
|
|
until(lambda:service.get()['state']=='running')
|
|
clock[0]=160
|
|
source.queue.put(event('pose',t=159,sequence=0))
|
|
until(lambda:source.queue.empty())
|
|
assert service.get()['planning_phase']=='waiting-cloud' and not calls
|
|
for i in range(20):frame(160+i*.5)
|
|
until(lambda:source.queue.empty())
|
|
assert service.get()['planning_phase']=='collecting'
|
|
clock[0]=170.01
|
|
until(lambda:calls==['entry'])
|
|
# Worker submission and publication happen on separate threads; the
|
|
# fit callback is not an acknowledgement that UI state was persisted.
|
|
until(lambda:service.get()['planning_phase']=='searching')
|
|
# Capture continues while the worker searches; an actual post-ready
|
|
# receipt gap must invalidate the prior, not be hidden by this fixture.
|
|
for i in range(40):
|
|
# Receipts cannot be ahead of the live clock during a continuity
|
|
# check. Drain each fixture frame at its actual delivery time.
|
|
clock[0] = 170 + i * .5 + .01
|
|
frame(170 + i * .5)
|
|
until(lambda:source.queue.empty())
|
|
until(lambda:source.queue.empty())
|
|
clock[0]=190
|
|
entry_release.set()
|
|
until(lambda:service.get().get('initialization_result') is not None
|
|
and service.get()['planning_phase']=='refreshing')
|
|
assert service.get()['planning_phase']=='refreshing'
|
|
assert service.get()['result'] is None and service.accepted_sample is None
|
|
previous_result=None
|
|
for j in range(3):
|
|
begin=190.5+j*5
|
|
for i in range(10):
|
|
clock[0]=begin+i*.5+.002
|
|
frame(begin+i*.5)
|
|
until(lambda:source.queue.empty())
|
|
until(lambda:service.get().get('result_source_sequence') not in (None,previous_result))
|
|
previous_result=service.get()['result_source_sequence']
|
|
assert service.get()['tracking_state']==('tracking' if j==2 else 'acquiring')
|
|
assert service.get()['tracking_established'] == (j == 2)
|
|
assert 'Кандидат совмещения' != service.get()['message']
|
|
assert service._scene_result['matched_query_indices']==([0] if j==2 else [])
|
|
assert calls==['entry','fresh','fresh','fresh']
|
|
assert service.get()['planning_phase']=='tracking'
|
|
if loss == "multi-lap":
|
|
# Exercise the real bootstrap/gate as well as the loop. The fit is
|
|
# synthetic, but fresh windows must continue beyond the old cap.
|
|
service.update(maximum_distance_m=4)
|
|
begin = clock[0] + .5
|
|
for i in range(1, 25):
|
|
leg, offset = divmod(i - 1, 8)
|
|
position = (offset + 1) * .5 if leg % 2 == 0 else 4 - (offset + 1) * .5
|
|
clock[0] = begin + i * .5 + .002
|
|
frame(begin + i * .5, p=(position, 0, 0))
|
|
until(source.queue.empty)
|
|
until(lambda: service.get()['distance_m'] == 12)
|
|
begin = clock[0] + .5
|
|
for i in range(12):
|
|
clock[0] = begin + i * .5 + .002
|
|
frame(begin + i * .5, p=(4, 0, 0))
|
|
until(source.queue.empty)
|
|
until(lambda: calls.count('fresh') >= 6)
|
|
assert service.get()['state'] == 'running'
|
|
assert service.get()['planning_phase'] == 'tracking'
|
|
assert service.accepted_sample is not None
|
|
assert service.get().get('recovery_attempt', 0) == 0
|
|
assert service.get().get('termination_reason') is None
|
|
assert source.state['active'] and lock.locked()
|
|
return
|
|
if loss in {"operator-stop", "operator-stop-pending"}:
|
|
if loss == "operator-stop-pending":
|
|
fail_next[0] = loss
|
|
begin = clock[0] + .5
|
|
for i in range(10):
|
|
clock[0] = begin + i*.5 + .002
|
|
frame(begin+i*.5)
|
|
until(source.queue.empty)
|
|
until(fresh_entered.is_set)
|
|
source.state['spatial_stop_requested'] = True
|
|
# Model the recorded 51-second raw finalisation with a virtual clock.
|
|
clock[0] += 51
|
|
until(lambda: service.get()['state'] == 'completed')
|
|
assert service.get()['termination_reason'] == 'spatial-stop-requested'
|
|
assert service.get().get('recovery_attempt', 0) == 0
|
|
assert service.get()['planning_phase'] == 'ended'
|
|
assert service.accepted_sample is None and source.state['active']
|
|
assert service.get()['result_source_sequence'] == previous_result
|
|
assert not any(t['phase'] in {'lost', 'recovering'}
|
|
for t in service.get()['phase_transitions'])
|
|
fresh_release.set()
|
|
until(lambda: not service.thread.is_alive())
|
|
assert service.get()['result_source_sequence'] == previous_result
|
|
assert service.accepted_sample is None
|
|
return
|
|
if loss=="stale":
|
|
clock[0]+=9
|
|
else:
|
|
import k1link.missions.stationary_live as live_loop
|
|
from k1link.missions.reference_window import ReferenceCoverageError
|
|
original_window=live_loop.reference_window
|
|
failed=[False]
|
|
def window(*args,**kwargs):
|
|
if not failed[0] and loss=="reference-coverage":
|
|
failed[0]=True
|
|
raise ReferenceCoverageError("fixture coverage failure")
|
|
return original_window(*args,**kwargs)
|
|
monkeypatch.setattr(live_loop,"reference_window",window)
|
|
fail_next[0]=loss
|
|
begin=clock[0]+.5
|
|
for i in range(8):
|
|
clock[0]=begin+i*.5+.002
|
|
frame(begin+i*.5,p=(100,0,0) if loss=="pose-jump" else (0,0,0))
|
|
until(source.queue.empty)
|
|
if service.get()['planning_phase']=='recovering':break
|
|
monkeypatch.setattr(live_loop,"reference_window",original_window)
|
|
until(lambda:service.get()['tracking_state']=='lost')
|
|
until(lambda:service.get()['planning_phase']=='recovering')
|
|
assert service.accepted_sample is None
|
|
assert service.get()['tracking_established'] is True
|
|
assert service.get()['state']=='running' and source.state['active']
|
|
assert service.get()['recovery_attempt']==1
|
|
previous_fresh=calls.count('fresh')
|
|
# A fresh stationary prefix, never replay of the previously accepted fit.
|
|
begin=clock[0]+.5
|
|
for i in range(20):
|
|
clock[0]=begin+i*.5+.002
|
|
frame(begin+i*.5)
|
|
until(source.queue.empty)
|
|
clock[0]=begin+10.01
|
|
until(lambda:calls.count('entry')==2)
|
|
until(lambda:service.run.get('initialization_temporal',{}).get('provisional') is True
|
|
and service.run.get('planning_phase')=='recovering'
|
|
and service.run.get('planning_reason')=='provisional-prior')
|
|
assert service.accepted_sample is None
|
|
begin=clock[0]+.5
|
|
for j in range(3):
|
|
for i in range(10):
|
|
clock[0]=begin+j*5+i*.5+.002
|
|
frame(begin+j*5+i*.5)
|
|
until(source.queue.empty)
|
|
until(lambda:calls.count('fresh')>=previous_fresh+1+j)
|
|
assert service.get()['planning_phase']==('tracking' if j==2 else 'recovering')
|
|
assert service.get()['tracking_state']=='tracking'
|
|
source.state['active']=False
|
|
until(lambda:service.get()['state']=='completed')
|
|
finally:
|
|
entry_release.set();fresh_release.set();service.close()
|
|
assert not lock.locked() and source.owner is None
|
|
|
|
|
|
def test_operator_can_retry_a_failed_route_identification_without_stopping_capture(tmp_path,monkeypatch):
|
|
import k1link.missions.live_tests as module
|
|
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
|
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
|
clock=[100.]
|
|
monkeypatch.setattr(module,'time',SimpleNamespace(
|
|
monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
|
searches=[]
|
|
def rejected_entry(*_args, **_kwargs):
|
|
searches.append('entry')
|
|
return dict(status='rejected',T_reference_query=np.eye(4).tolist(),matched_query_indices=[],
|
|
overlap=.6,inlier_rmse_m=.28,reasons=['Большое расстояние между поверхностями.'],
|
|
initialization=dict(complete=True,scope='selected-route',
|
|
policy=ROUTE_RELOCALIZATION_POLICY,expected_attempts=1,attempts=[{}],
|
|
reason='no-route-location'))
|
|
monkeypatch.setattr(module,'run_route_relocalization',rejected_entry)
|
|
run=service.start('draft',1)
|
|
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
|
sequence=0
|
|
def frame(t,p=(0,0,0)):
|
|
nonlocal sequence
|
|
sequence+=1;source.queue.put(event('pose',t=t,p=p,sequence=sequence))
|
|
sequence+=1;source.queue.put(event('points',t=t+.001,points=points,sequence=sequence))
|
|
try:
|
|
until(lambda:service.get()['state']=='waiting')
|
|
source.state.update(active=True,session_id='B',session_generation=2)
|
|
for i in range(20):frame(100+i*.5)
|
|
until(lambda:source.queue.empty());clock[0]=110.01
|
|
until(lambda:service.get()['planning_phase']=='lost')
|
|
assert searches==['entry'] and source.owner=='planning-'+run['id']
|
|
retried=service.request_reinitialization(run['id'])
|
|
assert retried['initialization_attempt']==2
|
|
assert retried['reinitialization_count']==1
|
|
assert retried['initialization_result'] is None
|
|
until(lambda:not service.reinitialization_requested)
|
|
# The scanner may be carried to a better point while the first failed
|
|
# location stays recoverable; those receipts must not contaminate a
|
|
# new stationary prefix or stop K1 capture.
|
|
frame(111.0)
|
|
frame(111.5,p=(1,0,0))
|
|
until(lambda:source.queue.empty())
|
|
assert service.get()['planning_phase']=='lost'
|
|
assert service.get()['state']=='running'
|
|
frame(112)
|
|
frame(112.5)
|
|
until(source.queue.empty)
|
|
assert service.get()['planning_phase']=='lost' # No implicit retry after a moved prefix.
|
|
retried=service.request_reinitialization(run['id'])
|
|
assert retried['initialization_attempt']==3
|
|
until(lambda:not service.reinitialization_requested)
|
|
for i in range(20):frame(120+i*.5)
|
|
until(lambda:source.queue.empty());clock[0]=130.01
|
|
until(lambda:searches==['entry','entry'])
|
|
until(lambda:service.get()['planning_phase']=='lost')
|
|
assert service.get()['query_session_id']=='B'
|
|
assert source.owner=='planning-'+run['id']
|
|
finally:
|
|
source.state['active']=False
|
|
service.close()
|
|
assert not lock.locked() and source.owner is None
|
|
|
|
|
|
def test_stationary_cancel_while_searching_does_not_publish_late_prior(tmp_path,monkeypatch):
|
|
import k1link.missions.live_tests as module
|
|
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
|
clock=[100.]; entered=threading.Event();release=threading.Event()
|
|
monkeypatch.setattr(module,'time',SimpleNamespace(monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
|
def search(*args,**kwargs):
|
|
entered.set();release.wait(3)
|
|
return dict(status='rejected',reasons=['test-cancel'])
|
|
monkeypatch.setattr(module,'run_route_relocalization',search)
|
|
run=service.start('draft',1)
|
|
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
|
try:
|
|
until(lambda:service.get()['state']=='waiting')
|
|
source.state.update(active=True,session_id='B',session_generation=2)
|
|
for i in range(20):
|
|
source.queue.put(event('pose',t=100+i*.5,sequence=i*2+1))
|
|
source.queue.put(event('points',t=100+i*.5+.001,sequence=i*2+2,points=points))
|
|
until(lambda:source.queue.empty());clock[0]=110.01
|
|
until(entered.is_set)
|
|
service.stop(run['id']);release.set()
|
|
until(lambda:service.get()['state']=='cancelled')
|
|
finally:
|
|
release.set();service.close()
|
|
assert service.accepted_sample is None and service.get()['result'] is None
|
|
assert not lock.locked() and source.owner is None
|
|
|
|
|
|
@pytest.mark.parametrize("cause", ["operator-stop", "motion", "source-ended"])
|
|
def test_long_search_receives_cancellation_without_waiting_for_total_search(tmp_path,monkeypatch,cause):
|
|
import k1link.missions.live_tests as module
|
|
from k1link.missions.route_relocalization_worker import incomplete_result
|
|
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
|
clock=[100.]
|
|
entered, cancelled = threading.Event(), threading.Event()
|
|
monkeypatch.setattr(module,'time',SimpleNamespace(
|
|
monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
|
def search(*args, cancel_event, **kwargs):
|
|
entered.set()
|
|
assert cancel_event.wait(3), 'Planner did not cancel its own search child'
|
|
cancelled.set()
|
|
return incomplete_result('worker-cancelled')
|
|
monkeypatch.setattr(module,'run_route_relocalization',search)
|
|
run=service.start('draft',1)
|
|
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
|
try:
|
|
until(lambda:service.get()['state']=='waiting')
|
|
source.state.update(active=True,session_id='B',session_generation=2)
|
|
for i in range(20):
|
|
source.queue.put(event('pose',t=100+i*.5,sequence=i*2+1))
|
|
source.queue.put(event('points',t=100+i*.5+.001,sequence=i*2+2,points=points))
|
|
until(source.queue.empty)
|
|
clock[0]=110.01
|
|
until(entered.is_set)
|
|
if cause=='operator-stop':
|
|
service.stop(run['id'])
|
|
elif cause=='source-ended':
|
|
source.state['active']=False
|
|
else:
|
|
source.queue.put(event('pose',t=110.005,sequence=41,p=(.2,0,0)))
|
|
until(lambda:service.get()['planning_phase']=='lost')
|
|
until(cancelled.is_set)
|
|
assert service.accepted_sample is None
|
|
if cause=='motion':
|
|
assert service.get()['state']=='running' and source.state['active']
|
|
service.request_reinitialization(run['id'])
|
|
until(lambda:service.get()['planning_phase']=='waiting-cloud')
|
|
assert service.get()['initialization_attempt']==2
|
|
finally:
|
|
source.state['active']=False
|
|
service.close()
|
|
assert not lock.locked() and source.owner is None
|