SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(planning): consolidate recorded-route localization and spatial scene

Browse Source 
Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
This commit is contained in:
DCCONSTRUCTIONS
2026-09-21 08:47:19 +03:00
parent be58d589e2
commit e515ab1b8c
189 changed files with 19074 additions and 758 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/missions/route_relocalization.py
+721
View File
@@ -0,0 +1,721 @@
"""Staged stationary localisation before the normal fresh-data tracking gate.
A known start is the reliable laboratory path, so it first receives a dense
multi-start fit. Only its honest rejection permits retrieval over the entire
selected route. That preserves a repeatable start while retaining an auditable
recovery path for a restarted rover that must look for *where it is*.
Neither stage grants tracking or vehicle authority: both only produce a
provisional hypothesis for the separate, disjoint fresh-data gate.
"""
from __future__ import annotations
import math
import time
from copy import deepcopy
from dataclasses import dataclass
from itertools import product
import numpy as np
from .entry_acquisition import acquire_entry
from .observation_profiles import TRACKING_INPUT
from .reference_window import reference_window
from .registration import POLICY as TRACKING_POLICY
from .registration import PreparedReference, angle_deg, cloud, rigid, transform
from .stationary_entry import STATIONARY_POLICY
ROUTE_RELOCALIZATION_POLICY = dict(
version="route-relocalization/v6",
scope="selected-route",
strategy="dense-start-first-then-route-recovery/v1",
# Local geometry is independent of the 80-m presentation envelope.
query_radius_m=TRACKING_INPUT["radius_m"],
anchor_spacing_m=5.0,
spatial_cell_m=10.0,
# Candidate retrieval stays local and distinctive. The chosen candidate is
# then matched against the high-resolution local tracking footprint.
descriptor_context_m=28.0,
descriptor_radial_bins=7,
descriptor_height_bins=6,
descriptor_height_low_m=-4.0,
descriptor_height_high_m=8.0,
polar_angle_bins=24,
# A batch controls scheduling, never eligibility. Ranking must not discard
# the real place merely because a coarse descriptor prefers an endpoint.
candidate_batch_size=6,
yaw_candidates_per_place=3,
yaw_step_deg=30.0,
target_context_margin_m=12.0,
target_maximum_points=None,
descriptor_voxel_m=0.5,
cluster_position_m=0.75,
cluster_rotation_deg=8.0,
ambiguity_overlap_margin=0.05,
ambiguity_rmse_margin_m=0.03,
# Keep the stationary prefix younger than the bootstrap's 40-s source-age
# fence. A late exhaustive calculation is an explicit incomplete search,
# never a stale provisional position.
deadline_s=30.0,
maximum_search_wall_s=35.0,
# This is a numerical convergence envelope, not an operator start-radius
# admission rule. Reaching its wall deadline is reported as incomplete.
registration_policy={
**TRACKING_POLICY,
"version": "route-relocalization-gicp/v1",
"maximum_correction_m": 25.0,
"maximum_correction_deg": 180.0,
},
)
def _valid_path(path):
path = np.asarray(path, dtype=float)
if path.ndim != 2 or path.shape[1] != 3 or len(path) < 2 or not np.isfinite(path).all():
raise ValueError("Для поиска по маршруту нужен конечный маршрут минимум из двух точек.")
lengths = np.linalg.norm(np.diff(path, axis=0), axis=1)
if not np.isfinite(lengths).all() or float(lengths.sum()) <= 0:
raise ValueError("Маршрут не содержит достаточной геометрии для поиска.")
return path, lengths
def route_reference_cloud(value):
"""Validate the complete route atlas without applying GICP's target cap.
A selected kilometre route is not one target: it is indexed here and only a
local, separately checked target is handed to GICP later.
"""
points = np.ascontiguousarray(value, dtype=np.float64)
if (
points.ndim != 2
or points.shape[1] != 3
or len(points) < 300
or not np.isfinite(points).all()
or np.abs(points).max() > 100_000
):
raise ValueError("Полная карта маршрута содержит недостаточно конечных точек в метрах.")
return points
def route_anchors(path, *, spacing_m=ROUTE_RELOCALIZATION_POLICY["anchor_spacing_m"]):
"""Resample the complete path; no endpoint or intermediate segment is skipped."""
if not 0 < spacing_m <= 25:
raise ValueError("Некорректный шаг индекса маршрута.")
path, lengths = _valid_path(path)
cumulative = np.r_[0.0, np.cumsum(lengths)]
distances = np.r_[np.arange(0.0, cumulative[-1], spacing_m), cumulative[-1]]
positions = []
for distance in distances:
segment = min(
int(np.searchsorted(cumulative, distance, side="right") - 1), len(lengths) - 1
)
fraction = (distance - cumulative[segment]) / lengths[segment]
positions.append(path[segment] + fraction * (path[segment + 1] - path[segment]))
return np.asarray(positions), distances
def _voxel(points, *, voxel_m):
if len(points) == 0:
return points
_, index = np.unique(np.floor(points / voxel_m).astype(np.int64), axis=0, return_index=True)
return points[np.sort(index)]
class ReferenceGrid:
"""Read-only spatial index for a full route map.
It prevents every atlas anchor from scanning every point in a kilometre
route. The grid is local to one isolated search process and is never
reused as a mutable tracking map.
"""
def __init__(self, reference, *, cell_m=ROUTE_RELOCALIZATION_POLICY["spatial_cell_m"]):
if not 1.0 <= cell_m <= 25.0:
raise ValueError("Некорректный размер ячейки карты маршрута.")
self.reference = route_reference_cloud(reference)
self.cell_m = float(cell_m)
cells = np.floor(self.reference / self.cell_m).astype(np.int64)
keys, inverse = np.unique(cells, axis=0, return_inverse=True)
order = np.argsort(inverse, kind="stable")
counts = np.bincount(inverse, minlength=len(keys))
boundaries = np.r_[0, np.cumsum(counts)]
self.ordered = self.reference[order]
self.slices = {
tuple(key): (int(boundaries[index]), int(boundaries[index + 1]))
for index, key in enumerate(keys)
}
def crop(self, center, radius_m):
center = np.asarray(center, dtype=float).reshape(3)
if not np.isfinite(center).all() or not 0 < radius_m <= 100:
raise ValueError("Некорректная локальная область маршрута.")
lower = np.floor((center - radius_m) / self.cell_m).astype(int)
upper = np.floor((center + radius_m) / self.cell_m).astype(int)
pieces = []
ranges = tuple(range(first, last + 1) for first, last in zip(lower, upper, strict=True))
for key in product(*ranges):
bounds = self.slices.get(key)
if bounds is not None:
pieces.append(self.ordered[slice(*bounds)])
if not pieces:
return np.empty((0, 3), dtype=float)
points = np.concatenate(pieces)
return points[np.linalg.norm(points - center, axis=1) <= radius_m]
def local_submap(reference, center, radius_m, *, maximum_points):
"""Radial crop. Production verification preserves the source resolution.
An explicit point budget is available only to descriptor/test callers.
It is never a density threshold for declaring tracking lost.
"""
if isinstance(reference, ReferenceGrid):
points = reference.crop(center, radius_m)
else:
full = route_reference_cloud(reference)
points = full[np.linalg.norm(full - center, axis=1) <= radius_m]
if maximum_points is not None and len(points) > maximum_points:
original = points
voxel_m = ROUTE_RELOCALIZATION_POLICY["descriptor_voxel_m"]
while len(points) > maximum_points:
reduced = _voxel(original, voxel_m=voxel_m)
if voxel_m > radius_m * 2.0:
return np.empty((0, 3), dtype=float)
points = reduced
voxel_m *= 2.0
if len(points) < 300:
return np.empty((0, 3), dtype=float)
return points
def radial_height_descriptor(points, center, *, policy=ROUTE_RELOCALIZATION_POLICY):
relative = np.asarray(points, dtype=float) - np.asarray(center, dtype=float)
radial = np.linalg.norm(relative[:, :2], axis=1)
histogram, _ = np.histogramdd(
np.column_stack([radial, relative[:, 2]]),
bins=(
policy["descriptor_radial_bins"],
policy["descriptor_height_bins"],
),
range=(
(0.0, policy["descriptor_context_m"]),
(policy["descriptor_height_low_m"], policy["descriptor_height_high_m"]),
),
)
flat = histogram.reshape(-1)
norm = float(np.linalg.norm(flat))
return flat / norm if norm else flat
def polar_descriptor(
points,
center,
*,
bins=ROUTE_RELOCALIZATION_POLICY["polar_angle_bins"],
context_m=ROUTE_RELOCALIZATION_POLICY["descriptor_context_m"],
):
relative = np.asarray(points, dtype=float) - np.asarray(center, dtype=float)
angle = np.mod(np.arctan2(relative[:, 1], relative[:, 0]), 2 * math.pi)
radial = np.linalg.norm(relative[:, :2], axis=1)
# Four equally sized radial rings prevent one distant, unrelated wall
# from deciding yaw while retaining the full declared context.
rings = np.minimum((radial / (context_m / 4.0)).astype(int), 3)
output = np.zeros((4, bins), dtype=float)
angles = np.minimum((angle / (2 * math.pi) * bins).astype(int), bins - 1)
np.add.at(output, (rings, angles), 1.0)
norm = float(np.linalg.norm(output))
return output / norm if norm else output
def _yaw_candidates(query, target, query_center, target_center, *, policy):
q = polar_descriptor(
query,
query_center,
bins=policy["polar_angle_bins"],
context_m=policy["descriptor_context_m"],
)
t = polar_descriptor(
target,
target_center,
bins=policy["polar_angle_bins"],
context_m=policy["descriptor_context_m"],
)
candidates = []
for yaw in np.arange(0.0, 360.0, policy["yaw_step_deg"]):
shift = int(round(yaw / 360.0 * policy["polar_angle_bins"]))
candidates.append((float(np.linalg.norm(t - np.roll(q, shift, axis=1))), float(yaw)))
return [yaw for _, yaw in sorted(candidates)[: policy["yaw_candidates_per_place"]]]
@dataclass(frozen=True)
class RouteCandidate:
index: int
position: np.ndarray
progress_m: float
descriptor_distance: float
def rank_route_candidates(
reference, reference_path, query, *, policy=ROUTE_RELOCALIZATION_POLICY, grid=None
):
"""Rank every resampled route position against the stationary query cloud."""
reference, query = route_reference_cloud(reference), cloud(query)
grid = grid or ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
anchors, progress = route_anchors(reference_path, spacing_m=policy["anchor_spacing_m"])
query_center = np.median(query, axis=0)
query_descriptor = radial_height_descriptor(query, query_center, policy=policy)
ranked = []
for index, (position, distance) in enumerate(zip(anchors, progress, strict=True)):
target = local_submap(
grid,
position,
policy["descriptor_context_m"],
maximum_points=policy["target_maximum_points"],
)
if len(target) < 300:
continue
descriptor = radial_height_descriptor(target, np.median(target, axis=0), policy=policy)
ranked.append(
RouteCandidate(
index=index,
position=position,
progress_m=float(distance),
descriptor_distance=float(np.linalg.norm(query_descriptor - descriptor)),
)
)
ranked.sort(key=lambda candidate: (candidate.descriptor_distance, candidate.index))
return ranked, dict(
route_anchor_count=len(anchors),
descriptor_covered_anchor_count=len(ranked),
descriptor_candidate_count=len(ranked),
descriptor_scope="entire-selected-route",
)
def _seed(query_center, target_center, yaw_deg):
angle = math.radians(yaw_deg)
rotation = np.array(
[
[math.cos(angle), -math.sin(angle), 0.0],
[math.sin(angle), math.cos(angle), 0.0],
[0, 0, 1],
],
dtype=float,
)
matrix = np.eye(4)
matrix[:3, :3] = rotation
matrix[:3, 3] = np.asarray(target_center) - rotation @ np.asarray(query_center)
return matrix
def _rejected_attempt(message, initial):
return dict(
status="rejected",
reasons=[message],
T_reference_query=rigid(initial).tolist(),
initial_T_reference_query=rigid(initial).tolist(),
overlap=0.0,
inlier_rmse_m=None,
matched_query_indices=[],
localization_confirmed=False,
vehicle_control=False,
registration_seconds=0.0,
)
def _distance(first, second, query_entry):
a, b = np.asarray(first), np.asarray(second)
position = float(
np.linalg.norm(
transform(np.asarray(query_entry).reshape(1, 3), a)
- transform(np.asarray(query_entry).reshape(1, 3), b)
)
)
return position, angle_deg(a[:3, :3] @ b[:3, :3].T)
def choose_route_location(attempts, query_entry, *, complete, policy=ROUTE_RELOCALIZATION_POLICY):
"""Accept one well-separated route location, or expose why we did not."""
candidates, diagnostics = [], []
for attempt in attempts:
result = attempt["result"]
diagnostic = {k: v for k, v in attempt.items() if k != "result"}
diagnostic["result"] = {k: v for k, v in result.items() if k != "matched_query_indices"}
diagnostics.append(diagnostic)
if result["status"] == "candidate":
candidates.append(attempt)
candidates.sort(
key=lambda attempt: (
-attempt["result"]["overlap"],
attempt["result"]["inlier_rmse_m"],
attempt["candidate"]["index"],
attempt["yaw_deg"],
)
)
clusters = []
for attempt in candidates:
for cluster in clusters:
if all(
_distance(
attempt["result"]["T_reference_query"],
other["result"]["T_reference_query"],
query_entry,
)[0]
<= policy["cluster_position_m"]
and _distance(
attempt["result"]["T_reference_query"],
other["result"]["T_reference_query"],
query_entry,
)[1]
<= policy["cluster_rotation_deg"]
for other in cluster
):
cluster.append(attempt)
break
else:
clusters.append([attempt])
# Keep the remaining distinct hypotheses for disjoint fresh confirmation.
# Their ambiguity is evaluated again relative to the remaining queue, not
# inherited from the best hypothesis after it has been rejected.
queue = []
for index, cluster in enumerate(clusters):
best = cluster[0]
ambiguous = any(
alternative[0]["result"]["overlap"]
>= best["result"]["overlap"] - policy["ambiguity_overlap_margin"]
and alternative[0]["result"]["inlier_rmse_m"]
<= best["result"]["inlier_rmse_m"] + policy["ambiguity_rmse_margin_m"]
for alternative in clusters[index + 1 :]
)
queue.append(dict(
candidate_index=best["candidate"]["index"],
route_progress_m=best["candidate"]["progress_m"],
T_reference_query=best["result"]["T_reference_query"],
overlap=best["result"]["overlap"],
inlier_rmse_m=best["result"]["inlier_rmse_m"],
ambiguous=ambiguous,
))
reason = None
if not complete:
reason = "incomplete-route-search"
elif not clusters:
reason = "no-route-location"
elif queue[0]["ambiguous"]:
# Distinctness comes from fitted SE(3), not the retrieval anchor: two
# seeds at one anchor can converge to different places or directions.
reason = "ambiguous-route-location"
selected = (
dict(clusters[0][0]["result"])
if clusters
else _rejected_attempt(
"Ни один кандидат маршрута не прошёл геометрическую проверку.", np.eye(4)
)
)
selected.update(
status="rejected" if reason else "candidate",
reasons=[reason] if reason else [],
matched_query_indices=[] if reason else selected.get("matched_query_indices", []),
localization_confirmed=False,
vehicle_control=False,
)
selected["initialization"] = dict(
policy=policy,
scope=policy["scope"],
complete=complete,
reason=reason,
expected_attempts=len(attempts),
attempts=diagnostics,
candidate_queue=queue if complete else [],
selected_candidate_index=clusters[0][0]["candidate"]["index"] if clusters else None,
selected_route_progress_m=(clusters[0][0]["candidate"]["progress_m"] if clusters else None),
clusters=[
dict(
candidate_indices=sorted({item["candidate"]["index"] for item in cluster}),
route_progress_m=cluster[0]["candidate"]["progress_m"],
support=len(cluster),
overlap=cluster[0]["result"]["overlap"],
rmse_m=cluster[0]["result"]["inlier_rmse_m"],
)
for cluster in clusters
],
)
selected["registration_seconds"] = sum(
item["result"].get("registration_seconds", 0.0) for item in attempts
)
return selected
def relocalize_route(
reference,
reference_path,
query,
query_entry,
*,
clock=time.monotonic,
policy=ROUTE_RELOCALIZATION_POLICY,
):
"""Run complete candidate retrieval and qualification against a selected route."""
started = clock()
reference, query = route_reference_cloud(reference), cloud(query)
query_entry = np.asarray(query_entry, dtype=float).reshape(3)
grid = ReferenceGrid(reference, cell_m=policy["spatial_cell_m"])
ranked, coverage = rank_route_candidates(
reference, reference_path, query, policy=policy, grid=grid
)
attempts, evaluated, batches = [], [], []
query_center = np.median(query, axis=0)
radius = max(
policy["descriptor_context_m"],
float(np.linalg.norm(query - query_center, axis=1).max())
+ policy["target_context_margin_m"],
)
expected = len(ranked) * policy["yaw_candidates_per_place"]
batch_size = policy["candidate_batch_size"]
for candidate in ranked:
if clock() - started > policy["deadline_s"]:
break
if len(evaluated) % batch_size == 0:
batches.append([])
target = local_submap(
grid, candidate.position, radius, maximum_points=policy["target_maximum_points"]
)
if len(target) < 300:
# Descriptor-admitted geometry unexpectedly disappeared. Do not
# call this a complete negative search or silently skip the place.
break
target_center = np.median(target, axis=0)
count_before = len(attempts)
prepared = None
for yaw_deg in _yaw_candidates(
query, target, query_center, target_center, policy=policy
):
if clock() - started > policy["deadline_s"]:
break
initial = _seed(query_center, target_center, yaw_deg)
try:
# Target preprocessing is independent of yaw. Keep one tree
# per place; all seeds and all eligibility checks stay intact.
if prepared is None:
prepared = PreparedReference(target)
result = prepared.register(
query, initial, policy=policy["registration_policy"]
)
except ValueError as exc:
result = _rejected_attempt(str(exc), initial)
attempts.append(
dict(
candidate=dict(
index=candidate.index,
position=candidate.position.tolist(),
progress_m=candidate.progress_m,
descriptor_distance=candidate.descriptor_distance,
),
yaw_deg=yaw_deg,
result=result,
)
)
if len(attempts) - count_before != policy["yaw_candidates_per_place"]:
break
evaluated.append(candidate.index)
batches[-1].append(candidate.index)
complete = len(evaluated) == len(ranked) and clock() - started <= policy["deadline_s"]
result = choose_route_location(attempts, query_entry, complete=complete, policy=policy)
result["initialization"].update(
coverage,
elapsed_s=clock() - started,
expected_attempts=expected,
evaluated_candidate_indices=evaluated,
remaining_candidate_indices=[c.index for c in ranked if c.index not in evaluated],
candidate_batches=batches,
candidate_queue_exhausted=complete,
)
return result
def _route_start_context(reference, reference_path, query, query_entry, reference_position=None):
"""Prepare the established dense start target without shrinking the scene.
The selected route's first point is still a valuable, explicitly chosen
laboratory datum. After loss, the last confirmed place takes its role.
This target preserves the precise local map representation used
by the successful start-area runs instead of voxelising a broad whole-route
crop before GICP has a chance to converge.
"""
reference, query = route_reference_cloud(reference), cloud(query)
path, _lengths = _valid_path(reference_path)
entry = np.asarray(query_entry, dtype=float).reshape(3)
initial = np.eye(4)
anchor = path[0] if reference_position is None else np.asarray(reference_position, dtype=float)
if anchor.shape != (3,) or not np.isfinite(anchor).all():
raise ValueError("Некорректная область восстановления привязки.")
initial[:3, 3] = anchor - entry
forward = next(
(point - path[0] for point in path[1:] if np.linalg.norm((point - path[0])[:2]) >= 3),
None,
)
if forward is None:
raise ValueError("Reference lacks a usable route basis.")
target, window = reference_window(
reference,
dict(points=query, path=np.asarray([entry])),
initial,
initializing=True,
)
return target, query, initial, entry, forward, window
def _stage_attempts(stage, initialization):
"""Keep every fit auditable while retaining its stage of the hybrid search."""
return [dict(stage=stage, **attempt) for attempt in initialization.get("attempts", [])]
def _hybrid_initialization(policy, start_result, route_result=None):
"""Normalize two numerical stages for StationaryBootstrap's strict gate."""
start = start_result["initialization"]
attempts = _stage_attempts("dense-start", start)
expected = start.get("expected_attempts", len(attempts))
stages = [
dict(
name="dense-start",
status=start_result["status"],
reason=start.get("reason"),
complete=start.get("complete", False),
elapsed_s=start.get("elapsed_s"),
expected_attempts=expected,
target_window=start.get("target_window"),
reference_position=start.get("reference_position"),
)
]
selected = dict(
selected_candidate_index=0 if start_result["status"] == "candidate" else None,
selected_route_progress_m=start.get("route_progress_m", 0.0)
if start_result["status"] == "candidate"
else None,
)
reason = start.get("reason")
complete = bool(start.get("complete"))
if route_result is not None:
route = route_result["initialization"]
attempts.extend(_stage_attempts("route-recovery", route))
expected += route.get("expected_attempts", len(route.get("attempts", [])))
stages.append(
dict(
name="route-recovery",
status=route_result["status"],
reason=route.get("reason"),
complete=route.get("complete", False),
elapsed_s=route.get("elapsed_s"),
expected_attempts=len(route.get("attempts", [])),
descriptor_scope=route.get("descriptor_scope"),
expected_attempts_total=route.get("expected_attempts"),
evaluated_candidate_indices=route.get("evaluated_candidate_indices"),
remaining_candidate_indices=route.get("remaining_candidate_indices"),
candidate_batches=route.get("candidate_batches"),
)
)
selected = dict(
selected_candidate_index=route.get("selected_candidate_index"),
selected_route_progress_m=route.get("selected_route_progress_m"),
candidate_queue=route.get("candidate_queue", []),
)
reason = route.get("reason")
complete = bool(route.get("complete"))
return dict(
policy=policy,
scope=policy["scope"],
strategy=policy["strategy"],
complete=complete,
reason=reason,
expected_attempts=expected,
attempts=attempts,
stages=stages,
**selected,
)
def relocalize_start_then_route(
reference,
reference_path,
query,
query_entry,
*,
clock=time.monotonic,
policy=ROUTE_RELOCALIZATION_POLICY,
reference_position=None,
route_only=False,
):
"""Use the proven start-area fit first, then a bounded route fallback.
This is deliberately not a looser acceptance rule. The dense start fit
runs every stationary multi-start seed against its high-resolution local
target. Only an honest rejection enters whole-route retrieval, whose
result remains provisional until the existing fresh-data gate confirms it.
"""
started = clock()
if route_only:
# A dense-start prior failed fresh confirmation. Recollect first, then
# search the route without repeatedly retrying that unconfirmed start.
return relocalize_route(reference, reference_path, query, query_entry,
clock=clock, policy=policy)
target, query, initial, entry, forward, window = _route_start_context(
reference, reference_path, query, query_entry, reference_position
)
start_result = acquire_entry(
target,
query,
initial,
entry,
forward,
clock=clock,
policy=STATIONARY_POLICY,
)
start_result["initialization"].update(
scope=policy["scope"],
target_window=window,
query_radius_m=policy["query_radius_m"],
reference_position=(np.asarray(query_entry) + initial[:3, 3]).tolist(),
route_progress_m=float(
np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(reference_path, axis=0), axis=1))][
np.argmin(
np.linalg.norm(
np.asarray(reference_path) - (np.asarray(query_entry) + initial[:3, 3]),
axis=1,
)
)
]
),
)
if start_result["status"] == "candidate":
start_result["initialization"] = _hybrid_initialization(policy, start_result)
return start_result
if not start_result["initialization"].get("complete"):
# Compute exhaustion is not evidence that this place did not match.
start_result["initialization"] = _hybrid_initialization(policy, start_result)
return start_result
# A failed standard start may still be a valid mid-route or recovery
# position. Give retrieval only the fresh-prefix time remaining: it must
# never turn a late calculation into an apparently usable prior.
remaining = policy["maximum_search_wall_s"] - (clock() - started)
if remaining <= 0:
route_result = choose_route_location([], entry, complete=False, policy=policy)
route_result["initialization"].update(
elapsed_s=0.0, worker_timeout_reason="start-stage-timeout"
)
else:
recovery_policy = deepcopy(policy)
recovery_policy["deadline_s"] = min(policy["deadline_s"], remaining)
route_result = relocalize_route(
reference,
reference_path,
query,
entry,
clock=clock,
policy=recovery_policy,
)
route_result["initialization"] = _hybrid_initialization(policy, start_result, route_result)
route_result["registration_seconds"] = start_result.get(
"registration_seconds", 0.0
) + route_result.get("registration_seconds", 0.0)
return route_result