Keep coastal voyages on checked onward water routes

README.ko.md

@@ -73,6 +73,10 @@ path = planner.route(start, goal)
 - 기본 육지 여유 거리: 80 m
 - 기본 탐색 제한 시간: 10초
 
+해안 경로는 지역 격자 안의 다음 목표나 전방 출구까지 검증된 수역을 따라갑니다.
+경로 끝에서도 만과 곶을 지나 계속 갈 수 있어야 합니다. 일반 해안 경유점은 항로
+진행을 안내합니다. 항구와 필수 통과점은 실제 도착 위치를 검사합니다.
+
 ## 구조
 
 | 경로 | 내용 |
@@ -90,6 +94,11 @@ path = planner.route(start, goal)
 
 `RegionalNavigator`는 `/terrain/jobs`와 지형 작업 조회를 위한 비동기 `rpc(path, body=None, timeout=...)` 함수를 받습니다. 선택적인 `before_prepare` 함수로 호출 앱의 캐시를 관리합니다. Gazebo 실행·종료는 호출 앱이 담당합니다.
 
+같은 구간에서 일반 경유점을 통과할 때만 `retain_coast=True`를 사용합니다.
+`state['regional_waiting']`이 있으면 검증된 다음 경로가 준비될 때까지 추진을
+멈춥니다. 물리 계산과 센서 수신은 계속합니다. 선택적인 해안 경로 준비에 실패하거나
+결과가 비어 있어도 검증된 해안 경로를 외해 목표로 바꾸지 않습니다.
+
 ## 시험과 이용 조건
 
 ```sh

README.md

@@ -50,6 +50,11 @@ path = planner.route(start, goal)
 
 Choose start and goal inside confirmed water. The planner rejects unknown coverage and continuously checks the swept footprint against the piecewise planar collision field.
 
+Coastal lookahead follows a checked water route to the onward target or a forward
+exit from the regional grid. Its endpoint has an onward route around bays and
+headlands. Ordinary coastal course vertices guide progress; ports and required
+passage points retain their arrival checks.
+
 | Policy | Default |
 |---|---|
 | Surface local depth clearance | 1.5 m |
@@ -90,6 +95,12 @@ search retains the supplied route. Numeric terrain checks remain necessary.
 
 `RegionalNavigator` accepts an async `rpc(path, body=None, timeout=...)` callback for `/terrain/jobs` and job reads. The optional `before_prepare` callback lets the caller manage its cache. It does not start or stop Gazebo.
 
+Set `retain_coast=True` only for an ordinary shape advance within the same leg.
+When `state['regional_waiting']` is present, the caller holds thrust until a
+checked replacement is ready. Physics and sensor sampling continue during the
+wait. Failed or empty optional renewal cannot replace a checked coastal route
+with an offshore goal.
+
 ## Tests and terms
 
 ```sh

pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "posim-routing"
-version = "0.1.1"
+version = "0.1.2"
 description = "Global maritime and terrain checked coastal route planning"
 requires-python = ">=3.10"
 license = {text = "Apache-2.0"}

src/posim_routing/coastal.py

@@ -151,29 +151,32 @@ def coastal_corridor(terrain, spec, start, target, timeout=25.):
     safe &= travel <= 18000
     to_target = np.hypot(xx-destination[0], yy-destination[1])
     progress = remaining-to_target
-    # Never select a coast on the other side of an island solely for its view.
-    candidates = safe & (coast_distance >= 200) & (coast_distance <= MAXIMUM_STANDOFF_M)
-    candidates &= (travel >= 1000) & (travel <= 15000)
-    candidates &= progress >= min(1000., remaining*.2)
-    if not candidates.any():
+    coast = safe & (coast_distance >= 200) & (coast_distance <= MAXIMUM_STANDOFF_M)
+    coast &= (travel >= 1000) & (travel <= 15000)
+    if not (coast & (progress >= min(1000., remaining*.2))).any():
         return None
