Bound coastal preference by checked distance and destination progress

README.ko.md

@@ -61,19 +61,22 @@ path = planner.route(start, goal)
 구간의 끝에서는 다음 구간이 준비될 때까지 기다립니다. 날짜 변경선에서도
 경도가 이어지도록 처리합니다.
 
-해안 경로는 다음 목표나 지역 지형의 출구까지 검증된 수역을 따라갑니다.
-끝점에는 만과 곶을 돌아가는 후속 경로가 이어집니다. 일반 해안 경유점은
-진행 방향을 안내하며, 항구와 필수 통과점은 도착 조건을 유지합니다.
+목적지 항구로 진행할 때만 해안 가까운 경로를 우선합니다. 마지막 구간은
+항로 목표와 항구를 향해야 합니다. 같은 후속 목표까지 검증한 짧은 경로보다
+전체 거리가 15% 넘게 늘어나면 일반 수상 경로를 사용합니다.
+육지나 여울을 피하려면 만과 섬을 돌아갈 수 있습니다.
+항구와 필수 통과점은 도착 조건을 유지합니다.
 
 후속 경유점은 검증된 구간의 끝을 표시합니다. 차량이 지나가는 전체 폭의
 수심과 지도상 육지를 확인한 뒤 직선 구간의 중간 지점을 줄입니다.
-출구와 해안 선호 경로는 유지합니다.
+검증한 출구는 유지합니다.
 
 | 기준 | 기본값 |
 |---|---|
 | 수상 차량 로컬 수심 여유 | 1.5 m |
 | 수상 차량 반경 + 추적 여유 | 3.5 m + 4 m |
 | 해안 선호 거리 | 250 m |
+| 해안 선호에 따른 추가 거리 한도 | 15% |
 | 해안 지역 여유 폭 / 수심 | 40 m / 2 m |
 | 도착 반경 | 300 m |
 | 도착 목표 검사 | 반경 350 m / 수심 3 m |
@@ -94,10 +97,6 @@ path = planner.route(start, goal)
 - 기본 육지 여유 거리: 80 m
 - 기본 탐색 제한 시간: 10초
 
-해안 경로는 지역 격자 안의 다음 목표나 전방 출구까지 검증된 수역을 따라갑니다.
-경로 끝에서도 만과 곶을 지나 계속 갈 수 있어야 합니다. 일반 해안 경유점은 항로
-진행을 안내합니다. 항구와 필수 통과점은 실제 도착 위치를 검사합니다.
-
 ## 구조
 
 | 경로 | 내용 |
@@ -117,6 +116,7 @@ path = planner.route(start, goal)
 `RegionalNavigator`는 `/terrain/jobs`와 지형 작업 조회를 위한 비동기 `rpc(path, body=None, timeout=...)` 함수를 받습니다. 선택적인 `before_prepare` 함수로 호출 앱의 캐시를 관리합니다. Gazebo 실행·종료는 호출 앱이 담당합니다.
 
 같은 구간에서 일반 경유점을 통과할 때만 `retain_coast=True`를 사용합니다.
+로컬 목표가 항로 경유점이면 최종 항구를 `progress_target`으로 전달합니다.
 `state['regional_waiting']`이 있으면 검증된 다음 경로가 준비될 때까지 추진을
 멈춥니다. 물리 계산과 센서 수신은 계속합니다. 선택적인 해안 경로 준비에 실패하거나
 결과가 비어 있어도 검증된 해안 경로를 외해 목표로 바꾸지 않습니다.

README.md

@@ -61,20 +61,23 @@ selecting a local depth-checked goal. The detour can initially increase distance
 to the course target. At its checked endpoint, the caller waits for the next
 checked segment. Longitude wrapping supports the international date line.
 
-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.
+Coastal lookahead favours shore only while progressing toward the destination
+port. Its final stretch must approach both the course target and the port.
+The full coastal route may add at most 15% to a shorter checked route to the
+same onward target. Longer scenic detours use the ordinary water route instead.
+Land and shallow water can still require a detour around bays or islands.
+Ports and required passage points retain their arrival checks.
 
 Onward waypoints mark checked segment ends. Straight grid runs are shortened
 when the complete swept corridor passes depth and mapped-land checks. The
