Advance through verified water until port arrival terrain is resident

Author: woensug-choi

README.ko.md

@@ -82,6 +82,8 @@ path = planner.route(start, goal)
 
 `port_approach.corridor(tiles, spec, start, target)`는 도착 영역으로 들어가는 가능한 경로를 비교합니다. `connections.CoastConnector`는 OSM 육지 형상으로 전역 항로와 항구 사이의 연결을 단축합니다. 이 검사는 해안선 여유만 확인하며 수심은 확인하지 않습니다.
 
+도착 영역이 현재 적재된 지형 밖에 있으면 검증한 지역 접근 경로를 따라 이동합니다. 실제로 500 m 진행하면 전체 도착 경로를 다시 검사합니다. 수심이 없거나 도착 영역이 막혀 있으면 차량이 대기합니다.
+
 항구를 지나쳐 돌아오는 경로를 줄이기 위해 앞쪽 외해 경유점도 연결 후보로 검사합니다.
 필수 통과점과 보존하도록 지정한 경로 구간은 유지합니다. 탐색에 실패하면 입력 경로를
 유지합니다. 실제 주행에는 수치 지형 검사가 필요합니다.

README.md

@@ -83,6 +83,8 @@ shortened route preserves its exit and coastline preference.
 
 `port_approach.corridor(tiles, spec, start, target)` compares feasible entries to the arrival region. `connections.CoastConnector` shortens graph-to-port connections using OSM land polygons; it checks coastline margins, not bathymetry.
 
+If the arrival area is outside resident terrain, the navigator advances along a verified regional approach. It retries the complete arrival corridor after 500 m of measured progress. Unknown depth or a blocked arrival area keeps the vessel waiting.
+
 The connector considers earlier offshore waypoints to avoid overshooting a port.
 It retains required passage points and any preserved route prefix. A failed
 search retains the supplied route. Numeric terrain checks remain necessary.

src/posim_routing/port_approach.py

@@ -8,6 +8,10 @@
 APPROACH_DISTANCE_M = 5000.
 
 
+class ArrivalOutsideResidentTerrain(RuntimeError):
+    """The vessel must advance before its arrival area is resident."""
+
+
 @lru_cache(maxsize=8)
 def forward_frame(spec):
     from posim_terrain.terrain import projection
@@ -28,12 +32,18 @@ def bend_lookahead(native, regional, seed_spec):
 def shortest_arrival(planner, field, origin, candidates):
     """Compare feasible region entries within one shared planning budget."""
     best, best_floor, best_length = None, None, math.inf
-    for goal in sorted(candidates, key=lambda point: math.dist(origin[:2], point[:2])):
+    floors = [(goal, field.maximum(*goal[:2], 25.)) for goal in candidates]
+    coverage = field
+    while hasattr(coverage, 'field'):
+        coverage = coverage.field
+    if all(floor is None for _, floor in floors) and all(
+            coverage.maximum(*goal[:2], 25.) is None for goal in candidates):
+        raise ArrivalOutsideResidentTerrain('port_arrival_region_outside_resident_terrain')
+    for goal, floor in sorted(floors, key=lambda item: math.dist(origin[:2], item[0][:2])):
         if math.dist(origin[:2], goal[:2]) >= best_length-.01:
             break
         if time.monotonic() > planner.deadline:
             break
-        floor = field.maximum(*goal[:2], 25.)
         if floor is None or floor > -3.:
             continue
         try:

src/posim_routing/regional.py

@@ -159,7 +159,9 @@ def update(self,state,target,needed=False,terminal=False,retain_coast=False):
         guide_exhausted=(plan.get('route_key')==key and plan.get('mapped_guidance')
                          and plan.get('points') and plan['index']>=len(plan['points']))
         needed=needed or bool(guide_exhausted)
-        prefer_port=terminal and plan.get('mode')!='port_approach'
+        approach_runway=(plan.get('mode')=='port_approach_lookahead' and point is not None
+                         and distance(state['position'],plan['probe_position'])<500.)
+        prefer_port=terminal and plan.get('mode')!='port_approach' and not approach_runway
         if terminal and point is not None and not prefer_port and not (self.task and self.task.done()):return point
         # Prepare the next coastal lookahead while the current checked plan
         # still has runway. Otherwise its end briefly falls back to an offshore
@@ -234,7 +236,7 @@ async def prepare_port(self,state,target,key):
         # A port's narrow entrance is resolved by the same triangles already
         # in Gazebo. A 20 km / 257 grid can close that entrance and create a
         # cyclic offshore frontier. No scenery preference replaces this goal.
-        from .port_approach import corridor
+        from .port_approach import corridor, ArrivalOutsideResidentTerrain
         native=await self.rpc('/promotional/state')
         stream=await self.rpc('/stream/state')
         epoch=(native.get('origin') or {}).get('epoch',0)