-    heading = (destination-origin)/remaining
-    nominal = origin + heading*min(7500., remaining)
-    # Rank stand-off bands before along-course convenience. Otherwise a
-    # waypoint a few kilometres nearer the nominal route can win even when
-    # an equally safe 250 m coastal corridor exists. One grid cell reflects
-    # the shoreline resolution; it is not permission to relax hull clearance.
-    band = np.floor(np.abs(coast_distance-STANDOFF_M)/max(dx, dy))
-    score = band*100000. + np.hypot(xx-nominal[0], yy-nominal[1])
-    scores = np.where(candidates, score, math.inf)
+    # A near-coast endpoint can be the head of a closed bay. First establish
+    # a through route to the onward target or a forward water exit, then take
+    # its coastal prefix. Distance gain at an isolated endpoint is insufficient.
+    goal_node = (int(np.argmin(abs(ys-destination[1]))), int(np.argmin(abs(xs-destination[0]))))
+    target_inside = (xs[0]+margin < destination[0] < xs[-1]-margin
+                     and ys[0]+margin < destination[1] < ys[-1]-margin
+                     and safe[goal_node])
+    edge = ((xx <= xs[0]+margin+2*dx) | (xx >= xs[-1]-margin-2*dx)
+            | (yy <= ys[0]+margin+2*dy) | (yy >= ys[-1]-margin-2*dy))
+    exits = safe & edge & (travel >= 1000) & (progress >= min(1000., remaining*.2))
+    scores = np.where(exits, to_target, math.inf)
     goals = []
-    for _ in range(6):
+    if target_inside:
+        goals.append(goal_node)
+    for _ in range(0 if target_inside else 6):
         node = tuple(map(int, np.unravel_index(np.argmin(scores), scores.shape)))
         if not math.isfinite(float(scores[node])):
             break
         goals.append(node)
         scores[np.hypot(xx-xs[node[1]], yy-ys[node[0]]) < 600] = math.inf
-    penalty = 1 + 8*np.minimum(6, np.maximum(0, coast_distance-(STANDOFF_M+150))/500)
+    penalty = (1 + np.minimum(1,np.abs(coast_distance-STANDOFF_M)/STANDOFF_M)
+               + 8*np.minimum(6,np.maximum(0,coast_distance-(STANDOFF_M+150))/500))
     validator = DetourPlanner(CachedTerrain(field), [*origin, 0.], True,
                              timeout=timeout, maximum_radius=18000, maximum_length=40000)
     validator.radius = REGIONAL_BUFFER_M
@@ -221,6 +224,23 @@ def xy(node):
         while nodes[-1] != start_node:
             nodes.append(parents[nodes[-1]])
         nodes.reverse()
+        coastal_nodes = [index for index,node in enumerate(nodes) if coast[node]]
+        if not coastal_nodes:
+            continue
+        if not any(progress[nodes[index]] >= min(1000., remaining*.2) for index in coastal_nodes):
+            continue
+        through = [[*origin, 0.], *[xy(node) for node in nodes[1:]]]
+        if sum(math.dist(a, b) for a, b in zip(through, through[1:])) > 40000:
+            continue
+        if not all(validator.segment(a,b,record=True) for a,b in zip(through,through[1:])):
+            continue
+        bands = {index: math.floor(abs(coast_distance[nodes[index]]-STANDOFF_M)/max(dx,dy))
+                 for index in coastal_nodes}
+        closest_band = min(bands.values())
+        end = max(index for index in coastal_nodes if bands[index]==closest_band)
+        onward_nodes = nodes[end+1:]
+        endpoint = nodes[end]
+        nodes = nodes[:end+1]
         # Shorten the grid path without erasing its coastal preference. Long
         # offshore shortcuts across bays have a higher coast-weighted cost.
         points, index = [[*origin, 0.]], 0
@@ -247,8 +267,12 @@ def xy(node):
                     index=0, mode="coastal", planning_seconds=time.monotonic()-started,
                     search_nodes=expanded, minimum_regional_clearance_m=validator.minimum,
                     preferred_standoff_m=STANDOFF_M,
-                    endpoint_standoff_m=float(coast_distance[goal]),
+                    endpoint_standoff_m=float(coast_distance[endpoint]),
                     maximum_preferred_standoff_m=MAXIMUM_STANDOFF_M,
-                    progress_to_course_waypoint_m=float(progress[goal]),
+                    progress_to_course_waypoint_m=float(progress[endpoint]),
+                    onward_points=[list(inverse.transform(*xy(node)[:2])) for node in onward_nodes],
+                    onward_exit=list(inverse.transform(*xy(goal)[:2])),
+                    onward_exit_progress_m=float(progress[goal]),
+                    onward_verified=True,
                     scope="numeric terrain coastal preference; native corridors checked separately")
     return None

src/posim_routing/course.py

@@ -9,7 +9,7 @@
 GEO = Geod(ellps='WGS84')
 
 