-shortened route preserves its exit and coastline preference.
+shortened route preserves its checked exit.
 
 | Policy | Default |
 |---|---|
 | Surface local depth clearance | 1.5 m |
 | Surface hull radius + tracking margin | 3.5 m + 4 m |
 | Coastal preferred distance | 250 m |
+| Maximum extra distance for coastal preference | 15% |
 | Coastal regional buffer / depth | 40 m / 2 m |
 | Arrival radius | 300 m |
 | Arrival target screening | 350 m buffer / 3 m depth |
@@ -114,6 +117,7 @@ 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.
+Pass the final port as `progress_target` when the local target is a course waypoint.
 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

src/posim_routing/coastal.py

@@ -24,7 +24,8 @@
 MAXIMUM_STANDOFF_M = 800.
 MINIMUM_DEPTH_M = 2.
 REGIONAL_BUFFER_M = 40.
-WAYPOINT_FORMAT = "checked-segment-ends-v1"
+WAYPOINT_FORMAT = "checked-segment-ends-v2-destination-progress"
+MAXIMUM_COASTAL_DETOUR_RATIO = 1.15
 
 
 def sphere(coordinates):
@@ -90,7 +91,7 @@ def progressive_shore(self, position, target):
         return float(travel[i]), points[i].tolist()
 
 
-def coastal_approach(start, target, shore):
+def coastal_approach(start, target, shore, progress_target=None):
     """One bounded step towards distant coast, still progressing on course.
 
     Numeric regional and native swept-hull checks must approve this candidate.
@@ -107,17 +108,24 @@ def coastal_approach(start, target, shore):
     east, north = .65*math.sin(b)+.35*math.sin(a), .65*math.cos(b)+.35*math.cos(a)
     angle = math.degrees(math.atan2(east, north))
     candidate = list(GEO.fwd(*start, angle, min(7500., remaining*.5))[:2])
-    if remaining-GEO.inv(*candidate, *target)[2] < 500:
+    travel = GEO.inv(*start, *candidate)[2]
+    # Scenery is optional: a mostly sideways step cannot spend most of its
+    # travel circling an island while making only a small net gain.
+    if remaining-GEO.inv(*candidate, *target)[2] < max(500., travel/MAXIMUM_COASTAL_DETOUR_RATIO):
         return None
+    if progress_target is not None:
+        if GEO.inv(*start, *progress_target)[2]-GEO.inv(*candidate, *progress_target)[2] < travel/MAXIMUM_COASTAL_DETOUR_RATIO:
+            return None
     return candidate
 
 
-def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None):
-    """Return a progressive coastal path, or None to retain the maritime course.
+def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None, progress_target=None):
+    """Return a coastal or shorter checked water path, or retain the course.
 
-    A bounded weighted A* prefers a 250 m stand-off. It cannot invent land or
-    depth in missing cells, cross a headland, or pull an intermediate waypoint
-    backwards. Close to a course waypoint, exact arrival takes precedence.
+    A checked coast preference may add at most 15 percent to the shortest
+    checked route to the same onward goal. Its final stretch must approach
+    both that goal and the destination. Required land detours remain available
+    through the ordinary planner. Close to a waypoint, exact arrival wins.
     """
     from posim_terrain.terrain import projection
     from .navigation import TerrainField, DetourPlanner
@@ -136,6 +144,7 @@ def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None):
         return None
     origin = np.array(forward.transform(*start))
     destination = np.array(forward.transform(*target))
+    progress_destination = np.array(forward.transform(*(progress_target or target)))
     remaining = float(np.linalg.norm(destination-origin))
     if remaining < 2000:
         return None
@@ -209,7 +218,8 @@ def xy(node):
         return None
     expanded = 0
     offsets = ((0, 1), (0, -1), (1, 0), (-1, 0), (1, 1), (1, -1), (-1, 1), (-1, -1))