@@ -247,7 +249,15 @@ async def prepare_port(self,state,target,key):
         from .land import guard_from_spec
         from posim_terrain.canal_override import in_region
         guard=None if in_region(current) else await asyncio.to_thread(guard_from_spec,spec,64000.)
-        value=await asyncio.to_thread(corridor,tiles,spec,current,target,guard=guard)
+        try:
+            value=await asyncio.to_thread(corridor,tiles,spec,current,target,guard=guard)
+        except ArrivalOutsideResidentTerrain:
+            # Robot-centred streaming cannot load a distant entrance while the
+            # ship is stationary. Advance on verified regional water, with the
+            # native resident corridor rechecking every movement command.
+            value=await self.prepare(state,target,key,needed=True,extent=6000)
+            value.update(mode='port_approach_lookahead',arrival_centre=list(target))
+            return value
         latest=await self.rpc('/promotional/state')
         if latest['session_nonce']!=native['session_nonce'] or (latest.get('origin') or {}).get('epoch',0)!=epoch:
             raise RuntimeError('port_frame_changed')
@@ -269,7 +279,7 @@ async def terrain_region(self, centre, extent):
             except Exception:break
             if (sources.get('baseline_status') or {}).get('status')!='unavailable':break
         raise RuntimeError('regional_terrain_unavailable')
-    async def prepare(self,state,target,key,nearest=None,needed=True):
+    async def prepare(self,state,target,key,nearest=None,needed=True,extent=None):
         from posim_terrain.canal_override import in_region
         canal=in_region(state['position'])
         if self.coastal and not canal and nearest is None and self.index_task is not None:
@@ -289,7 +299,7 @@ async def prepare(self,state,target,key,nearest=None,needed=True):
             # distant coast. The 1:10M discovery line never supplies depth.
             heading,_,_=GEO.inv(*pos,*nearest[1])
             centre=list(GEO.fwd(*pos,heading,min(6000.,nearest[0]-6000.))[:2])
-        extent=4000 if canal else 20000
+        extent=extent or (4000 if canal else 20000)
         if self.before_prepare is not None:await self.before_prepare()
         job=await self.terrain_region(centre,extent)
         grid=await self.rpc('/terrain/jobs/'+job['id']+'/files/terrain.json',timeout=20)

tests/test_port_residency.py

@@ -0,0 +1,34 @@
+"""Arrival coverage may need another checked streaming step."""
+import math
+import unittest
+from types import SimpleNamespace
+from unittest.mock import AsyncMock,patch
+from posim_routing.port_approach import shortest_arrival,ArrivalOutsideResidentTerrain
+from posim_routing.regional import RegionalNavigator,REGIONAL_POLICY
+
+class ArrivalCoverage(unittest.TestCase):
+    def test_missing_arrival_coverage_is_distinct_from_shallow_water(self):
+        planner=SimpleNamespace(deadline=math.inf,route=lambda *_: self.fail('unknown goal routed'))
+        with self.assertRaises(ArrivalOutsideResidentTerrain):
+            shortest_arrival(planner,SimpleNamespace(maximum=lambda *_:None),[0,0,0],[[2000,0,0]])
+        with self.assertRaisesRegex(RuntimeError,'no_confirmed_corridor'):
+            shortest_arrival(planner,SimpleNamespace(maximum=lambda *_:-2.),[0,0,0],[[2000,0,0]])
+        blocked=SimpleNamespace(maximum=lambda *_:None,field=SimpleNamespace(maximum=lambda *_:-10.))
+        with self.assertRaisesRegex(RuntimeError,'no_confirmed_corridor'):
+            shortest_arrival(planner,blocked,[0,0,0],[[2000,0,0]])
+
+class PortRenewal(unittest.IsolatedAsyncioTestCase):
+    async def test_verified_runway_is_used_before_resident_arrival_retry(self):
+        nav=RegionalNavigator(AsyncMock());key=[0,1,11]
+        state=dict(lap=0,leg=1,waypoint=11,position=[118.10,24.425],native={},
+            regional_navigation=dict(route_key=key,points=[[118.093,24.428]],index=0,
+            mode='port_approach_lookahead',probe_position=[118.10,24.425],policy=REGIONAL_POLICY))
+        nav.prepare_port=AsyncMock()
+        self.assertEqual(nav.update(state,[118.077,24.436],terminal=True),[118.093,24.428])
+        self.assertIsNone(nav.task)
+        state['position']=[118.094,24.427]
+        nav.update(state,[118.077,24.436],terminal=True)
+        self.assertIsNotNone(nav.task)
+        await nav.close()
+
+if __name__=='__main__':unittest.main()