-def passed_vertex(previous, vertex, following, position, maximum_offset=80000., minimum_leg=2000., corner_margin=2000.):
+def passed_vertex(previous, vertex, following, position, maximum_offset=80000., minimum_leg=2000., corner_margin=2000., adjacent_limit=True):
     incoming_back, _, incoming = GEO.inv(*vertex, *previous)
     outgoing_bearing, _, outgoing = GEO.inv(*vertex, *following)
     bearing, _, separation = GEO.inv(*vertex, *position)
@@ -20,7 +20,8 @@ def passed_vertex(previous, vertex, following, position, maximum_offset=80000.,
     # that its vertex has been passed. Only round ordinary progressive bends.
     if math.cos(math.radians(outgoing_bearing-incoming_bearing)) < -.25:
         return False
-    if separation > min(maximum_offset, min(incoming, outgoing)*.5):
+    horizon = min(maximum_offset, min(incoming, outgoing)*.5) if adjacent_limit else maximum_offset
+    if separation > horizon:
         return False
     along_in = separation*math.cos(math.radians(bearing-incoming_bearing))
     along_out = separation*math.cos(math.radians(bearing-outgoing_bearing))
@@ -48,7 +49,7 @@ def coastal_shape(leg, index, state):
 def passed_coastal_shape(leg, index, state):
     points=leg['coordinates']
     return coastal_shape(leg,index,state) and passed_vertex(
-        points[index-1],points[index],points[index+1],state['position'])
+        points[index-1],points[index],points[index+1],state['position'],adjacent_limit=False)
 
 
 def course_target(leg,index,state):
@@ -58,5 +59,9 @@ def course_target(leg,index,state):
     incoming_back,_,incoming=GEO.inv(*vertex,*points[index-1])
     bearing,_,outgoing=GEO.inv(*vertex,*points[index+1])
     if min(incoming,outgoing)<2000 or math.cos(math.radians(bearing-incoming_back-180.))<-.25:return vertex
-    if GEO.inv(*state['position'],*vertex)[2]>min(80000.,min(incoming,outgoing)*.5):return vertex
-    return list(GEO.fwd(*vertex,bearing,min(25000.,outgoing*.25))[:2])
+    if GEO.inv(*state['position'],*vertex)[2]>80000.:return vertex
+    # Coastal travel can pass well inside an offshore graph bend. A short
+    # offset from that bend still pulls the vessel out to sea. Plan towards
+    # the onward vertex; measured passage advances the shape index separately.
+    # Neither this goal proposal nor passage records arrival at the final port.
+    return list(points[index+1])

src/posim_routing/regional.py

@@ -78,7 +78,12 @@ def active_point(self,state,key):
                         state['position'],maximum_offset=200.,minimum_leg=0.,corner_margin=40.))
             if not reached and not passed:break
             plan['index']+=1
-        if plan['index']>=len(plan['points']):return None
+        if plan['index']>=len(plan['points']):
+            # Only consume an explicitly validated finite onward route. This
+            # supplies runway during renewal without extrapolating a tangent.
+            if plan.get('onward_verified') and plan.get('onward_points'):
+                plan['points'].extend(plan.pop('onward_points'));plan['onward_used']=True
+            else:return None
         point=plan['points'][plan['index']]
         # A local detour may bypass the coarse frontier without entering its
         # 80 m arrival circle. Reusing that persisted goal hundreds of km
@@ -101,16 +106,26 @@ def active_point(self,state,key):
                 self.discard(state,'regional_goal_progress_stalled')
                 return None
         return point
-    def update(self,state,target,needed=False,terminal=False):
+    def update(self,state,target,needed=False,terminal=False,retain_coast=False):
         from posim_terrain.canal_override import in_region
         canal=in_region(state['position'])
         key=[state['lap'],state['leg'],state['waypoint']]
         from .port_approach import APPROACH_DISTANCE_M
         terminal=terminal and distance(state['position'],target)<APPROACH_DISTANCE_M
+        state.pop('regional_waiting',None)
         if self.key!=key:
             if self.task:self.task.cancel()
             self.task=None;self.key=key;self.retry=0
             self.probed=None