-    for goal in goals:
+    def search(goal, weights):
+        nonlocal expanded
         costs, parents = {start_node: 0.}, {}
         heap = [(math.dist(xy(start_node), xy(goal)), 0., start_node)]
         found = False
@@ -229,18 +239,86 @@ def xy(node):
                     continue
                 if i and j and (not safe[node[0]+j, node[1]] or not safe[node[0], node[1]+i]):
                     continue
-                edge = math.hypot(i*dx, j*dy)*(penalty[node]+penalty[nxt])/2
+                edge = math.hypot(i*dx, j*dy)*(weights[node]+weights[nxt])/2
                 value = cost+edge
                 if value >= costs.get(nxt, math.inf):
                     continue
                 costs[nxt], parents[nxt] = value, node
                 heapq.heappush(heap, (value+math.dist(xy(nxt), xy(goal)), value, nxt))
         if not found:
-            continue
+            return None, costs
         nodes = [goal]
         while nodes[-1] != start_node:
             nodes.append(parents[nodes[-1]])
         nodes.reverse()
+        return nodes, costs
+
+    def shorten(route, costs, weights, lookahead=None):
+        points, index = [xy(route[0])], 0
+        while index < len(route)-1:
+            chosen = index+1
+            last = min(len(route)-1, index+lookahead) if lookahead else len(route)-1
+            for candidate in range(last, index, -1):
+                if time.monotonic() > deadline:
+                    raise RuntimeError("coastal_planning_budget_exceeded")
+                a, b = xy(route[index]), xy(route[candidate])
+                length = math.dist(a, b)
+                count = max(2, math.ceil(length/min(dx, dy)*3))
+                ix = np.rint(np.linspace(route[index][1], route[candidate][1], count)).astype(int)
+                iy = np.rint(np.linspace(route[index][0], route[candidate][0], count)).astype(int)
+                weighted = length*float(np.mean(weights[iy, ix]))
+                old = costs[route[candidate]]-costs[route[index]]
+                if (safe[iy, ix].all() and weighted <= old*1.08
+                        and validator.segment(a, b)):
+                    chosen = candidate
+                    break
+            points.append(xy(route[chosen]))
+            index = chosen
+        return points
+
+    def route_length(points):
+        return sum(math.dist(a, b) for a, b in zip(points, points[1:]))
+
+    water_fallback = None
+    for goal in goals:
+        try:
+            baseline_nodes, baseline_costs = search(goal, np.ones_like(penalty))
+            baseline = shorten(baseline_nodes, baseline_costs, np.ones_like(penalty)) if baseline_nodes else None
+        except RuntimeError:
+            if water_fallback is not None:
+                return water_fallback
+            raise
+        if baseline_nodes is None:
+            continue
+        baseline[0] = [*origin, 0.]
+        if not all(validator.segment(a, b) for a, b in zip(baseline, baseline[1:])):
+            continue
+        baseline_length = route_length(baseline)
+        if water_fallback is None:
+            # Retain the same checked exit. A separate angular frontier search
+            # can otherwise replace the rejected scenic loop with another loop.
+            segments = [baseline[0]]
+            for a, b in zip(baseline, baseline[1:]):
+                count = max(1, math.ceil(math.dist(a, b)/7500.))
+                segments.extend((np.array(a)+(np.array(b)-np.array(a))*i/count).tolist()
+                                for i in range(1, count+1))
+            if all(validator.segment(a, b, record=True) for a, b in zip(segments, segments[1:])):
+                water_fallback = dict(points=[list(inverse.transform(*p[:2])) for p in segments[1:]],
+                    index=0, mode="detour", planning_seconds=time.monotonic()-started,
+                    search_nodes=expanded, minimum_regional_clearance_m=validator.minimum,
+                    required_depth_m=MINIMUM_DEPTH_M, regional_buffer_m=REGIONAL_BUFFER_M,
+                    shortest_checked_length_m=baseline_length,
+                    coastal_preference_reason="shorter_checked_water_route",
+                    destination_progress_target=list(progress_target or target),
+                    scope="numeric terrain water route; native corridors checked separately")
+        try:
+            nodes, costs = search(goal, penalty)
+        except RuntimeError:
+            if water_fallback is not None:
+                return water_fallback
+            raise
+        if nodes is None:
+            continue
         coastal_nodes = [index for index,node in enumerate(nodes) if coast[node]]
         if not coastal_nodes:
             continue
