From 6b6ebc3488ecc5c5273707071511441b8dec17f8 Mon Sep 17 00:00:00 2001 From: ioes Date: Fri, 9 Oct 2026 22:30:36 +0900 Subject: [PATCH] Bound coastal preference by checked distance and destination progress --- README.ko.md | 16 +-- README.md | 14 ++- src/posim_routing/coastal.py | 161 +++++++++++++++++++++++-------- src/posim_routing/regional.py | 20 ++-- tests/test_coastal_navigation.py | 83 ++++++++++++---- tests/test_land_exclusion.py | 17 ++-- 6 files changed, 221 insertions(+), 90 deletions(-) diff --git a/README.ko.md b/README.ko.md index a3c8676..a725a3e 100644 --- a/README.ko.md +++ b/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']`이 있으면 검증된 다음 경로가 준비될 때까지 추진을 멈춥니다. 물리 계산과 센서 수신은 계속합니다. 선택적인 해안 경로 준비에 실패하거나 결과가 비어 있어도 검증된 해안 경로를 외해 목표로 바꾸지 않습니다. diff --git a/README.md b/README.md index 47a194b..c60f81b 100644 --- a/README.md +++ b/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 diff --git a/src/posim_routing/coastal.py b/src/posim_routing/coastal.py index e9b449d..4e136be 100644 --- a/src/posim_routing/coastal.py +++ b/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 diff --git a/src/posim_routing/regional.py b/src/posim_routing/regional.py index c20f1ee..db852f7 100644 --- a/src/posim_routing/regional.py +++ b/src/posim_routing/regional.py @@ -6,7 +6,7 @@ import asyncio,math,time from pyproj import Geod GEO=Geod(ellps='WGS84') -REGIONAL_POLICY='buffered-water-v8-mapped-land-exclusion' +REGIONAL_POLICY='buffered-water-v9-destination-progress' REGIONAL_BUFFER_M=40. REGIONAL_DEPTH_M=2. @@ -109,7 +109,7 @@ 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,retain_coast=False): + def update(self,state,target,needed=False,terminal=False,retain_coast=False,progress_target=None): from posim_terrain.canal_override import in_region canal=in_region(state['position']) key=[state['lap'],state['leg'],state['waypoint']] @@ -213,7 +213,7 @@ def update(self,state,target,needed=False,terminal=False,retain_coast=False): coast_needed=nearest is not None and (self.probed is None or distance(self.probed,state['position'])>=3000) if (prefer_port or needed or coast_needed or renew_coast) and not self.task and time.monotonic()>=self.retry: 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)) + self.task=asyncio.create_task(self.prepare_port(state,target,key) if terminal else self.prepare(state,target,key,nearest,needed,progress_target=progress_target)) if point is not None:return point plan=state.get('regional_navigation') or {} if (plan.get('route_key')==key and plan.get('policy')==REGIONAL_POLICY @@ -279,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,extent=None): + async def prepare(self,state,target,key,nearest=None,needed=True,extent=None,progress_target=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: @@ -308,7 +308,7 @@ async def prepare(self,state,target,key,nearest=None,needed=True,extent=None): from .land import guard_from_spec guard=await asyncio.to_thread(guard_from_spec,job['spec'],64000.) current=list(state['position']) - course_target=target;guidance=None + course_target=target;progress_target=progress_target or target;guidance=None if guard is not None: from posim_terrain.terrain import projection from .land_guidance import detour,local_target @@ -328,21 +328,24 @@ async def prepare(self,state,target,key,nearest=None,needed=True,extent=None): value=None;coast_error=None if not canal and (nearest or needed): from .coastal import coastal_corridor - try:value=await asyncio.to_thread(coastal_corridor,grid,job['spec'],current,target,guard=guard) + try:value=await asyncio.to_thread(coastal_corridor,grid,job['spec'],current,target,guard=guard,progress_target=progress_target) except RuntimeError as error:coast_error=str(error)[:160] if value is None: current=list(state['position']) approach=None if nearest: from .coastal import coastal_approach - approach=coastal_approach(current,target,nearest[1]) + approach=coastal_approach(current,target,nearest[1],progress_target=progress_target) if approach: try: # This small step is checked against real numeric depth, # then checked again against the live Gazebo collision mesh. value=await asyncio.to_thread(corridor,grid,job['spec'],current,approach,guard=guard) endpoint=value['points'][-1] - if (distance(current,target)-distance(endpoint,target)<500 or + from .coastal import MAXIMUM_COASTAL_DETOUR_RATIO + path_length=sum(distance(a,b) for a,b in zip([current,*value['points'][:-1]],value['points'])) + if (distance(current,progress_target)-distance(endpoint,progress_target)