+            plan=state.get('regional_navigation') or {};old=plan.get('route_key')
+            if (retain_coast and not terminal and not canal and old and len(old)==3
+                    and old[:2]==key[:2] and old[2]<key[2]
+                    and plan.get('policy')==REGIONAL_POLICY
+                    and plan.get('mode') in ('coastal','coastal_approach')):
+                plan['route_key']=list(key);plan.pop('continuation',None)
+                state['regional_handoff']=dict(previous_key=old,route_key=list(key),
+                    position=list(state['position']),reason='measured_coastal_shape_advance')
+                self.progress=None
         nearest=None
         if self.coastal and not canal:
             if self.index_task is None and self.coast_index is None:
@@ -145,6 +160,8 @@ def update(self,state,target,needed=False,terminal=False):
                      and distance(state['position'],plan['points'][-1])<2500
                      and distance(state['position'],plan.get('probe_position',state['position']))>=1000
                      and (self.probed is None or distance(self.probed,state['position'])>=1000))
+        renew_coast=renew_coast or (point is not None and nearest is not None
+                                    and plan.get('mode')=='coastal' and not plan.get('onward_verified'))
         coast_needed=nearest is not None and (self.probed is None or distance(self.probed,state['position'])>=3000)
         prefer_coast=(point is not None and coast_needed and not needed and
                       (state.get('regional_navigation') or {}).get('mode') not in ('coastal','coastal_approach'))
@@ -160,10 +177,17 @@ def update(self,state,target,needed=False,terminal=False):
                 value=self.task.result()
                 # A failed optional preference must preserve a previously
                 # checked detour while waiting for a better coastal region.
-                if not (point is not None and not value['points']):state['regional_navigation']=value
+                coast_plan=(plan.get('route_key')==key and plan.get('mode') in ('coastal','coastal_approach')
+                            and plan.get('policy')==REGIONAL_POLICY)
+                preserve_empty=(not value['points'] and (point is not None or
+                                (coast_plan and nearest is not None and not terminal and not needed)))
+                if not preserve_empty:state['regional_navigation']=value
                 self.probed=list(value['probe_position']) if value.get('mode') not in ('coastal','coastal_approach') else None
                 state['regional_planning']=dict(status='ready' if value['points'] else 'course_retained',
                                                 job=value['job'],mode=value.get('mode','detour'),coastal_preference_error=value.get('coastal_preference_error'))
+                if preserve_empty and coast_plan:
+                    self.probed=None;self.retry=time.monotonic()+60
+                    state['regional_planning']['status']='retrying'
             except (Exception,asyncio.CancelledError) as e:
                 state['regional_planning']=dict(status='retrying',reason=str(e)[:160]);self.retry=time.monotonic()+60
             self.task=None
@@ -179,18 +203,19 @@ def update(self,state,target,needed=False,terminal=False):
             state['regional_planning']=dict(status='background',mode='port_approach' if terminal else 'coastal' if coast_needed else 'detour')
             self.task=asyncio.create_task(self.prepare_port(state,target,key) if terminal else self.prepare(state,target,key,nearest,needed))
         if point is not None:return point
-        if self.task and plan.get('route_key')==key and plan.get('mode') in ('coastal','coastal_approach'):
-            points=plan.get('points',[])
-            if len(points)>=2 and distance(state['position'],points[-1])<1500:
-                continuation=plan.get('continuation')
-                if continuation is None or distance(state['position'],continuation)<80:
-                    heading=GEO.inv(*points[-2],*points[-1])[0]
-                    continuation=list(GEO.fwd(*state['position'],heading,750.)[:2])
-                    plan['continuation']=continuation
-                if distance(continuation,target)<distance(state['position'],target):
-                    # Only a goal proposal: the native resident-terrain planner
-                    # must approve it before thrust is commanded.
-                    return continuation
+        plan=state.get('regional_navigation') or {}
+        if (plan.get('route_key')==key and plan.get('policy')==REGIONAL_POLICY
+                and plan.get('mode') in ('coastal','coastal_approach') and plan.get('points')
+                and plan['index']>=len(plan['points'])
+                and distance(state['position'],plan['points'][-1])<1500
+                and (nearest is not None or self.task or self.index_task)):
+            # An exhausted checked path is not permission to steer offshore.
+            # The caller holds thrust, keeping physics and sensors running.
+            # Retry backoff and optional empty results retain this boundary.
+            plan.pop('continuation',None)
+            state['regional_waiting']=dict(reason='coastal_renewal',job=plan['job'],
+                                           endpoint=plan['points'][-1])
+            return plan['points'][-1]
         return target
     async def prepare_port(self,state,target,key):
         # A port's narrow entrance is resolved by the same triangles already