@@ -258,34 +336,34 @@ def xy(node):
         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.
-        def shorten(route, lookahead=None):
-            points, index = [xy(route[0])], 0
-            while index < len(route)-1:
-                chosen = index+1
-                last = min(len(route)-1, index+lookahead) if lookahead else len(route)-1
-                for candidate in range(last, index, -1):
-                    if time.monotonic() > deadline:
-                        raise RuntimeError("coastal_planning_budget_exceeded")
-                    a, b = xy(route[index]), xy(route[candidate])
-                    length = math.dist(a, b)
-                    count = max(2, math.ceil(length/min(dx, dy)*3))
-                    ix = np.rint(np.linspace(route[index][1], route[candidate][1], count)).astype(int)
-                    iy = np.rint(np.linspace(route[index][0], route[candidate][0], count)).astype(int)
-                    weighted = length*float(np.mean(penalty[iy, ix]))
-                    old = costs[route[candidate]]-costs[route[index]]
-                    if (safe[iy, ix].all() and weighted <= old*1.08
-                            and validator.segment(a, b)):
-                        chosen = candidate
-                        break
-                points.append(xy(route[chosen]))
-                index = chosen
-            return points
-
-        points = shorten(nodes, lookahead=10)
+        try:
+            points = shorten(nodes, costs, penalty, lookahead=10)
+            onward = shorten([endpoint, *onward_nodes], costs, penalty)
+        except RuntimeError:
+            if water_fallback is not None:
+                return water_fallback
+            raise
         points[0] = [*origin, 0.]
-        onward = shorten([endpoint, *onward_nodes])
+        through_length = route_length(points) + route_length(onward)
+        if through_length > baseline_length*MAXIMUM_COASTAL_DETOUR_RATIO:
+            continue
+        # Check the last kilometre, not just a net distance gain. A bay head
+        # can be closer overall while its terminal stretch turns back uphill.
+        tail = points[-1]
+        port_remaining = float(np.linalg.norm(progress_destination-origin))
+        port_progress = port_remaining-float(np.linalg.norm(progress_destination-np.array(tail[:2])))
+        if port_progress < min(1000., port_remaining*.2):
+            continue
+        tail_start = points[-2] if len(points)>1 else points[0]
+        for point in reversed(points[:-1]):
+            tail_start = point
+            if math.dist(point, tail) >= 1000.:
+                break
+        directions = (np.array(tail[:2])-np.array(tail_start[:2]),
+                      np.array(tail[:2])-np.array(points[-2][:2]))
+        if any(float(np.dot(direction, goal_xy-np.array(tail[:2]))) < -1e-6
+               for direction in directions for goal_xy in (destination, progress_destination)):
+            continue
         if sum(math.dist(a, b) for a, b in zip(points, points[1:])) > 40000:
             continue
         if not all(validator.segment(a, b, record=True) for route in (points, onward)
@@ -303,5 +381,10 @@ def shorten(route, lookahead=None):
                     onward_exit_progress_m=float(progress[goal]),
                     onward_verified=True,
                     waypoint_format=WAYPOINT_FORMAT,
+                    shortest_checked_length_m=baseline_length,
+                    coastal_through_length_m=through_length,
+                    maximum_coastal_detour_ratio=MAXIMUM_COASTAL_DETOUR_RATIO,
+                    destination_progress_target=list(progress_target or target),
+                    progress_to_destination_m=port_progress,
                     scope="numeric terrain coastal preference; native corridors checked separately")
-    return None
+    return water_fallback