From 2ede794a5025abef0849d3af5f1927ed4b35c70c Mon Sep 17 00:00:00 2001 From: ioes Date: Fri, 9 Oct 2026 14:54:35 +0900 Subject: [PATCH] Keep coastal routes on checked onward water corridors --- README.ko.md | 9 +++ README.md | 11 ++++ pyproject.toml | 2 +- src/posim_routing/coastal.py | 60 ++++++++++++++------ src/posim_routing/course.py | 15 +++-- src/posim_routing/regional.py | 55 +++++++++++++----- tests/test_coastal_navigation.py | 96 +++++++++++++++++++++++++++++--- tests/test_course.py | 35 ++++++++++++ 8 files changed, 235 insertions(+), 48 deletions(-) create mode 100644 tests/test_course.py diff --git a/README.ko.md b/README.ko.md index 046c06a..34aeacb 100644 --- a/README.ko.md +++ b/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 diff --git a/README.md b/README.md index e643663..91ca44e 100644 --- a/README.md +++ b/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 diff --git a/pyproject.toml b/pyproject.toml index 6d5d7d6..5a0d1e2 100644 --- a/pyproject.toml +++ b/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"} diff --git a/src/posim_routing/coastal.py b/src/posim_routing/coastal.py index 6177d86..644762b 100644 --- a/src/posim_routing/coastal.py +++ b/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 diff --git a/src/posim_routing/course.py b/src/posim_routing/course.py index 5032fa6..9d4a615 100644 --- a/src/posim_routing/course.py +++ b/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]) diff --git a/src/posim_routing/regional.py b/src/posim_routing/regional.py index 6bbb843..ab79c42 100644 --- a/src/posim_routing/regional.py +++ b/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)=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)=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 diff --git a/tests/test_coastal_navigation.py b/tests/test_coastal_navigation.py index 4cdceae..b653f5b 100644 --- a/tests/test_coastal_navigation.py +++ b/tests/test_coastal_navigation.py @@ -77,6 +77,24 @@ def test_headland_cannot_be_shortcut(self): self.assertGreater(points[:, 0].max(), 2000) self.assert_swept_safe(terrain, start, plan) + def test_closed_bay_requires_an_onward_water_exit(self): + terrain = grid() + x,y = np.meshgrid(terrain['xs'],terrain['ys']) + # Bay opens east; its western head is nearer the southwest goal. + land = (x < -2000) | ((y < -1000) & (x < 5000)) | ((y > 4500) & (x < 5000)) + terrain['heights'] = np.where(land,20.,-15.).tolist() + start,target = geo(1000,2000),geo(-20000,-20000) + plan = coastal_corridor(terrain,SPEC,start,target) + self.assertIsNotNone(plan) + self.assertTrue(plan['onward_verified']) + exit_x,exit_y = FORWARD.transform(*plan['onward_exit']) + self.assertGreater(exit_x,5000) + self.assertLess(exit_y,-9000) + end_x,end_y = FORWARD.transform(*plan['points'][-1]) + self.assertGreater(end_x,5000) + self.assertLess(end_y,-1000) + self.assert_swept_safe(terrain,start,dict(points=plan['points']+plan['onward_points'])) + def test_shallow_and_unknown_coast_is_not_a_destination(self): terrain = grid() heights = np.array(terrain['heights']) @@ -154,15 +172,75 @@ def nearest(self,position):return 1000.,geo(0,0) self.assertEqual(navigator.update(sample,target),point) self.assertIs(navigator.task,pending) self.assertEqual(sample['status'],'sailing') - # If slow background preparation outlasts the old coast plan, propose - # its tangent, not the offshore graph point. Goal stays fixed between - # samples, and native collision approval remains required downstream. - sample['position']=geo(800,6100) - continuation=navigator.update(sample,target) - self.assertLess(distance(sample['position'],continuation),751) - self.assertNotEqual(continuation,target) - sample['position']=geo(800,6110) - self.assertEqual(navigator.update(sample,target),continuation) + # Exhaust the checked path during a slow download. No tangent may be + # chained indefinitely, even if it would approach the offshore target. + sample['position']=geo(800,6000) + sample['regional_navigation']['job']='checked' + self.assertEqual(navigator.update(sample,target),point) + self.assertEqual(sample['regional_waiting']['reason'],'coastal_renewal') + sample['position']=geo(800,6010) + self.assertEqual(navigator.update(sample,target),point) + self.assertNotIn('continuation',sample['regional_navigation']) + await navigator.close() + + async def test_recorded_shape_handoff_preserves_checked_coast(self): + async def rpc(*args,**kwargs):await asyncio.Event().wait() + class Near: + def nearest(self,position):return 4000.,[118.73,24.7] + old=[0,2,7];point=[118.7441,24.6863] + sample=dict(lap=0,leg=2,waypoint=8,position=[118.75956451849835,24.67215438376837], + regional_navigation=dict(route_key=old,points=[point],index=0,mode='coastal', + policy=REGIONAL_POLICY,probe_position=[118.760,24.671],job='checked'),native={}) + navigator=RegionalNavigator(rpc,coastal=True);navigator.coast_index=Near();navigator.key=old + self.assertEqual(navigator.update(sample,[118.48391605957087,24.503214246085488],retain_coast=True),point) + self.assertEqual(sample['regional_navigation']['route_key'],[0,2,8]) + self.assertEqual(sample['regional_handoff']['previous_key'],old) + await navigator.close() + + async def test_handoff_requires_ordinary_shape_in_same_leg_and_lap(self): + import copy + async def rpc(*args,**kwargs):await asyncio.Event().wait() + original=dict(lap=0,leg=2,waypoint=8,position=geo(800,0), + regional_navigation=dict(route_key=[0,2,7],points=[geo(800,3000)],index=0,mode='coastal', + policy=REGIONAL_POLICY,job='checked'),native={}) + for overrides,retain,terminal in [({},False,False),({'leg':3},True,False), + ({'lap':1},True,False),({'waypoint':6},True,False),({},True,True)]: + sample=copy.deepcopy(original);sample.update(overrides) + navigator=RegionalNavigator(rpc) + target=geo(6500,9000) if not terminal else geo(1500,0) + self.assertEqual(navigator.update(sample,target,retain_coast=retain,terminal=terminal),target) + self.assertNotIn('regional_handoff',sample) + await navigator.close() + + async def test_failed_and_empty_renewal_hold_until_a_checked_replacement(self): + class Near: + def nearest(self,position):return 1000.,geo(0,0) + async def rpc(*args,**kwargs):await asyncio.Event().wait() + key=[0,0,1];end=geo(800,4000);target=geo(6500,9000) + sample=dict(lap=0,leg=0,waypoint=1,position=end,status='sailing',native={}, + regional_navigation=dict(route_key=key,points=[geo(800,1000),end],index=2,mode='coastal', + policy=REGIONAL_POLICY,probe_position=geo(800,0),job='checked')) + navigator=RegionalNavigator(rpc,coastal=True);navigator.coast_index=Near();navigator.key=key + async def failed():raise RuntimeError('regional_terrain_unavailable') + navigator.task=asyncio.create_task(failed()) + await asyncio.gather(navigator.task,return_exceptions=True) + self.assertEqual(navigator.update(sample,target),end) + self.assertEqual(sample['regional_planning']['status'],'retrying') + self.assertIsNotNone(sample.get('regional_waiting')) + for _ in range(10):self.assertEqual(navigator.update(sample,target),end) + async def empty():return dict(route_key=key,points=[],index=0,mode='course_retained', + policy=REGIONAL_POLICY,probe_position=end,job='empty') + navigator.task=asyncio.create_task(empty());await navigator.task + self.assertEqual(navigator.update(sample,target),end) + self.assertEqual(sample['regional_navigation']['job'],'checked') + self.assertIsNotNone(sample.get('regional_waiting')) + replacement=geo(800,6000) + async def ready():return dict(route_key=key,points=[replacement],index=0,mode='coastal', + policy=REGIONAL_POLICY,probe_position=end,job='new') + navigator.task=asyncio.create_task(ready());await navigator.task + self.assertEqual(navigator.update(sample,target),replacement) + self.assertNotIn('regional_waiting',sample) + self.assertEqual(sample['status'],'sailing') await navigator.close() async def test_coastal_preference_can_replace_an_active_offshore_detour(self): diff --git a/tests/test_course.py b/tests/test_course.py new file mode 100644 index 0000000..64eed69 --- /dev/null +++ b/tests/test_course.py @@ -0,0 +1,35 @@ +"""Measured shape progress must not require offshore visits or skip arrivals.""" +import copy +import unittest +from posim_routing.course import course_target,passed_coastal_shape + +LEG=dict(coordinates=[[119.761963,25.085599],[118.937988,24.542126], + [118.48391605957087,24.503214246085488]],canal=False) + +def state(position): + return dict(lap=0,leg=2,waypoint=1,position=position, + coastal_navigation=dict(status='coastal_region')) + +class CourseTests(unittest.TestCase): + def test_recorded_shenhu_shape_uses_onward_course(self): + self.assertEqual(course_target(LEG,1,state([118.85,24.8])),LEG['coordinates'][2]) + self.assertTrue(passed_coastal_shape(LEG,1,state([118.70,24.65]))) + self.assertFalse(passed_coastal_shape(LEG,1,state([117.5,24.65]))) + + def test_required_passage_and_port_stay_strict(self): + gate=copy.deepcopy(LEG);gate['passage_gates']=[LEG['coordinates'][1]] + self.assertEqual(course_target(gate,1,state([118.85,24.8])),LEG['coordinates'][1]) + self.assertFalse(passed_coastal_shape(gate,1,state([118.70,24.65]))) + canal=copy.deepcopy(LEG);canal['canal']=True + self.assertEqual(course_target(canal,1,state([118.85,24.8])),LEG['coordinates'][1]) + self.assertFalse(passed_coastal_shape(LEG,2,state(LEG['coordinates'][2]))) + self.assertEqual(course_target(LEG,2,state([118.70,24.65])),LEG['coordinates'][2]) + + def test_ocean_distant_shapes_and_hairpins_keep_course(self): + ocean=state([118.85,24.8]);ocean.pop('coastal_navigation') + self.assertEqual(course_target(LEG,1,ocean),LEG['coordinates'][1]) + self.assertEqual(course_target(LEG,1,state([117.5,24.65])),LEG['coordinates'][1]) + hairpin=dict(coordinates=[[118.8,24.5],[118.9,24.5],[118.81,24.5]]) + self.assertEqual(course_target(hairpin,1,state([118.88,24.5])),hairpin['coordinates'][1]) + +if __name__=='__main__':unittest.main()