5041 lines
236 KiB
Python
5041 lines
236 KiB
Python
# SLAM_worker.py
|
||
from __future__ import annotations
|
||
|
||
import json # used by the mid360_config.json IP-rewrite block at connect time
|
||
import logging
|
||
import math # used by the autonomous-wander goal sampler
|
||
import time
|
||
import queue
|
||
import traceback
|
||
import threading
|
||
import multiprocessing as mp
|
||
from collections import deque
|
||
from pathlib import Path
|
||
from typing import Optional, Any, Dict, List, Tuple
|
||
|
||
import numpy as np
|
||
|
||
# Module-level scipy import — six call sites previously did a per-call
|
||
# `from scipy.spatial import cKDTree`, which costs a `sys.modules` walk +
|
||
# attribute lookup on every ICP/registration tick. One module-level import
|
||
# eliminates that overhead and makes the dependency explicit. Falls back to
|
||
# `None` so a missing scipy is detected by the runtime checks in the worker
|
||
# (the previous lazy form silently raised inside the loop).
|
||
try:
|
||
from scipy.spatial import cKDTree as _scipy_cKDTree # type: ignore
|
||
except Exception: # pragma: no cover — scipy install variance on Jetson
|
||
_scipy_cKDTree = None # type: ignore[assignment]
|
||
|
||
from SLAM_engine import (
|
||
EngineConfig,
|
||
FilterConfig,
|
||
MapConfig,
|
||
LocalizationConfig,
|
||
RuntimeConfig,
|
||
load_slam_config,
|
||
)
|
||
from SLAM_Validation import run_startup_self_check
|
||
|
||
_log = logging.getLogger("SLAM_worker")
|
||
|
||
|
||
def _now() -> float:
|
||
return time.time()
|
||
|
||
|
||
def _as_bool(v: Any, default: bool = False) -> bool:
|
||
if v is None:
|
||
return default
|
||
if isinstance(v, bool):
|
||
return v
|
||
if isinstance(v, (int, float)):
|
||
return bool(v)
|
||
return str(v).strip().lower() in ("1", "true", "yes", "on")
|
||
|
||
|
||
def _drain_keep_latest(q: mp.Queue) -> None:
|
||
try:
|
||
while True:
|
||
q.get_nowait()
|
||
except Exception:
|
||
return
|
||
|
||
|
||
def _safe_put(q: mp.Queue, item: Any, keep_latest: bool = False) -> None:
|
||
try:
|
||
if keep_latest:
|
||
_drain_keep_latest(q)
|
||
q.put_nowait(item)
|
||
except Exception:
|
||
pass
|
||
|
||
|
||
def slam_worker(
|
||
data_q: mp.Queue,
|
||
status_q: mp.Queue,
|
||
cmd_q: mp.Queue,
|
||
eng_cfg: EngineConfig,
|
||
filt_cfg: FilterConfig,
|
||
map_cfg: MapConfig,
|
||
loc_cfg: LocalizationConfig,
|
||
run_cfg: RuntimeConfig,
|
||
):
|
||
# ─────────────────────────────────────────────────────────────────────
|
||
# Redirect this subprocess's stdout AND stderr through a FILTER PIPE
|
||
# to logs/lidar_sdk.log.
|
||
#
|
||
# The Livox C++ SDK is extremely chatty — it spdlog-prints
|
||
# `[console] [info]` lines like "Handle detection data" and
|
||
# "Receive Command" at ~2-4 Hz per LiDAR. A previous version of this
|
||
# block did a plain dup2 to the log file, which produced 1.3 GB
|
||
# files in a few hours and saturated the Jetson's eMMC. That disk
|
||
# I/O storm eventually starved the asyncio event loop too.
|
||
#
|
||
# The fix: pipe stdout/stderr through a small reader thread that
|
||
# filters out the heartbeat info-level lines BEFORE they hit disk.
|
||
# Errors and warnings still pass through verbatim. We also rotate
|
||
# the file when it crosses _LIDAR_SDK_MAX_BYTES so an extended
|
||
# error storm can't fill the disk either.
|
||
# ─────────────────────────────────────────────────────────────────────
|
||
import os as _os, sys as _sys, threading as _threading
|
||
_LIDAR_SDK_MAX_BYTES = 50 * 1024 * 1024 # 50 MiB cap
|
||
_SKIP_PHRASES = (
|
||
b"console] [info]",
|
||
b"console] [debug]",
|
||
)
|
||
# If a line matches one of the skip prefixes AND contains one of these,
|
||
# it's heartbeat traffic — drop it. We're permissive: any info line
|
||
# that mentions a serial number / handle is heartbeat noise.
|
||
_SKIP_NEEDLES = (
|
||
b"Handle detection data",
|
||
b"Receive Command:",
|
||
b"Receive Ack:",
|
||
b"Update lidar:", # also info-level on success
|
||
b"Query Fw type succ",
|
||
)
|
||
try:
|
||
_log_dir = _os.path.join(
|
||
_os.path.dirname(_os.path.dirname(_os.path.abspath(__file__))),
|
||
"logs",
|
||
)
|
||
_os.makedirs(_log_dir, exist_ok=True)
|
||
_err_path = _os.path.join(_log_dir, "lidar_sdk.log")
|
||
|
||
_r_fd, _w_fd = _os.pipe()
|
||
_os.dup2(_w_fd, 1)
|
||
_os.dup2(_w_fd, 2)
|
||
_os.close(_w_fd)
|
||
_sys.stdout = _os.fdopen(1, "w", buffering=1)
|
||
_sys.stderr = _os.fdopen(2, "w", buffering=1)
|
||
|
||
def _sdk_log_reader(read_fd, log_path):
|
||
"""Pump SDK output through a filter and write to log_path.
|
||
Drops heartbeat info-level lines; keeps errors / warnings /
|
||
init messages. Rotates when file passes the cap."""
|
||
try:
|
||
rdr = _os.fdopen(read_fd, "rb", 0)
|
||
except Exception:
|
||
return
|
||
with rdr:
|
||
while True:
|
||
try:
|
||
line = rdr.readline()
|
||
except Exception:
|
||
break
|
||
if not line:
|
||
break
|
||
# Filter: drop heartbeat info lines.
|
||
is_skip = False
|
||
for skip_prefix in _SKIP_PHRASES:
|
||
if skip_prefix in line:
|
||
for needle in _SKIP_NEEDLES:
|
||
if needle in line:
|
||
is_skip = True
|
||
break
|
||
break
|
||
if is_skip:
|
||
continue
|
||
# Rotate if the file has grown past the cap.
|
||
try:
|
||
if _os.path.getsize(log_path) > _LIDAR_SDK_MAX_BYTES:
|
||
backup = log_path + ".1"
|
||
try:
|
||
if _os.path.exists(backup):
|
||
_os.remove(backup)
|
||
_os.rename(log_path, backup)
|
||
except Exception:
|
||
pass
|
||
except OSError:
|
||
pass
|
||
try:
|
||
with open(log_path, "ab") as f:
|
||
f.write(line)
|
||
except Exception:
|
||
pass
|
||
|
||
_threading.Thread(
|
||
target=_sdk_log_reader,
|
||
args=(_r_fd, _err_path),
|
||
daemon=True,
|
||
name="sdk-log-filter",
|
||
).start()
|
||
except Exception:
|
||
pass # never crash just because the log redirect failed
|
||
|
||
# suppress noisy warning from ctypes->numpy conversion
|
||
import warnings
|
||
warnings.filterwarnings(
|
||
"ignore",
|
||
message="A builtin ctypes object gave a PEP3118 format string",
|
||
category=RuntimeWarning,
|
||
)
|
||
|
||
try:
|
||
from livox2_python import Livox2
|
||
from kiss_icp.config import load_config
|
||
from kiss_icp.pipeline import KissICP
|
||
from SLAM_Diagnostics import WorkerDiagnostics
|
||
from SLAM_LocalizationService import LocalizationFrameState
|
||
from SLAM_Transforms import (
|
||
apply_transform_points as _apply_transform_points,
|
||
blend_rigid_tf as _blend_rigid_tf_impl,
|
||
tf_delta as _tf_delta_impl,
|
||
tf_from_xyzyaw as _tf_from_xyzyaw_impl,
|
||
to_world_points as _to_world_points,
|
||
yaw_deg_from_tf as _yaw_deg_from_tf_impl,
|
||
)
|
||
|
||
from SLAM_Filter import VoxelPersistenceFilter
|
||
from SLAM_Filter import FilterConfig as VFilterCfg
|
||
from SLAM_Filter import IndoorMapQualityConfig, IndoorMapQualityFilter
|
||
|
||
from SLAM_MAP import StableMapLayer
|
||
from SLAM_MAP import MapConfig as StableMapCfg
|
||
|
||
from SLAM_LoopClosure import LoopClosureConfig, LoopClosureBackend
|
||
from SLAM_PlaceRecognition import PlaceRecognitionConfig, PlaceRecognitionIndex
|
||
from SLAM_StateMachine import LocalizationStateConfig, LocalizationStateMachine
|
||
from SLAM_Submap import SubmapConfig, LocalGlobalSubmapMapper, SubmapCheckpointer
|
||
from SLAM_Session import SessionMemoryConfig, SessionTransformStore
|
||
from SLAM_Navigation import NavigationExportConfig, NavigationExporter
|
||
from SLAM_Fusion import FusionConfig as SensorFusionConfig, SensorPoseFusion
|
||
from SLAM_NavRuntime import (
|
||
LiveCostmapConfig,
|
||
LiveCostmapRuntime,
|
||
GlobalAStarPlanner,
|
||
LocalPlannerConfig,
|
||
LocalReactivePlanner,
|
||
)
|
||
from SLAM_Mission import MissionConfig, WaypointMissionManager
|
||
from SLAM_Safety import SafetyConfig, SafetySupervisor
|
||
|
||
except Exception as e:
|
||
_safe_put(status_q, ("ERROR", f"Import failed: {e}"))
|
||
return
|
||
|
||
# Notable event keys that survive to the fault log; high-frequency
|
||
# status snapshots (LOC_STATE / MODE / FILTER_TUNING) are excluded so
|
||
# the log stays readable.
|
||
_FAULT_LOG_INFO_KEYS = {
|
||
"LOOP", "MAP_LOCK", "MAP_CLEANUP", "RELOC", "AUTOSAVED", "SAVED",
|
||
"EXPORT", "NAV_EXPORTED", "WORKFLOW_PROFILE", "AUTOSAVE",
|
||
"PLACE_RECOG", "RECORD_SAVED", "SUBMAP_MODE", "LOC_MACHINE",
|
||
"MIN_STABLE_POINTS", "LOOP_MODE",
|
||
}
|
||
|
||
def st(level: str, msg: Any):
|
||
_safe_put(status_q, (level, msg))
|
||
# Persist non-routine events to the fault log so post-hoc
|
||
# diagnostics (drift root-causing, "did loop closure fire") work
|
||
# without the GUI being open at the time.
|
||
if diagnostics is None:
|
||
return
|
||
try:
|
||
lvl = str(level).upper().strip()
|
||
if lvl in ("WARN", "ERROR"):
|
||
diagnostics.note(f"{lvl}: {msg}")
|
||
elif lvl == "INFO" and isinstance(msg, dict):
|
||
for k in msg.keys():
|
||
key_up = str(k).upper()
|
||
if key_up in _FAULT_LOG_INFO_KEYS:
|
||
diagnostics.log_state(key_up, msg.get(k))
|
||
break
|
||
except Exception:
|
||
pass
|
||
|
||
Path(map_cfg.data_folder).mkdir(parents=True, exist_ok=True)
|
||
diagnostics = None
|
||
try:
|
||
fault_log_path = Path(map_cfg.data_folder) / "SLAM_worker_fault.log"
|
||
diagnostics = WorkerDiagnostics(fault_log_path)
|
||
except Exception as e:
|
||
diagnostics = None
|
||
_safe_put(status_q, ("WARN", f"Diagnostics disabled: {e}"))
|
||
try:
|
||
full_cfg = load_slam_config()
|
||
except Exception:
|
||
full_cfg = {}
|
||
loc_cfg_raw = full_cfg.get("localization", {}) if isinstance(full_cfg, dict) else {}
|
||
guard_cfg = full_cfg.get("mapping_guard", {}) if isinstance(full_cfg, dict) else {}
|
||
loc_pt2plane_unsafe = _as_bool(
|
||
(loc_cfg_raw or {}).get("allow_point_to_plane_unsafe", False),
|
||
False,
|
||
)
|
||
loc_use_point_to_plane_requested = _as_bool(
|
||
(loc_cfg_raw or {}).get("use_point_to_plane", False),
|
||
False,
|
||
)
|
||
if loc_use_point_to_plane_requested and not loc_pt2plane_unsafe:
|
||
_safe_put(
|
||
status_q,
|
||
(
|
||
"WARN",
|
||
"Localization point-to-plane disabled for stability. Set localization.allow_point_to_plane_unsafe=true to force it.",
|
||
),
|
||
)
|
||
loc_live_period_sec = max(0.25, float((loc_cfg_raw or {}).get("live_period_sec", 0.55)))
|
||
loc_tracking_window_enabled = _as_bool(
|
||
(loc_cfg_raw or {}).get("tracking_window_enabled", True),
|
||
True,
|
||
)
|
||
loc_tracking_window_radius_m = max(
|
||
2.0,
|
||
float((loc_cfg_raw or {}).get("tracking_window_radius_m", 8.0)),
|
||
)
|
||
loc_tracking_window_min_ref_points = max(
|
||
120,
|
||
int((loc_cfg_raw or {}).get("tracking_window_min_ref_points", 450)),
|
||
)
|
||
loc_tracking_window_max_ref_points = max(
|
||
int(loc_tracking_window_min_ref_points),
|
||
int((loc_cfg_raw or {}).get("tracking_window_max_ref_points", 140000)),
|
||
)
|
||
loc_global_reloc_anchor_voxel_m = max(
|
||
0.4,
|
||
float((loc_cfg_raw or {}).get("global_reloc_anchor_voxel_m", 1.4)),
|
||
)
|
||
loc_global_reloc_max_anchors = max(
|
||
8,
|
||
int((loc_cfg_raw or {}).get("global_reloc_max_anchors", 24)),
|
||
)
|
||
loc_global_reloc_yaw_step_deg = float(
|
||
np.clip(
|
||
float((loc_cfg_raw or {}).get("global_reloc_yaw_step_deg", 60.0)),
|
||
15.0,
|
||
120.0,
|
||
)
|
||
)
|
||
loc_global_reloc_corr_mult = max(
|
||
1.2,
|
||
float((loc_cfg_raw or {}).get("global_reloc_corr_mult", 1.9)),
|
||
)
|
||
loc_global_reloc_coarse_iter = max(
|
||
4,
|
||
int((loc_cfg_raw or {}).get("global_reloc_coarse_iter", 8)),
|
||
)
|
||
loc_global_reloc_refine_iter = max(
|
||
8,
|
||
int((loc_cfg_raw or {}).get("global_reloc_refine_iter", 20)),
|
||
)
|
||
loc_global_reloc_min_corr = max(
|
||
15,
|
||
int((loc_cfg_raw or {}).get("global_reloc_min_corr", 25)),
|
||
)
|
||
loc_bidir_enabled = _as_bool((loc_cfg_raw or {}).get("bidirectional_check_enabled", True), True)
|
||
loc_bidir_min_ratio = float(
|
||
np.clip(float((loc_cfg_raw or {}).get("bidirectional_min_ratio", 0.18)), 0.01, 1.0)
|
||
)
|
||
loc_bidir_max_rmse = max(0.05, float((loc_cfg_raw or {}).get("bidirectional_max_rmse", 0.55)))
|
||
loc_guess_ttl_sec = max(2.0, float((loc_cfg_raw or {}).get("approx_guess_ttl_sec", 60.0)))
|
||
loc_guess_crop_radius_m = max(
|
||
2.0,
|
||
float((loc_cfg_raw or {}).get("approx_guess_crop_radius_m", 9.0)),
|
||
)
|
||
loc_guess_default_z_m = float((loc_cfg_raw or {}).get("approx_guess_default_z_m", 0.0))
|
||
loc_guess_strict_local = _as_bool(
|
||
(loc_cfg_raw or {}).get("approx_guess_strict_local", True),
|
||
True,
|
||
)
|
||
loc_guess_local_radius_m = max(
|
||
1.0,
|
||
min(
|
||
float(loc_guess_crop_radius_m),
|
||
float((loc_cfg_raw or {}).get("approx_guess_local_radius_m", 4.5)),
|
||
),
|
||
)
|
||
loc_guess_global_fallback_after_failures = max(
|
||
1,
|
||
int((loc_cfg_raw or {}).get("approx_guess_global_fallback_after_failures", 6)),
|
||
)
|
||
loc_guess_yaw_step_deg = float(
|
||
np.clip(
|
||
float((loc_cfg_raw or {}).get("approx_guess_yaw_step_deg", 30.0)),
|
||
10.0,
|
||
120.0,
|
||
)
|
||
)
|
||
loc_guess_max_start_offset_m = max(
|
||
1.0,
|
||
float((loc_cfg_raw or {}).get("approx_guess_max_start_offset_m", 6.0)),
|
||
)
|
||
loc_guess_bootstrap_min_hits = max(
|
||
1,
|
||
int((loc_cfg_raw or {}).get("approx_guess_bootstrap_min_hits", 2)),
|
||
)
|
||
loc_guess_bootstrap_min_fitness = float(
|
||
np.clip(
|
||
float((loc_cfg_raw or {}).get("approx_guess_bootstrap_min_fitness", 0.34)),
|
||
0.0,
|
||
1.0,
|
||
)
|
||
)
|
||
loc_guess_bootstrap_min_bidir = float(
|
||
np.clip(
|
||
float((loc_cfg_raw or {}).get("approx_guess_bootstrap_min_bidir", 0.24)),
|
||
0.0,
|
||
1.0,
|
||
)
|
||
)
|
||
loc_live_track_strict = _as_bool(
|
||
(loc_cfg_raw or {}).get("live_track_strict", True),
|
||
True,
|
||
)
|
||
loc_live_track_min_fit = float(
|
||
np.clip(
|
||
float((loc_cfg_raw or {}).get("live_track_min_fitness", 0.42)),
|
||
0.0,
|
||
1.0,
|
||
)
|
||
)
|
||
loc_live_track_min_bidir = float(
|
||
np.clip(
|
||
float((loc_cfg_raw or {}).get("live_track_min_bidir", 0.28)),
|
||
0.0,
|
||
1.0,
|
||
)
|
||
)
|
||
loc_live_track_max_step_trans_m = max(
|
||
0.02,
|
||
float((loc_cfg_raw or {}).get("live_track_max_step_translation_m", 0.20)),
|
||
)
|
||
loc_live_track_max_step_rot_deg = max(
|
||
0.5,
|
||
float((loc_cfg_raw or {}).get("live_track_max_step_rotation_deg", 12.0)),
|
||
)
|
||
loc_live_stack_enabled = _as_bool((loc_cfg_raw or {}).get("live_stack_enabled", True), True)
|
||
loc_live_stack_frames = max(1, int((loc_cfg_raw or {}).get("live_stack_frames", 8)))
|
||
loc_live_stack_voxel_m = max(
|
||
0.04,
|
||
float((loc_cfg_raw or {}).get("live_stack_voxel_m", max(0.06, float(loc_cfg.voxel_localize) * 0.7))),
|
||
)
|
||
loc_live_stack_max_points = max(800, int((loc_cfg_raw or {}).get("live_stack_max_points", 22000)))
|
||
loc_live_stack_min_frames = max(1, int((loc_cfg_raw or {}).get("live_stack_min_frames", 2)))
|
||
|
||
vis_cfg = full_cfg.get("localization_visualization", {}) if isinstance(full_cfg, dict) else {}
|
||
loc_vis_enabled = _as_bool((vis_cfg or {}).get("enabled", True), True)
|
||
loc_vis_period_sec = max(0.10, float((vis_cfg or {}).get("period_sec", 0.35)))
|
||
loc_vis_voxel_m = max(0.08, float((vis_cfg or {}).get("voxel_m", 0.24)))
|
||
loc_vis_max_points = max(4000, int((vis_cfg or {}).get("max_points", 28000)))
|
||
loc_vis_local_radius_m = max(2.0, float((vis_cfg or {}).get("local_radius_m", 12.0)))
|
||
loc_vis_match_dist_m = max(0.03, float((vis_cfg or {}).get("match_dist_m", 0.30)))
|
||
loc_vis_near_dist_m = max(loc_vis_match_dist_m + 0.02, float((vis_cfg or {}).get("near_dist_m", 0.60)))
|
||
loc_vis_confirm_frames = max(1, int((vis_cfg or {}).get("confirm_frames", 3)))
|
||
loc_vis_decay_per_frame = max(1, int((vis_cfg or {}).get("decay_per_frame", 1)))
|
||
|
||
lidar = None
|
||
slam = None
|
||
filt = None
|
||
stable = None
|
||
|
||
loop_cfg = LoopClosureConfig.from_dict(full_cfg.get("loop_closure", {}))
|
||
loop_backend = LoopClosureBackend(loop_cfg)
|
||
|
||
loc_state_cfg = LocalizationStateConfig.from_dict(full_cfg.get("state_machine", {}))
|
||
loc_state = LocalizationStateMachine(loc_state_cfg)
|
||
|
||
session_cfg = SessionMemoryConfig.from_dict(full_cfg.get("session_memory", {}))
|
||
session_store = SessionTransformStore(map_cfg.data_folder, session_cfg)
|
||
|
||
nav_cfg = NavigationExportConfig.from_dict(full_cfg.get("navigation_export", {}))
|
||
nav_exporter = NavigationExporter(nav_cfg, map_cfg.data_folder)
|
||
nav_rt_cfg = LiveCostmapConfig.from_dict(full_cfg.get("navigation_runtime", full_cfg.get("navigation_export", {})))
|
||
nav_runtime = LiveCostmapRuntime(nav_rt_cfg)
|
||
nav_global_planner = GlobalAStarPlanner(blocked_cost=int(nav_rt_cfg.blocked_cost))
|
||
nav_local_planner = LocalReactivePlanner(LocalPlannerConfig.from_dict(full_cfg.get("local_planner", {})))
|
||
# Enforce production speed/rate caps over local-planner defaults. The
|
||
# operator-facing knob is `production.enabled`; when set, the override
|
||
# values from SLAM_Config.json clamp the planner outputs at the source
|
||
# — the server's actuator clamp is a second layer, not the only one.
|
||
_prod_cfg_init = full_cfg.get("production", {}) or {}
|
||
if bool(_prod_cfg_init.get("enabled", False)):
|
||
try:
|
||
_max_lin = _prod_cfg_init.get("max_linear_mps_override")
|
||
_max_ang = _prod_cfg_init.get("max_angular_rps_override")
|
||
if _max_lin is not None:
|
||
nav_local_planner.cfg.max_linear_mps = max(
|
||
0.05, min(float(nav_local_planner.cfg.max_linear_mps), float(_max_lin)))
|
||
if _max_ang is not None:
|
||
nav_local_planner.cfg.max_angular_rps = max(
|
||
0.1, min(float(nav_local_planner.cfg.max_angular_rps), float(_max_ang)))
|
||
except Exception:
|
||
pass
|
||
|
||
fusion_cfg = SensorFusionConfig.from_dict(full_cfg.get("fusion", {}))
|
||
sensor_fusion = SensorPoseFusion(fusion_cfg)
|
||
|
||
mission_cfg = MissionConfig.from_dict(full_cfg.get("mission", {}))
|
||
mission = WaypointMissionManager(mission_cfg)
|
||
|
||
safety_cfg = SafetyConfig.from_dict(full_cfg.get("safety", {}))
|
||
safety = SafetySupervisor(safety_cfg)
|
||
|
||
map_quality_cfg = IndoorMapQualityConfig.from_dict(full_cfg.get("map_quality", {}))
|
||
map_quality = IndoorMapQualityFilter(map_quality_cfg)
|
||
|
||
submap_cfg = SubmapConfig.from_dict(full_cfg.get("submap_mapping", {}))
|
||
submap_mapper = LocalGlobalSubmapMapper(submap_cfg)
|
||
submap_mode_enabled = bool(submap_cfg.enabled)
|
||
_submap_raw = full_cfg.get("submap_mapping", {}) or {}
|
||
_ckpt_enabled = _as_bool(_submap_raw.get("checkpoint_enabled", True), True)
|
||
_ckpt_interval = max(10.0, float(_submap_raw.get("checkpoint_interval_sec", 60.0)))
|
||
submap_ckpt = SubmapCheckpointer(str(map_cfg.data_folder), interval_s=_ckpt_interval) if _ckpt_enabled else None
|
||
if submap_ckpt is not None and submap_mode_enabled:
|
||
if submap_ckpt.load_into(submap_mapper):
|
||
st("INFO", f"Submap checkpoint restored from {map_cfg.data_folder}")
|
||
place_cfg = PlaceRecognitionConfig.from_dict(full_cfg.get("place_recognition", {}))
|
||
place_recog = PlaceRecognitionIndex(place_cfg, Path(__file__).resolve().parent)
|
||
|
||
livox_dbg_cfg = full_cfg.get("livox_debug", {}) or {}
|
||
livox_debug_enabled = _as_bool(livox_dbg_cfg.get("enabled", False), False)
|
||
livox_print_every_n = max(1, int(livox_dbg_cfg.get("print_every_n_frames", 20)))
|
||
|
||
pose_guard_enabled = _as_bool((guard_cfg or {}).get("enabled", True), True)
|
||
pose_guard_max_trans = max(0.08, float((guard_cfg or {}).get("max_frame_translation_m", 0.45)))
|
||
pose_guard_max_rot_deg = max(2.0, float((guard_cfg or {}).get("max_frame_rotation_deg", 30.0)))
|
||
pose_guard_ref_dt = max(0.01, float((guard_cfg or {}).get("reference_dt_sec", 0.06)))
|
||
|
||
map_lock_cfg = full_cfg.get("mapping_lock", {}) if isinstance(full_cfg, dict) else {}
|
||
map_lock_enabled = _as_bool((map_lock_cfg or {}).get("enabled", True), True)
|
||
map_lock_period_sec = max(0.5, float((map_lock_cfg or {}).get("period_sec", 1.5)))
|
||
map_lock_voxel_m = max(0.05, float((map_lock_cfg or {}).get("voxel_m", 0.22)))
|
||
map_lock_max_corr_m = max(0.3, float((map_lock_cfg or {}).get("max_corr_m", 1.2)))
|
||
map_lock_min_stable_pts = max(200, int((map_lock_cfg or {}).get("min_stable_points", 700)))
|
||
map_lock_accept_fitness = float(np.clip(float((map_lock_cfg or {}).get("accept_fitness", 0.28)), 0.0, 1.0))
|
||
map_lock_accept_rmse = max(0.05, float((map_lock_cfg or {}).get("accept_rmse", 0.40)))
|
||
map_lock_max_trans_m = max(0.02, float((map_lock_cfg or {}).get("max_step_translation_m", 0.35)))
|
||
map_lock_max_rot_deg = max(1.0, float((map_lock_cfg or {}).get("max_step_rotation_deg", 15.0)))
|
||
map_lock_apply_damping = _as_bool((map_lock_cfg or {}).get("apply_damping", True), True)
|
||
map_lock_apply_alpha_t = float(np.clip(float((map_lock_cfg or {}).get("apply_alpha_translation", 0.42)), 0.05, 1.0))
|
||
map_lock_apply_alpha_r = float(np.clip(float((map_lock_cfg or {}).get("apply_alpha_rotation", 0.50)), 0.05, 1.0))
|
||
map_lock_apply_max_trans_m = max(0.01, float((map_lock_cfg or {}).get("apply_max_translation_m", 0.08)))
|
||
map_lock_apply_max_rot_deg = max(0.5, float((map_lock_cfg or {}).get("apply_max_rotation_deg", 5.0)))
|
||
map_lock_window_radius_m = max(0.0, float((map_lock_cfg or {}).get("window_radius_m", 8.0)))
|
||
map_lock_min_live_pts = max(60, int((map_lock_cfg or {}).get("min_live_points", 140)))
|
||
map_lock_icp_max_iter = max(5, int((map_lock_cfg or {}).get("icp_max_iter", 20)))
|
||
map_lock_block_on_reject = _as_bool((map_lock_cfg or {}).get("block_on_reject", True), True)
|
||
map_lock_retry_after_sec = float((map_lock_cfg or {}).get("retry_after_reject_sec", 0.20))
|
||
map_lock_retry_after_sec = float(np.clip(map_lock_retry_after_sec, 0.05, max(0.05, map_lock_period_sec)))
|
||
map_lock_block_on_severe = _as_bool((map_lock_cfg or {}).get("block_on_severe_reject", True), True)
|
||
map_lock_severe_fit = float(np.clip(float((map_lock_cfg or {}).get("severe_fit", 0.14)), 0.0, 1.0))
|
||
map_lock_severe_rmse = max(0.10, float((map_lock_cfg or {}).get("severe_rmse", 0.75)))
|
||
map_lock_severe_trans_m = max(
|
||
float(map_lock_max_trans_m),
|
||
float((map_lock_cfg or {}).get("severe_trans_m", 0.45)),
|
||
)
|
||
map_lock_severe_rot_deg = max(
|
||
float(map_lock_max_rot_deg),
|
||
float((map_lock_cfg or {}).get("severe_rot_deg", 22.0)),
|
||
)
|
||
rotate_cfg = full_cfg.get("rotate_guard", {}) if isinstance(full_cfg, dict) else {}
|
||
rotate_mode_enabled = _as_bool((rotate_cfg or {}).get("enabled", True), True)
|
||
rotate_in_place_max_trans_m = max(0.01, float((rotate_cfg or {}).get("in_place_max_trans_m", 0.05)))
|
||
rotate_in_place_min_rot_deg = max(0.5, float((rotate_cfg or {}).get("in_place_min_rot_deg", 4.0)))
|
||
rotate_pose_guard_max_rot_deg = max(
|
||
float(pose_guard_max_rot_deg),
|
||
float((rotate_cfg or {}).get("pose_guard_max_rot_deg", 34.0)),
|
||
)
|
||
rotate_skip_continuity = _as_bool((rotate_cfg or {}).get("skip_continuity", True), True)
|
||
rotate_map_lock_period_sec = max(0.05, float((rotate_cfg or {}).get("map_lock_period_sec", 0.25)))
|
||
rotate_map_lock_accept_fitness = float(
|
||
np.clip(float((rotate_cfg or {}).get("map_lock_accept_fitness", max(0.05, map_lock_accept_fitness - 0.06))), 0.0, 1.0)
|
||
)
|
||
rotate_map_lock_accept_rmse = max(
|
||
float(map_lock_accept_rmse),
|
||
float((rotate_cfg or {}).get("map_lock_accept_rmse", map_lock_accept_rmse + 0.10)),
|
||
)
|
||
rotate_map_lock_max_step_rot_deg = max(
|
||
float(map_lock_max_rot_deg),
|
||
float((rotate_cfg or {}).get("map_lock_max_step_rot_deg", 14.0)),
|
||
)
|
||
rotate_map_lock_apply_alpha_t = float(
|
||
np.clip(
|
||
float((rotate_cfg or {}).get("map_lock_apply_alpha_translation", max(0.05, map_lock_apply_alpha_t * 0.55))),
|
||
0.03,
|
||
1.0,
|
||
)
|
||
)
|
||
rotate_map_lock_apply_alpha_r = float(
|
||
np.clip(
|
||
float((rotate_cfg or {}).get("map_lock_apply_alpha_rotation", max(0.05, map_lock_apply_alpha_r * 0.70))),
|
||
0.03,
|
||
1.0,
|
||
)
|
||
)
|
||
rotate_map_lock_apply_max_trans_m = max(
|
||
0.005,
|
||
float((rotate_cfg or {}).get("map_lock_apply_max_translation_m", max(0.01, map_lock_apply_max_trans_m * 0.45))),
|
||
)
|
||
rotate_map_lock_apply_max_rot_deg = max(
|
||
0.5,
|
||
float((rotate_cfg or {}).get("map_lock_apply_max_rotation_deg", max(1.0, map_lock_apply_max_rot_deg * 0.70))),
|
||
)
|
||
rotate_freeze_mapping = _as_bool((rotate_cfg or {}).get("freeze_mapping_while_rotating", True), True)
|
||
rotate_freeze_min_stable_pts = max(0, int((rotate_cfg or {}).get("freeze_min_stable_points", 250)))
|
||
rotate_map_lock_block_on_reject = _as_bool((rotate_cfg or {}).get("block_on_reject", False), False)
|
||
rotate_map_lock_block_on_severe = _as_bool((rotate_cfg or {}).get("block_on_severe", True), True)
|
||
continuity_cfg = full_cfg.get("continuity_guard", {}) if isinstance(full_cfg, dict) else {}
|
||
continuity_enabled = _as_bool((continuity_cfg or {}).get("enabled", True), True)
|
||
continuity_voxel_m = max(0.05, float((continuity_cfg or {}).get("voxel_m", 0.24)))
|
||
continuity_min_points = max(40, int((continuity_cfg or {}).get("min_points", 120)))
|
||
continuity_match_radius_m = max(0.08, float((continuity_cfg or {}).get("match_radius_m", 0.45)))
|
||
continuity_min_inlier_ratio = float(np.clip(float((continuity_cfg or {}).get("min_inlier_ratio", 0.35)), 0.01, 1.0))
|
||
continuity_max_median_m = max(0.05, float((continuity_cfg or {}).get("max_median_m", 0.30)))
|
||
continuity_max_p90_m = max(0.08, float((continuity_cfg or {}).get("max_p90_m", 0.60)))
|
||
continuity_recover_after_rejects = max(2, int((continuity_cfg or {}).get("recover_after_rejects", 10)))
|
||
cleanup_cfg = full_cfg.get("map_cleanup", {}) if isinstance(full_cfg, dict) else {}
|
||
cleanup_enabled = _as_bool((cleanup_cfg or {}).get("enabled", True), True)
|
||
cleanup_period_sec = max(0.5, float((cleanup_cfg or {}).get("period_sec", 1.8)))
|
||
cleanup_voxel_m = max(0.05, float((cleanup_cfg or {}).get("voxel_m", 0.16)))
|
||
cleanup_cluster_radius_m = max(0.12, float((cleanup_cfg or {}).get("cluster_radius_m", 0.42)))
|
||
cleanup_min_cluster_points = max(20, int((cleanup_cfg or {}).get("min_cluster_points", 120)))
|
||
cleanup_keep_largest_n = max(1, int((cleanup_cfg or {}).get("keep_largest_n", 1)))
|
||
cleanup_strict_largest_only = _as_bool((cleanup_cfg or {}).get("strict_largest_only", True), True)
|
||
cleanup_min_total_points = max(150, int((cleanup_cfg or {}).get("min_total_points", 800)))
|
||
|
||
autosave_cfg = full_cfg.get("autosave", {}) or {}
|
||
autosave_enabled = _as_bool(autosave_cfg.get("enabled", False), False)
|
||
autosave_interval_sec = max(5.0, float(autosave_cfg.get("interval_sec", 90.0)))
|
||
autosave_base_name = str(autosave_cfg.get("base_name", "autosave_map"))
|
||
last_autosave_t = 0.0
|
||
|
||
self_check_report = run_startup_self_check(full_cfg, Path(__file__).resolve().parent)
|
||
|
||
connected = False
|
||
mapping_enabled = False
|
||
localize_only_enabled = False
|
||
worker_mode = "IDLE"
|
||
running = True
|
||
state_lock = threading.RLock()
|
||
|
||
# localization
|
||
ref_map_path: Optional[str] = None
|
||
|
||
# publish control
|
||
pub_hz = float(run_cfg.publish_hz)
|
||
last_pub = 0.0
|
||
|
||
# keyframe gating
|
||
last_key_pose = None
|
||
last_frame_t = 0.0
|
||
last_guard_pose = None
|
||
guard_reject_count = 0
|
||
guard_last_warn_t = 0.0
|
||
odom_to_map_transform = np.eye(4, dtype=np.float64)
|
||
map_lock_last_t = 0.0
|
||
map_lock_accept_count = 0
|
||
map_lock_reject_count = 0
|
||
# Two consecutive-rejection counters for the auto-recover gate:
|
||
#
|
||
# map_lock_consecutive_rejects — total streak (status display)
|
||
# map_lock_consecutive_safe_rejects — only NON-severe rejects; this
|
||
# is what triggers auto-recover.
|
||
#
|
||
# A "severe" rejection (low fitness, high RMSE, large pose jump) is
|
||
# the gate's signal that pose has actually drifted and the frame is
|
||
# unsafe to integrate — force-accepting in that case would corrupt
|
||
# the map (cause duplicate walls / "ghosting"). So severe rejects
|
||
# reset the safe-streak counter; only sustained NON-severe rejection
|
||
# (the gate is being conservative on borderline good poses) trips
|
||
# the auto-recover.
|
||
map_lock_consecutive_rejects = 0
|
||
map_lock_consecutive_safe_rejects = 0
|
||
map_lock_auto_recover_after = max(5, int(
|
||
(map_lock_cfg or {}).get("auto_recover_after_consecutive", 25)
|
||
))
|
||
map_lock_last_warn_t = 0.0
|
||
continuity_prev_pts = None
|
||
continuity_reject_count = 0
|
||
continuity_consecutive_rejects = 0
|
||
continuity_last_warn_t = 0.0
|
||
rotate_freeze_count = 0
|
||
rotate_last_warn_t = 0.0
|
||
cleanup_last_t = 0.0
|
||
cleanup_removed_total = 0
|
||
cleanup_last_info: Dict[str, Any] = {"status": "na"}
|
||
|
||
# localization cadence/state
|
||
last_localize_t = 0.0
|
||
last_localize_points = 0
|
||
last_localize_transform = np.eye(4, dtype=np.float64)
|
||
last_localize_confidence = 0.0
|
||
# Snapshot of the most recent ICP outcome — written by do_localize,
|
||
# read by the FRAME builder so the dashboard can show live fitness/
|
||
# RMSE/inliers + the per-call ICP processing time (a.k.a. the
|
||
# `delay_ms` topic in FAST_LIO_LOCALIZATION_HUMANOID). lidar_api
|
||
# already plumbs this through to nav.loc_result → loc_metrics.
|
||
last_loc_icp_result: Dict[str, Any] = {}
|
||
|
||
# ── Loop closure state ─────────────────────────────────────────
|
||
# Periodic ICP against a local crop of the WORKER's OWN stable
|
||
# buffer (no ref-map required). When the robot returns to a
|
||
# previously-visited area, the ICP transform IS the drift
|
||
# correction — applied to current pose to "snap" the loop shut.
|
||
#
|
||
# Disabled by default (loop_closure.enabled=false in config).
|
||
# Flip the config flag once the stable map is big enough to
|
||
# benefit (typically >2 minutes of mapping a >20 m space).
|
||
_lc_cfg: Dict[str, Any] = full_cfg.get("loop_closure", {}) or {}
|
||
_lc_enabled = bool(_lc_cfg.get("enabled", False))
|
||
_lc_keyframe_dist_m = float(_lc_cfg.get("keyframe_dist_m", 2.0))
|
||
_lc_keyframe_max = int(_lc_cfg.get("keyframe_max_kept", 30))
|
||
_lc_check_interval = float(_lc_cfg.get("check_interval_sec", 5.0))
|
||
_lc_min_age_sec = float(_lc_cfg.get("min_keyframe_age_sec", 30.0))
|
||
_lc_match_radius_m = float(_lc_cfg.get("match_radius_m", 3.5))
|
||
_lc_icp_voxel = float(_lc_cfg.get("icp_voxel_m", 0.20))
|
||
_lc_icp_max_iter = int(_lc_cfg.get("icp_max_iter", 25))
|
||
_lc_icp_max_corr = float(_lc_cfg.get("icp_max_corr_m", 1.0))
|
||
_lc_min_fitness = float(_lc_cfg.get("min_fitness", 0.45))
|
||
_lc_min_drift = float(_lc_cfg.get("min_drift_m", 0.20))
|
||
_lc_max_drift = float(_lc_cfg.get("max_drift_m", 5.0))
|
||
# Keyframes: list of (x, y, timestamp). Tracks every save_loop_kf
|
||
# event; capped at _lc_keyframe_max via FIFO trim.
|
||
_lc_keyframes: List[Tuple[float, float, float]] = []
|
||
_lc_last_check_t: float = 0.0
|
||
_lc_closures_applied: int = 0
|
||
_lc_last_closure_info: Dict[str, Any] = {}
|
||
ref_pcd_cache = None
|
||
ref_points_cache: Optional[np.ndarray] = None
|
||
ref_nav_points_cache: Optional[np.ndarray] = None
|
||
ref_nav_cache_src_n: int = 0
|
||
ref_cache_path: Optional[str] = None
|
||
slam_to_ref_transform = np.eye(4, dtype=np.float64)
|
||
slam_to_ref_valid = False
|
||
approx_pose_guess_tf: Optional[np.ndarray] = None
|
||
approx_pose_guess_until_t: float = 0.0
|
||
approx_pose_guess_fail_count: int = 0
|
||
approx_pose_guess_success_count: int = 0
|
||
ref_match_points_cache: Optional[np.ndarray] = None
|
||
ref_match_confirm_hits: Optional[np.ndarray] = None
|
||
ref_match_last_pub_t: float = 0.0
|
||
ref_match_last_stats: Dict[str, Any] = {}
|
||
loc_live_stack: deque[np.ndarray] = deque(maxlen=int(loc_live_stack_frames))
|
||
loc_frames = LocalizationFrameState()
|
||
|
||
# runtime point-density control (LOW/MEDIUM/HIGH)
|
||
base_stride = max(1, int(eng_cfg.pre_downsample_stride))
|
||
density_mode = "MEDIUM"
|
||
live_stride = base_stride
|
||
runtime_filter_voxel = float(filt_cfg.voxel_size)
|
||
runtime_filter_hit_threshold = int(max(1, filt_cfg.hits_threshold))
|
||
runtime_filter_decay = float(filt_cfg.strict_sec if filt_cfg.use_strict else filt_cfg.window_sec)
|
||
stability_profile = "BALANCED"
|
||
|
||
# telemetry/performance
|
||
perf_input_count = 0
|
||
perf_publish_count = 0
|
||
perf_last_rate_t = _now()
|
||
perf_last_cpu_wall_t = _now()
|
||
perf_last_cpu_proc_t = time.process_time()
|
||
perf_input_fps = 0.0
|
||
perf_publish_fps = 0.0
|
||
perf_icp_ms = 0.0
|
||
perf_cpu_pct = 0.0
|
||
perf_queue_lag = 0
|
||
perf_stable_growth = 0.0
|
||
perf_prev_stable_count = 0
|
||
perf_prev_stable_t = _now()
|
||
|
||
# recording / replay capture
|
||
recording_enabled = False
|
||
recording_frames: List[np.ndarray] = []
|
||
recording_poses: List[np.ndarray] = []
|
||
recording_base_name = "slam_recording"
|
||
recording_max_frames = max(100, int((full_cfg.get("replay", {}) or {}).get("max_record_frames", 6000)))
|
||
recording_drop_count = 0
|
||
|
||
# nav runtime / planner / mission
|
||
nav_goal_xy: Optional[Tuple[float, float]] = None
|
||
# ── Autonomous wander state ──
|
||
# When `wander_active` is True AND the current workflow is in the
|
||
# config's allowed list, the worker picks its own goals every
|
||
# `pick_interval_sec` (or whenever the current goal is reached /
|
||
# times out). Goals are random reachable cells from the costmap
|
||
# within [min_reach_radius_m, max_reach_radius_m]. Holonomic
|
||
# evasion + stuck-recovery are auto-enabled while wander is active
|
||
# so the robot can actually get around obstacles autonomously even
|
||
# in MAP_NEW / EXTEND_MAP (which normally lock those off).
|
||
#
|
||
# `_wander_last_pick_t` is the wall-clock time of the most recent
|
||
# goal pick; `_wander_goal_origin` records whether the *current*
|
||
# nav_goal_xy was set by wander or by the operator — so a manual
|
||
# click pauses wander cleanly (operator wins, wander resumes after
|
||
# the manual goal is reached or cleared).
|
||
wander_active: bool = False
|
||
_wander_last_pick_t: float = 0.0
|
||
_wander_goal_origin: str = "operator" # "operator" or "wander"
|
||
_wander_cfg: Dict[str, Any] = full_cfg.get("autonomous_wander", {}) or {}
|
||
# ── Wander exploration intelligence state ────────────────────────
|
||
# COVERAGE MEMORY (#2): coarse 2 m grid counting how many ticks the
|
||
# robot's pose has fallen inside each cell. Lets the goal picker
|
||
# bias AWAY from already-visited cells so the robot doesn't shuffle
|
||
# in the same corner. Keyed by (cgx, cgy) where cgx=floor(rx/2.0).
|
||
_coverage_visits: Dict[Tuple[int, int], int] = {}
|
||
# FAILED-GOAL BLACKLIST (#3): goals that timed out (robot couldn't
|
||
# reach them in `goal_timeout_sec`) get added here. Future picks
|
||
# within 2 m of any entry are rejected. Each entry expires after
|
||
# 2 minutes so a temporarily-blocked area (person standing in
|
||
# doorway) re-opens automatically.
|
||
_blacklisted_goals: List[Tuple[float, float, float]] = []
|
||
# POSE-QUALITY AUTO-PAUSE (#11): track when localization fitness
|
||
# dropped below threshold so motion can be paused on sustained
|
||
# bad pose. Reset to None when fitness recovers.
|
||
_pose_quality_low_since: Optional[float] = None
|
||
_pose_quality_auto_paused: bool = False
|
||
# BOUNDED EXPLORE-THEN-RETURN (#5): operator can arm a time-budget
|
||
# via the WS `set_explore_budget` cmd. When the budget expires the
|
||
# worker overrides wander's goal picker to return-to-origin once.
|
||
_explore_origin_pose: Optional[Tuple[float, float]] = None
|
||
_explore_budget_deadline: Optional[float] = None
|
||
_explore_returning_home: bool = False
|
||
nav_last_update_t = 0.0
|
||
nav_last_plan_t = 0.0
|
||
nav_plan_period_sec = max(0.15, float((full_cfg.get("navigation_runtime", {}) or {}).get("plan_period_sec", 0.35)))
|
||
nav_path_world: List[Tuple[float, float]] = []
|
||
latest_live_localize_pts: Optional[np.ndarray] = None
|
||
raw_points_q: queue.Queue = queue.Queue(maxsize=max(2, int(run_cfg.frame_queue_maxsize)))
|
||
|
||
# ── Optional ZMQ raw-frame transport ─────────────────────────────
|
||
# SLAM_RAW_PUB_PORT : if set, on_points ALSO publishes each parsed
|
||
# frame on tcp://0.0.0.0:<port> as a single ZMQ
|
||
# PUB message. Used by the Jetson's brain
|
||
# worker so a parallel workstation worker can
|
||
# do SLAM on the same input without binding the
|
||
# Livox SDK directly.
|
||
# SLAM_REMOTE_INPUT_URL : if set (e.g. "tcp://10.0.0.5:55600"),
|
||
# do_connect() SKIPS Livox SDK init and instead
|
||
# spawns a SUB thread that calls on_points with
|
||
# each received frame. Lets the workstation
|
||
# run full SLAM off the brain's relayed stream.
|
||
#
|
||
# Both are env-gated so the brain's normal SDK path + the
|
||
# workstation's remote-input path stay clean; standalone runs (no
|
||
# env vars set) work exactly as before.
|
||
import os as _os
|
||
_raw_pub_port_str = (_os.environ.get("SLAM_RAW_PUB_PORT") or "").strip()
|
||
_remote_input_url = (_os.environ.get("SLAM_REMOTE_INPUT_URL") or "").strip()
|
||
_raw_pub_sock: "object | None" = None
|
||
_raw_sub_thread: "object | None" = None
|
||
_raw_sub_stop = threading.Event()
|
||
if _raw_pub_port_str:
|
||
try:
|
||
import zmq as _zmq_pub
|
||
_raw_pub_ctx = _zmq_pub.Context.instance()
|
||
_raw_pub_sock = _raw_pub_ctx.socket(_zmq_pub.PUB)
|
||
_raw_pub_sock.setsockopt(_zmq_pub.SNDHWM, 2)
|
||
_raw_pub_sock.setsockopt(_zmq_pub.LINGER, 0)
|
||
_raw_pub_sock.bind(f"tcp://0.0.0.0:{int(_raw_pub_port_str)}")
|
||
_log.info("Raw-frame ZMQ PUB bound on tcp://0.0.0.0:%s",
|
||
_raw_pub_port_str)
|
||
except Exception as _e:
|
||
_log.error("Raw-frame ZMQ PUB bind failed: %s", _e)
|
||
_raw_pub_sock = None
|
||
|
||
def _sync_loc_vars_from_service() -> None:
|
||
nonlocal odom_to_map_transform, last_localize_transform, last_localize_confidence
|
||
nonlocal slam_to_ref_transform, slam_to_ref_valid
|
||
odom_to_map_transform = np.array(loc_frames.odom_to_map, dtype=np.float64, copy=True)
|
||
last_localize_transform = np.array(loc_frames.last_alignment, dtype=np.float64, copy=True)
|
||
last_localize_confidence = float(loc_frames.confidence)
|
||
slam_to_ref_transform = np.array(loc_frames.odom_to_ref, dtype=np.float64, copy=True)
|
||
slam_to_ref_valid = bool(loc_frames.ref_valid)
|
||
|
||
def _worker_diag_state() -> Dict[str, Any]:
|
||
return {
|
||
"connected": bool(connected),
|
||
"mapping_enabled": bool(mapping_enabled),
|
||
"localize_only_enabled": bool(localize_only_enabled),
|
||
"worker_mode": str(worker_mode),
|
||
"ref_map_path": str(ref_map_path) if ref_map_path else "",
|
||
"last_localize_points": int(last_localize_points),
|
||
"last_localize_t": float(last_localize_t),
|
||
"density_mode": str(density_mode),
|
||
"stability_profile": str(stability_profile),
|
||
"submap_enabled": bool(submap_mode_enabled),
|
||
"loop_closure_enabled": bool(loop_cfg.enabled),
|
||
"loc_frames": loc_frames.snapshot(),
|
||
}
|
||
|
||
def _clear_raw_points_queue() -> None:
|
||
try:
|
||
while True:
|
||
raw_points_q.get_nowait()
|
||
except Exception:
|
||
return
|
||
|
||
# ---------------- init stack ----------------
|
||
def init_slam_stack():
|
||
nonlocal slam, filt, stable
|
||
|
||
# kiss_icp ≥ 1.2.x makes `deskew` a required positional arg with no
|
||
# default. G1_Lootah's worker was written against an older release
|
||
# where it had a default. Try the new signature first, fall back to
|
||
# the old one — same algorithm either way, just signature drift.
|
||
try:
|
||
cfg = load_config(
|
||
config_file=None,
|
||
deskew=True,
|
||
max_range=float(eng_cfg.max_range),
|
||
)
|
||
except TypeError:
|
||
cfg = load_config(config_file=None, max_range=float(eng_cfg.max_range))
|
||
cfg.mapping.voxel_size = float(eng_cfg.slam_voxel_size)
|
||
slam = KissICP(cfg)
|
||
|
||
vf_cfg = VFilterCfg(
|
||
voxel_size=float(runtime_filter_voxel),
|
||
hit_threshold=int(runtime_filter_hit_threshold),
|
||
decay_seconds=float(runtime_filter_decay),
|
||
max_voxels=int(filt_cfg.max_voxels),
|
||
)
|
||
filt = VoxelPersistenceFilter(vf_cfg)
|
||
|
||
sm_cfg = StableMapCfg(
|
||
display_voxel=float(map_cfg.display_voxel),
|
||
save_voxel=float(map_cfg.save_voxel),
|
||
data_folder=str(map_cfg.data_folder),
|
||
save_extension=str(map_cfg.save_extension),
|
||
)
|
||
stable = StableMapLayer(sm_cfg)
|
||
|
||
def _run_locked(fn, *args, **kwargs):
|
||
with state_lock:
|
||
return fn(*args, **kwargs)
|
||
|
||
def reset_all():
|
||
nonlocal mapping_enabled, localize_only_enabled, worker_mode, last_key_pose
|
||
nonlocal last_guard_pose, guard_reject_count, guard_last_warn_t
|
||
nonlocal last_frame_t
|
||
nonlocal map_lock_last_t, map_lock_accept_count, map_lock_reject_count, map_lock_last_warn_t
|
||
nonlocal map_lock_consecutive_rejects, map_lock_consecutive_safe_rejects
|
||
nonlocal continuity_prev_pts, continuity_reject_count, continuity_consecutive_rejects, continuity_last_warn_t
|
||
nonlocal rotate_freeze_count, rotate_last_warn_t
|
||
nonlocal cleanup_last_t, cleanup_removed_total, cleanup_last_info
|
||
nonlocal last_localize_t, last_localize_points
|
||
nonlocal ref_pcd_cache, ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n, ref_cache_path
|
||
nonlocal approx_pose_guess_tf, approx_pose_guess_until_t, approx_pose_guess_fail_count, approx_pose_guess_success_count
|
||
nonlocal ref_match_points_cache, ref_match_confirm_hits, ref_match_last_pub_t, ref_match_last_stats
|
||
nonlocal last_autosave_t
|
||
nonlocal perf_input_count, perf_publish_count, perf_last_rate_t
|
||
nonlocal perf_last_cpu_wall_t, perf_last_cpu_proc_t
|
||
nonlocal perf_input_fps, perf_publish_fps, perf_icp_ms, perf_cpu_pct
|
||
nonlocal perf_queue_lag, perf_stable_growth, perf_prev_stable_count, perf_prev_stable_t
|
||
nonlocal recording_enabled, recording_frames, recording_poses, recording_drop_count
|
||
nonlocal nav_goal_xy, nav_last_update_t, nav_last_plan_t, nav_path_world, latest_live_localize_pts
|
||
init_slam_stack()
|
||
mapping_enabled = False
|
||
localize_only_enabled = False
|
||
worker_mode = "IDLE"
|
||
last_frame_t = 0.0
|
||
last_key_pose = None
|
||
last_guard_pose = None
|
||
guard_reject_count = 0
|
||
guard_last_warn_t = 0.0
|
||
loc_frames.reset()
|
||
_sync_loc_vars_from_service()
|
||
map_lock_last_t = 0.0
|
||
map_lock_accept_count = 0
|
||
map_lock_reject_count = 0
|
||
map_lock_consecutive_rejects = 0
|
||
map_lock_consecutive_safe_rejects = 0
|
||
map_lock_last_warn_t = 0.0
|
||
continuity_prev_pts = None
|
||
continuity_reject_count = 0
|
||
continuity_consecutive_rejects = 0
|
||
continuity_last_warn_t = 0.0
|
||
rotate_freeze_count = 0
|
||
rotate_last_warn_t = 0.0
|
||
cleanup_last_t = 0.0
|
||
cleanup_removed_total = 0
|
||
cleanup_last_info = {"status": "na"}
|
||
last_localize_t = 0.0
|
||
last_localize_points = 0
|
||
ref_pcd_cache = None
|
||
ref_points_cache = None
|
||
ref_nav_points_cache = None
|
||
ref_nav_cache_src_n = 0
|
||
ref_cache_path = None
|
||
approx_pose_guess_tf = None
|
||
approx_pose_guess_until_t = 0.0
|
||
approx_pose_guess_fail_count = 0
|
||
approx_pose_guess_success_count = 0
|
||
ref_match_points_cache = None
|
||
ref_match_confirm_hits = None
|
||
ref_match_last_pub_t = 0.0
|
||
ref_match_last_stats = {}
|
||
loc_live_stack.clear()
|
||
loop_backend.reset()
|
||
loc_state.reset()
|
||
submap_mapper.reset()
|
||
if submap_ckpt is not None:
|
||
submap_ckpt.delete()
|
||
sensor_fusion.reset()
|
||
_imu_integrator.reset()
|
||
mission.reset()
|
||
nav_runtime.reset()
|
||
last_autosave_t = 0.0
|
||
perf_input_count = 0
|
||
perf_publish_count = 0
|
||
perf_last_rate_t = _now()
|
||
perf_last_cpu_wall_t = _now()
|
||
perf_last_cpu_proc_t = time.process_time()
|
||
perf_input_fps = 0.0
|
||
perf_publish_fps = 0.0
|
||
perf_icp_ms = 0.0
|
||
perf_cpu_pct = 0.0
|
||
perf_queue_lag = 0
|
||
perf_stable_growth = 0.0
|
||
perf_prev_stable_count = 0
|
||
perf_prev_stable_t = _now()
|
||
recording_enabled = False
|
||
recording_frames = []
|
||
recording_poses = []
|
||
recording_drop_count = 0
|
||
nav_goal_xy = None
|
||
nav_last_update_t = 0.0
|
||
nav_last_plan_t = 0.0
|
||
nav_path_world = []
|
||
latest_live_localize_pts = None
|
||
_clear_raw_points_queue()
|
||
st("INFO", "RESET done: SLAM + Filter + Stable map cleared. (no save)")
|
||
|
||
init_slam_stack()
|
||
|
||
# ---------------- helpers ----------------
|
||
def height_colors_fast(pts: np.ndarray, alpha: float = 0.9) -> np.ndarray:
|
||
if pts is None or len(pts) == 0:
|
||
return np.zeros((0, 4), dtype=np.float32)
|
||
z = pts[:, 2]
|
||
colors = np.zeros((len(pts), 4), dtype=np.float32)
|
||
colors[:, 3] = alpha
|
||
norm = np.clip((z + 1.5) / 3.0, 0, 1)
|
||
colors[:, 0] = norm
|
||
colors[:, 1] = 1.0 - np.abs(norm - 0.5) * 2.0
|
||
colors[:, 2] = 1.0 - norm
|
||
return colors
|
||
|
||
def pose_delta_ok(pose, last_pose) -> bool:
|
||
if last_pose is None:
|
||
return True
|
||
dp = pose[:3, 3] - last_pose[:3, 3]
|
||
trans = float(np.linalg.norm(dp))
|
||
R = pose[:3, :3] @ last_pose[:3, :3].T
|
||
ang = float(np.degrees(np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1))))
|
||
return (trans >= float(eng_cfg.keyframe_min_translation_m)) or (ang >= float(eng_cfg.keyframe_min_rotation_deg))
|
||
|
||
def to_world_points(points_sensor: np.ndarray, pose: Optional[np.ndarray]) -> np.ndarray:
|
||
"""
|
||
Convert sensor-frame points to world frame using pose (world_T_sensor).
|
||
Falls back to input points when pose is unavailable/invalid.
|
||
"""
|
||
return _to_world_points(points_sensor, pose)
|
||
|
||
def apply_transform_points(points: Optional[np.ndarray], transform: np.ndarray) -> Optional[np.ndarray]:
|
||
return _apply_transform_points(points, transform)
|
||
|
||
def _tf_delta(prev_tf: np.ndarray, new_tf: np.ndarray) -> Tuple[float, float]:
|
||
return _tf_delta_impl(prev_tf, new_tf)
|
||
|
||
def _blend_rigid_tf(prev_tf: np.ndarray, new_tf: np.ndarray, alpha_t: float, alpha_r: float) -> np.ndarray:
|
||
return _blend_rigid_tf_impl(prev_tf, new_tf, alpha_t, alpha_r)
|
||
|
||
def _yaw_deg_from_tf(tf: np.ndarray) -> float:
|
||
return _yaw_deg_from_tf_impl(tf)
|
||
|
||
def _tf_from_xyzyaw(x: float, y: float, z: float, yaw_deg: float) -> np.ndarray:
|
||
return _tf_from_xyzyaw_impl(x, y, z, yaw_deg)
|
||
|
||
def voxel_downsample_np(points: np.ndarray, voxel_m: float) -> np.ndarray:
|
||
pts = np.asarray(points, dtype=np.float32)
|
||
if pts.ndim != 2 or pts.shape[1] != 3 or len(pts) == 0:
|
||
return np.zeros((0, 3), dtype=np.float32)
|
||
v = float(voxel_m)
|
||
if v <= 0.0:
|
||
return pts
|
||
keys = np.floor(pts / v).astype(np.int32)
|
||
_, first_idx = np.unique(keys, axis=0, return_index=True)
|
||
return pts[np.sort(first_idx)]
|
||
|
||
def _rigid_fit_svd(src_pts: np.ndarray, dst_pts: np.ndarray) -> Optional[np.ndarray]:
|
||
"""Estimate rigid transform (dst = T * src) with SVD."""
|
||
if src_pts is None or dst_pts is None:
|
||
return None
|
||
if len(src_pts) < 3 or len(dst_pts) < 3:
|
||
return None
|
||
a = np.asarray(src_pts, dtype=np.float64)
|
||
b = np.asarray(dst_pts, dtype=np.float64)
|
||
if a.shape != b.shape or a.ndim != 2 or a.shape[1] != 3:
|
||
return None
|
||
c_a = np.mean(a, axis=0)
|
||
c_b = np.mean(b, axis=0)
|
||
aa = a - c_a
|
||
bb = b - c_b
|
||
H = aa.T @ bb
|
||
try:
|
||
U, _, Vt = np.linalg.svd(H, full_matrices=False)
|
||
except Exception:
|
||
return None
|
||
R = Vt.T @ U.T
|
||
if np.linalg.det(R) < 0:
|
||
Vt[2, :] *= -1.0
|
||
R = Vt.T @ U.T
|
||
t = c_b - (R @ c_a)
|
||
T = np.eye(4, dtype=np.float64)
|
||
T[:3, :3] = R
|
||
T[:3, 3] = t
|
||
return T
|
||
|
||
def _icp_point_to_point_np(
|
||
src_pts: np.ndarray,
|
||
tgt_pts: np.ndarray,
|
||
init_tf: Optional[np.ndarray],
|
||
max_corr_m: float,
|
||
max_iter: int,
|
||
min_corr: int = 30,
|
||
) -> Dict[str, Any]:
|
||
"""
|
||
Pure NumPy/SciPy point-to-point ICP to avoid Open3D native crashes.
|
||
Returns {transform, fitness, rmse, inliers}.
|
||
"""
|
||
src0 = np.asarray(src_pts, dtype=np.float64)
|
||
tgt = np.asarray(tgt_pts, dtype=np.float64)
|
||
if src0.ndim != 2 or tgt.ndim != 2 or src0.shape[1] != 3 or tgt.shape[1] != 3:
|
||
return {"transform": np.eye(4, dtype=np.float64), "fitness": 0.0, "rmse": 9e9, "inliers": 0}
|
||
if len(src0) < 3 or len(tgt) < 3:
|
||
return {"transform": np.eye(4, dtype=np.float64), "fitness": 0.0, "rmse": 9e9, "inliers": 0}
|
||
|
||
# scipy resolved once at module load (`_scipy_cKDTree`). The earlier
|
||
# per-call `from scipy.spatial import cKDTree` walked sys.modules
|
||
# every ICP tick (~20 Hz) on the worker hot path.
|
||
if _scipy_cKDTree is None:
|
||
return {"transform": np.eye(4, dtype=np.float64), "fitness": 0.0, "rmse": 9e9, "inliers": 0}
|
||
cKDTree = _scipy_cKDTree
|
||
|
||
T = np.eye(4, dtype=np.float64) if init_tf is None else np.asarray(init_tf, dtype=np.float64).copy()
|
||
if T.shape != (4, 4):
|
||
T = np.eye(4, dtype=np.float64)
|
||
tree = cKDTree(tgt)
|
||
max_corr = float(max(0.05, max_corr_m))
|
||
min_corr_n = int(max(10, min_corr))
|
||
last_rmse = 9e9
|
||
last_inliers = 0
|
||
|
||
for _ in range(int(max(1, max_iter))):
|
||
src_w = (src0 @ T[:3, :3].T) + T[:3, 3]
|
||
d, idx = tree.query(src_w, k=1)
|
||
keep = np.isfinite(d) & (d <= max_corr)
|
||
n_in = int(np.count_nonzero(keep))
|
||
if n_in < min_corr_n:
|
||
break
|
||
src_corr = src_w[keep]
|
||
tgt_corr = tgt[np.asarray(idx[keep], dtype=np.int32)]
|
||
delta = _rigid_fit_svd(src_corr, tgt_corr)
|
||
if delta is None:
|
||
break
|
||
T = np.asarray(delta, dtype=np.float64) @ T
|
||
prev_rmse = float(last_rmse)
|
||
rmse = float(np.sqrt(np.mean(np.square(d[keep]))))
|
||
last_rmse = rmse
|
||
last_inliers = n_in
|
||
# Tiny improvement => converged.
|
||
if abs(prev_rmse - rmse) < 1e-5:
|
||
break
|
||
|
||
fitness = float(last_inliers) / max(1.0, float(len(src0)))
|
||
return {"transform": T, "fitness": float(fitness), "rmse": float(last_rmse), "inliers": int(last_inliers)}
|
||
|
||
def _bidir_match_stats(
|
||
src_pts: np.ndarray,
|
||
tgt_pts: np.ndarray,
|
||
transform: np.ndarray,
|
||
max_corr_m: float,
|
||
) -> Dict[str, Any]:
|
||
stats = {
|
||
"ok": False,
|
||
"live_to_ref_inlier": 0.0,
|
||
"ref_to_live_inlier": 0.0,
|
||
"bidir_inlier": 0.0,
|
||
"bidir_rmse": 9e9,
|
||
}
|
||
if _scipy_cKDTree is None:
|
||
return stats
|
||
cKDTree = _scipy_cKDTree
|
||
try:
|
||
src = np.asarray(src_pts, dtype=np.float64)
|
||
tgt = np.asarray(tgt_pts, dtype=np.float64)
|
||
tf = np.asarray(transform, dtype=np.float64)
|
||
if src.ndim != 2 or tgt.ndim != 2 or src.shape[1] != 3 or tgt.shape[1] != 3:
|
||
return stats
|
||
if len(src) < 8 or len(tgt) < 8 or tf.shape != (4, 4):
|
||
return stats
|
||
src_w = (src @ tf[:3, :3].T) + tf[:3, 3]
|
||
max_corr = max(0.05, float(max_corr_m))
|
||
tree_t = cKDTree(tgt)
|
||
d_st, _ = tree_t.query(src_w, k=1)
|
||
in_st = np.isfinite(d_st) & (d_st <= max_corr)
|
||
in_st_ratio = float(np.count_nonzero(in_st)) / max(1.0, float(len(src_w)))
|
||
|
||
tree_s = cKDTree(src_w)
|
||
d_ts, _ = tree_s.query(tgt, k=1)
|
||
in_ts = np.isfinite(d_ts) & (d_ts <= max_corr)
|
||
in_ts_ratio = float(np.count_nonzero(in_ts)) / max(1.0, float(len(tgt)))
|
||
|
||
inlier_mix = np.concatenate([d_st[in_st], d_ts[in_ts]], axis=0)
|
||
rmse = float(np.sqrt(np.mean(np.square(inlier_mix)))) if len(inlier_mix) > 0 else 9e9
|
||
|
||
stats.update(
|
||
{
|
||
"ok": True,
|
||
"live_to_ref_inlier": float(in_st_ratio),
|
||
"ref_to_live_inlier": float(in_ts_ratio),
|
||
"bidir_inlier": float(min(in_st_ratio, in_ts_ratio)),
|
||
"bidir_rmse": float(rmse),
|
||
}
|
||
)
|
||
return stats
|
||
except Exception:
|
||
return stats
|
||
|
||
def _build_ref_match_visual(
|
||
live_ref_pts: Optional[np.ndarray],
|
||
pose_ref: Optional[np.ndarray],
|
||
now_t: float,
|
||
) -> Tuple[Optional[np.ndarray], Optional[np.ndarray], Dict[str, Any]]:
|
||
nonlocal ref_match_points_cache, ref_match_confirm_hits, ref_match_last_pub_t, ref_match_last_stats
|
||
stats: Dict[str, Any] = {"enabled": bool(loc_vis_enabled), "updated": False}
|
||
if not loc_vis_enabled:
|
||
ref_match_last_stats = stats
|
||
return None, None, stats
|
||
if ref_points_cache is None or len(ref_points_cache) == 0:
|
||
stats["reason"] = "no_ref"
|
||
ref_match_last_stats = stats
|
||
return None, None, stats
|
||
if (float(now_t) - float(ref_match_last_pub_t)) < float(loc_vis_period_sec):
|
||
return None, None, dict(ref_match_last_stats)
|
||
|
||
ref_match_last_pub_t = float(now_t)
|
||
if ref_match_points_cache is None or ref_match_confirm_hits is None:
|
||
ref_ds = voxel_downsample_np(np.asarray(ref_points_cache, dtype=np.float32), float(loc_vis_voxel_m))
|
||
if len(ref_ds) > int(loc_vis_max_points):
|
||
step = max(2, int(np.ceil(float(len(ref_ds)) / float(loc_vis_max_points))))
|
||
ref_ds = ref_ds[::step]
|
||
ref_match_points_cache = np.asarray(ref_ds, dtype=np.float32)
|
||
ref_match_confirm_hits = np.zeros((len(ref_match_points_cache),), dtype=np.int16)
|
||
|
||
pts_ref = np.asarray(ref_match_points_cache, dtype=np.float32)
|
||
if len(pts_ref) == 0:
|
||
stats["reason"] = "empty_ref_sample"
|
||
ref_match_last_stats = stats
|
||
return pts_ref, np.zeros((0, 4), dtype=np.float32), stats
|
||
|
||
local_mask = np.ones((len(pts_ref),), dtype=bool)
|
||
center = None
|
||
if pose_ref is not None:
|
||
pose_np = np.asarray(pose_ref, dtype=np.float64)
|
||
if pose_np.shape == (4, 4):
|
||
center = np.asarray(pose_np[:3, 3], dtype=np.float32)
|
||
if center is not None and np.isfinite(center).all():
|
||
rel = pts_ref - center.reshape((1, 3))
|
||
d2 = np.einsum("ij,ij->i", rel, rel)
|
||
local_mask = d2 <= (float(loc_vis_local_radius_m) * float(loc_vis_local_radius_m))
|
||
|
||
colors = np.zeros((len(pts_ref), 4), dtype=np.float32)
|
||
colors[:, :3] = np.array([0.45, 0.45, 0.45], dtype=np.float32)
|
||
colors[:, 3] = 0.16
|
||
|
||
live = None if live_ref_pts is None else np.asarray(live_ref_pts, dtype=np.float32)
|
||
matched = np.zeros((len(pts_ref),), dtype=bool)
|
||
near = np.zeros((len(pts_ref),), dtype=bool)
|
||
unmatched = np.zeros((len(pts_ref),), dtype=bool)
|
||
try:
|
||
if (live is not None and live.ndim == 2 and live.shape[1] == 3
|
||
and len(live) >= 20 and np.any(local_mask)
|
||
and _scipy_cKDTree is not None):
|
||
tree = _scipy_cKDTree(np.asarray(live, dtype=np.float64))
|
||
idx = np.where(local_mask)[0]
|
||
d, _ = tree.query(np.asarray(pts_ref[idx], dtype=np.float64), k=1)
|
||
d = np.asarray(d, dtype=np.float64)
|
||
match_idx = idx[d <= float(loc_vis_match_dist_m)]
|
||
near_idx = idx[(d > float(loc_vis_match_dist_m)) & (d <= float(loc_vis_near_dist_m))]
|
||
unmatch_idx = idx[d > float(loc_vis_near_dist_m)]
|
||
matched[match_idx] = True
|
||
near[near_idx] = True
|
||
unmatched[unmatch_idx] = True
|
||
except Exception as _vis_exc:
|
||
st("WARN", f"[loc_vis] match visual update failed: {_vis_exc}")
|
||
|
||
if ref_match_confirm_hits is None or len(ref_match_confirm_hits) != len(pts_ref):
|
||
ref_match_confirm_hits = np.zeros((len(pts_ref),), dtype=np.int16)
|
||
hits = np.asarray(ref_match_confirm_hits, dtype=np.int16)
|
||
hits[matched] = np.clip(hits[matched] + 1, 0, 32000)
|
||
decay_mask = local_mask & (~matched)
|
||
if np.any(decay_mask):
|
||
hits[decay_mask] = np.maximum(0, hits[decay_mask] - int(loc_vis_decay_per_frame))
|
||
confirmed = local_mask & (hits >= int(loc_vis_confirm_frames))
|
||
ref_match_confirm_hits = hits
|
||
|
||
colors[unmatched, :3] = np.array([0.95, 0.15, 0.15], dtype=np.float32)
|
||
colors[unmatched, 3] = 0.22
|
||
colors[near, :3] = np.array([0.96, 0.86, 0.16], dtype=np.float32)
|
||
colors[near, 3] = 0.24
|
||
colors[matched, :3] = np.array([0.15, 0.85, 0.20], dtype=np.float32)
|
||
colors[matched, 3] = 0.24
|
||
colors[confirmed, :3] = np.array([0.05, 1.00, 0.28], dtype=np.float32)
|
||
colors[confirmed, 3] = 0.34
|
||
|
||
local_n = int(np.count_nonzero(local_mask))
|
||
stats.update(
|
||
{
|
||
"updated": True,
|
||
"ref_points": int(len(pts_ref)),
|
||
"local_points": int(local_n),
|
||
"matched": int(np.count_nonzero(matched)),
|
||
"near": int(np.count_nonzero(near)),
|
||
"unmatched": int(np.count_nonzero(unmatched)),
|
||
"confirmed": int(np.count_nonzero(confirmed)),
|
||
"matched_ratio": float(np.count_nonzero(matched)) / max(1.0, float(local_n)),
|
||
}
|
||
)
|
||
ref_match_last_stats = dict(stats)
|
||
return pts_ref, colors, stats
|
||
|
||
def compute_map_lock_correction(
|
||
points_sensor: np.ndarray,
|
||
pose_map: Optional[np.ndarray],
|
||
now_t: float,
|
||
overrides: Optional[Dict[str, Any]] = None,
|
||
) -> Tuple[Optional[np.ndarray], Dict[str, Any]]:
|
||
nonlocal map_lock_last_t
|
||
info: Dict[str, Any] = {"status": "skip", "ran": False}
|
||
ov = overrides if isinstance(overrides, dict) else {}
|
||
period_sec_local = max(0.05, float(ov.get("period_sec", map_lock_period_sec)))
|
||
max_corr_local = max(0.20, float(ov.get("max_corr_m", map_lock_max_corr_m)))
|
||
accept_fit_local = float(np.clip(float(ov.get("accept_fitness", map_lock_accept_fitness)), 0.0, 1.0))
|
||
accept_rmse_local = max(0.05, float(ov.get("accept_rmse", map_lock_accept_rmse)))
|
||
max_step_trans_local = max(0.01, float(ov.get("max_step_translation_m", map_lock_max_trans_m)))
|
||
max_step_rot_local = max(0.5, float(ov.get("max_step_rotation_deg", map_lock_max_rot_deg)))
|
||
icp_iter_local = max(5, int(ov.get("icp_max_iter", map_lock_icp_max_iter)))
|
||
|
||
if (not map_lock_enabled) or (pose_map is None):
|
||
return None, info
|
||
if (now_t - float(map_lock_last_t)) < float(period_sec_local):
|
||
return None, info
|
||
map_lock_last_t = float(now_t)
|
||
|
||
try:
|
||
stable_all = stable.get_points()
|
||
except Exception:
|
||
stable_all = None
|
||
n_stable = 0 if stable_all is None else int(len(stable_all))
|
||
if stable_all is None or n_stable < int(map_lock_min_stable_pts):
|
||
info.update({"status": "skip", "reason": "few_stable", "stable_points": int(n_stable)})
|
||
return None, info
|
||
|
||
src_sensor = np.asarray(points_sensor, dtype=np.float32)
|
||
if src_sensor.ndim != 2 or src_sensor.shape[1] != 3 or len(src_sensor) < int(map_lock_min_live_pts):
|
||
info.update({"status": "skip", "reason": "few_live", "live_points": int(len(src_sensor))})
|
||
return None, info
|
||
|
||
pose_np = np.asarray(pose_map, dtype=np.float64)
|
||
if pose_np.shape != (4, 4):
|
||
info.update({"status": "skip", "reason": "bad_pose"})
|
||
return None, info
|
||
|
||
src_world = to_world_points(src_sensor, pose_np)
|
||
tgt_world = np.asarray(stable_all, dtype=np.float32)
|
||
|
||
if float(map_lock_window_radius_m) > 0.0 and len(tgt_world) > 0:
|
||
center = np.asarray(pose_np[:3, 3], dtype=np.float32)
|
||
rel = tgt_world - center
|
||
d2 = np.einsum("ij,ij->i", rel, rel)
|
||
win2 = float(map_lock_window_radius_m) * float(map_lock_window_radius_m)
|
||
keep = d2 <= win2
|
||
min_local = max(120, int(map_lock_min_stable_pts) // 3)
|
||
if int(np.count_nonzero(keep)) >= min_local:
|
||
tgt_world = tgt_world[keep]
|
||
|
||
src_ds = voxel_downsample_np(src_world, float(map_lock_voxel_m))
|
||
tgt_ds = voxel_downsample_np(tgt_world, float(map_lock_voxel_m))
|
||
if len(src_ds) < 60 or len(tgt_ds) < 120:
|
||
info.update(
|
||
{
|
||
"status": "skip",
|
||
"reason": "few_downsampled",
|
||
"src_points": int(len(src_ds)),
|
||
"tgt_points": int(len(tgt_ds)),
|
||
}
|
||
)
|
||
return None, info
|
||
|
||
# Robust in-process point-to-point ICP (avoids Open3D native crashes seen in map-lock path).
|
||
if _scipy_cKDTree is None:
|
||
info.update({"status": "skip", "reason": "scipy_missing"})
|
||
return None, info
|
||
|
||
src_icp = np.asarray(src_ds, dtype=np.float64)
|
||
tgt_icp = np.asarray(tgt_ds, dtype=np.float64)
|
||
tree = _scipy_cKDTree(tgt_icp)
|
||
max_corr = float(max_corr_local)
|
||
min_corr = max(40, int(0.15 * len(src_icp)))
|
||
|
||
def _rigid_fit(src_pts: np.ndarray, dst_pts: np.ndarray) -> Optional[np.ndarray]:
|
||
if len(src_pts) < 3 or len(dst_pts) < 3:
|
||
return None
|
||
c_src = np.mean(src_pts, axis=0)
|
||
c_dst = np.mean(dst_pts, axis=0)
|
||
src0 = src_pts - c_src
|
||
dst0 = dst_pts - c_dst
|
||
H = src0.T @ dst0
|
||
try:
|
||
U, _, Vt = np.linalg.svd(H, full_matrices=False)
|
||
except Exception:
|
||
return None
|
||
R = Vt.T @ U.T
|
||
if np.linalg.det(R) < 0:
|
||
Vt[-1, :] *= -1.0
|
||
R = Vt.T @ U.T
|
||
t = c_dst - (R @ c_src)
|
||
T = np.eye(4, dtype=np.float64)
|
||
T[:3, :3] = R
|
||
T[:3, 3] = t
|
||
return T
|
||
|
||
corr = np.eye(4, dtype=np.float64)
|
||
converged = False
|
||
for _ in range(int(icp_iter_local)):
|
||
src_w = (src_icp @ corr[:3, :3].T) + corr[:3, 3]
|
||
dist, idx = tree.query(src_w, k=1)
|
||
mask = np.isfinite(dist) & (dist <= max_corr)
|
||
if int(np.count_nonzero(mask)) < min_corr:
|
||
break
|
||
src_in = src_w[mask]
|
||
dst_in = tgt_icp[idx[mask]]
|
||
delta = _rigid_fit(src_in, dst_in)
|
||
if delta is None:
|
||
break
|
||
corr = delta @ corr
|
||
step_t = float(np.linalg.norm(delta[:3, 3]))
|
||
step_r = float(
|
||
np.degrees(
|
||
np.arccos(np.clip((np.trace(delta[:3, :3]) - 1.0) * 0.5, -1.0, 1.0))
|
||
)
|
||
)
|
||
if step_t < 0.001 and step_r < 0.08:
|
||
converged = True
|
||
break
|
||
|
||
src_final = (src_icp @ corr[:3, :3].T) + corr[:3, 3]
|
||
dist_f, _ = tree.query(src_final, k=1)
|
||
mask_f = np.isfinite(dist_f) & (dist_f <= max_corr)
|
||
inliers = int(np.count_nonzero(mask_f))
|
||
if inliers < min_corr:
|
||
info.update(
|
||
{
|
||
"ran": True,
|
||
"status": "rejected",
|
||
"reason": "few_inliers",
|
||
"src_points": int(len(src_icp)),
|
||
"tgt_points": int(len(tgt_icp)),
|
||
}
|
||
)
|
||
return None, info
|
||
fit = float(inliers) / float(max(1, len(src_icp)))
|
||
rmse = float(np.sqrt(np.mean((dist_f[mask_f]) ** 2)))
|
||
dpos = float(np.linalg.norm(corr[:3, 3]))
|
||
drot = float(np.degrees(np.arccos(np.clip((np.trace(corr[:3, :3]) - 1.0) * 0.5, -1.0, 1.0))))
|
||
|
||
ok = bool(
|
||
fit >= float(accept_fit_local)
|
||
and rmse <= float(accept_rmse_local)
|
||
and dpos <= float(max_step_trans_local)
|
||
and drot <= float(max_step_rot_local)
|
||
)
|
||
info.update(
|
||
{
|
||
"ran": True,
|
||
"status": "accepted" if ok else "rejected",
|
||
"fitness": fit,
|
||
"rmse": rmse,
|
||
"dpos_m": dpos,
|
||
"drot_deg": drot,
|
||
"src_points": int(len(src_ds)),
|
||
"tgt_points": int(len(tgt_ds)),
|
||
"converged": bool(converged),
|
||
"inliers": int(inliers),
|
||
"period_sec": float(period_sec_local),
|
||
"accept_fit": float(accept_fit_local),
|
||
"accept_rmse": float(accept_rmse_local),
|
||
}
|
||
)
|
||
return (corr if ok else None), info
|
||
|
||
def check_frame_continuity(points_world: np.ndarray) -> Tuple[bool, Dict[str, Any], Optional[np.ndarray]]:
|
||
info: Dict[str, Any] = {"enabled": bool(continuity_enabled), "status": "skip"}
|
||
pts = np.asarray(points_world, dtype=np.float32)
|
||
pts_ds = voxel_downsample_np(pts, float(continuity_voxel_m))
|
||
if (not continuity_enabled) or pts_ds is None or len(pts_ds) < int(continuity_min_points):
|
||
info.update({"status": "skip", "reason": "disabled_or_few_points", "points": int(len(pts_ds))})
|
||
return True, info, pts_ds
|
||
if continuity_prev_pts is None or len(continuity_prev_pts) < int(continuity_min_points):
|
||
info.update({"status": "bootstrap", "points": int(len(pts_ds))})
|
||
return True, info, pts_ds
|
||
|
||
if _scipy_cKDTree is None:
|
||
info.update({"status": "skip", "reason": "scipy_missing"})
|
||
return True, info, pts_ds
|
||
try:
|
||
tree = _scipy_cKDTree(np.asarray(continuity_prev_pts, dtype=np.float64))
|
||
dist, _ = tree.query(np.asarray(pts_ds, dtype=np.float64), k=1)
|
||
except Exception as e:
|
||
info.update({"status": "skip", "reason": f"tree_error:{e}"})
|
||
return True, info, pts_ds
|
||
|
||
mask = np.isfinite(dist)
|
||
if int(np.count_nonzero(mask)) < int(continuity_min_points):
|
||
info.update({"status": "skip", "reason": "few_matches"})
|
||
return True, info, pts_ds
|
||
d = dist[mask]
|
||
inlier_ratio = float(np.mean(d <= float(continuity_match_radius_m)))
|
||
median_d = float(np.median(d))
|
||
p90_d = float(np.percentile(d, 90.0))
|
||
ok = bool(
|
||
inlier_ratio >= float(continuity_min_inlier_ratio)
|
||
and median_d <= float(continuity_max_median_m)
|
||
and p90_d <= float(continuity_max_p90_m)
|
||
)
|
||
info.update(
|
||
{
|
||
"status": "ok" if ok else "reject",
|
||
"points": int(len(pts_ds)),
|
||
"inlier_ratio": float(inlier_ratio),
|
||
"median_m": float(median_d),
|
||
"p90_m": float(p90_d),
|
||
}
|
||
)
|
||
return ok, info, pts_ds
|
||
|
||
def cleanup_map_islands(points_world: np.ndarray) -> Tuple[np.ndarray, Dict[str, Any]]:
|
||
pts = np.asarray(points_world, dtype=np.float32)
|
||
info: Dict[str, Any] = {
|
||
"enabled": bool(cleanup_enabled),
|
||
"status": "skip",
|
||
"before": int(len(pts)),
|
||
"after": int(len(pts)),
|
||
"removed": 0,
|
||
}
|
||
if (not cleanup_enabled) or len(pts) < int(cleanup_min_total_points):
|
||
info["reason"] = "disabled_or_few_points"
|
||
return pts, info
|
||
|
||
pts_ds = voxel_downsample_np(pts, float(cleanup_voxel_m))
|
||
if len(pts_ds) < int(cleanup_min_cluster_points):
|
||
info["reason"] = "few_downsampled"
|
||
return pts, info
|
||
|
||
if _scipy_cKDTree is None:
|
||
info["reason"] = "scipy_missing"
|
||
return pts, info
|
||
try:
|
||
tree = _scipy_cKDTree(np.asarray(pts_ds, dtype=np.float64))
|
||
except Exception as e:
|
||
info["reason"] = f"tree_error:{e}"
|
||
return pts, info
|
||
|
||
n = int(len(pts_ds))
|
||
visited = np.zeros((n,), dtype=bool)
|
||
comp_id = np.full((n,), -1, dtype=np.int32)
|
||
sizes: List[int] = []
|
||
|
||
for i in range(n):
|
||
if visited[i]:
|
||
continue
|
||
q = [i]
|
||
visited[i] = True
|
||
members: List[int] = []
|
||
while q:
|
||
j = int(q.pop())
|
||
members.append(j)
|
||
neigh = tree.query_ball_point(pts_ds[j], r=float(cleanup_cluster_radius_m))
|
||
for k in neigh:
|
||
kk = int(k)
|
||
if not visited[kk]:
|
||
visited[kk] = True
|
||
q.append(kk)
|
||
cid = len(sizes)
|
||
sizes.append(len(members))
|
||
for m in members:
|
||
comp_id[m] = int(cid)
|
||
|
||
if not sizes:
|
||
info["reason"] = "no_components"
|
||
return pts, info
|
||
|
||
order = sorted(range(len(sizes)), key=lambda c: int(sizes[c]), reverse=True)
|
||
keep_ids = set(order[: int(cleanup_keep_largest_n)])
|
||
if not cleanup_strict_largest_only:
|
||
for cid, sz in enumerate(sizes):
|
||
if int(sz) >= int(cleanup_min_cluster_points):
|
||
keep_ids.add(int(cid))
|
||
if len(keep_ids) == len(sizes):
|
||
info.update({"status": "ok", "components": int(len(sizes)), "kept_components": int(len(keep_ids))})
|
||
return pts, info
|
||
|
||
# Map original points to nearest downsampled component and keep only selected components.
|
||
_, nn = tree.query(np.asarray(pts, dtype=np.float64), k=1)
|
||
keep_mask = np.isin(comp_id[np.asarray(nn, dtype=np.int32)], np.asarray(list(keep_ids), dtype=np.int32))
|
||
cleaned = pts[keep_mask]
|
||
removed = int(len(pts) - len(cleaned))
|
||
info.update(
|
||
{
|
||
"status": "ok",
|
||
"components": int(len(sizes)),
|
||
"kept_components": int(len(keep_ids)),
|
||
"before": int(len(pts)),
|
||
"after": int(len(cleaned)),
|
||
"removed": int(removed),
|
||
}
|
||
)
|
||
return cleaned, info
|
||
|
||
def push_loc_state() -> None:
|
||
st("INFO", {"LOC_STATE": loc_state.snapshot()})
|
||
if diagnostics is not None:
|
||
diagnostics.log_state("LOC_STATE", _worker_diag_state())
|
||
|
||
def push_worker_mode() -> None:
|
||
st(
|
||
"INFO",
|
||
{
|
||
"MODE": {
|
||
"mode": str(worker_mode),
|
||
"mapping": bool(mapping_enabled),
|
||
"localize_only": bool(localize_only_enabled),
|
||
}
|
||
},
|
||
)
|
||
if diagnostics is not None:
|
||
diagnostics.log_state("MODE", _worker_diag_state())
|
||
|
||
def _place_profile_allowed() -> bool:
|
||
profile = str(stability_profile).upper().strip()
|
||
return bool(place_recog.profile_allowed(profile))
|
||
|
||
# ---------------- loop closure ----------------
|
||
def _save_loop_keyframe(pose_xy: Tuple[float, float], now_t: float) -> None:
|
||
"""Append a keyframe if the robot has moved >= keyframe_dist_m
|
||
from the last one. Cap the list at _lc_keyframe_max via FIFO."""
|
||
nonlocal _lc_keyframes
|
||
if not _lc_enabled:
|
||
return
|
||
if _lc_keyframes:
|
||
lx, ly, _ = _lc_keyframes[-1]
|
||
dx = float(pose_xy[0]) - lx
|
||
dy = float(pose_xy[1]) - ly
|
||
if (dx * dx + dy * dy) < (_lc_keyframe_dist_m * _lc_keyframe_dist_m):
|
||
return
|
||
_lc_keyframes.append((float(pose_xy[0]), float(pose_xy[1]), float(now_t)))
|
||
# FIFO trim — bound memory + keep recent history relevant.
|
||
if len(_lc_keyframes) > int(_lc_keyframe_max):
|
||
_lc_keyframes[:] = _lc_keyframes[-int(_lc_keyframe_max):]
|
||
|
||
def _check_loop_closure(pose_world: np.ndarray, now_t: float,
|
||
current_scan_sensor) -> None:
|
||
"""Periodic revisit detection: when current pose is within
|
||
match_radius of a non-recent keyframe, run ICP between the
|
||
current LiDAR scan (in world frame) and a local crop of the
|
||
stable buffer around the keyframe. If fitness is high and the
|
||
ICP transform implies a drift correction >= min_drift_m, snap
|
||
the current pose forward by that correction.
|
||
|
||
`current_scan_sensor` is the body-frame point array — passed
|
||
in explicitly because this helper lives at the worker's outer
|
||
scope and can't close over process_points's local `pts`.
|
||
|
||
This is the basic single-shot "snap forward" version — does
|
||
NOT back-propagate the correction through the trajectory.
|
||
Sufficient for sub-2m drift on office-sized maps. For full
|
||
graph-optimised loop closure we'd need a pose graph + g2o.
|
||
"""
|
||
nonlocal _lc_last_check_t, _lc_closures_applied, _lc_last_closure_info
|
||
nonlocal last_localize_transform, slam_to_ref_transform, slam_to_ref_valid
|
||
nonlocal map_lock_consecutive_rejects, map_lock_consecutive_safe_rejects
|
||
nonlocal map_lock_block_on_reject, last_localize_t
|
||
|
||
if not _lc_enabled:
|
||
return
|
||
if (now_t - float(_lc_last_check_t)) < float(_lc_check_interval):
|
||
return
|
||
_lc_last_check_t = now_t
|
||
if pose_world is None:
|
||
return
|
||
if not _lc_keyframes:
|
||
return
|
||
|
||
try:
|
||
pose_arr = np.asarray(pose_world, dtype=np.float64)
|
||
if pose_arr.shape != (4, 4):
|
||
return
|
||
rx = float(pose_arr[0, 3])
|
||
ry = float(pose_arr[1, 3])
|
||
except Exception:
|
||
return
|
||
|
||
# Find the OLDEST keyframe within match_radius that's also old
|
||
# enough (>= min_keyframe_age_sec). Oldest = most drift to
|
||
# correct against. Skip the K most-recent keyframes always.
|
||
best_kf = None
|
||
best_age = -1.0
|
||
for (kx, ky, kt) in _lc_keyframes[:-3]: # always skip 3 most recent
|
||
age = now_t - kt
|
||
if age < _lc_min_age_sec:
|
||
continue
|
||
dx = rx - kx
|
||
dy = ry - ky
|
||
if (dx * dx + dy * dy) > (_lc_match_radius_m * _lc_match_radius_m):
|
||
continue
|
||
if age > best_age:
|
||
best_age = age
|
||
best_kf = (kx, ky, kt)
|
||
if best_kf is None:
|
||
return
|
||
|
||
# Get the local stable-buffer crop around the keyframe.
|
||
stable_now = stable.get_save_points()
|
||
if stable_now is None or len(stable_now) < 200:
|
||
return
|
||
sn = np.asarray(stable_now, dtype=np.float64)
|
||
kx, ky, kt = best_kf
|
||
crop_r2 = (_lc_match_radius_m * 1.5) ** 2 # slightly bigger crop than match radius
|
||
dxsq = (sn[:, 0] - kx) ** 2 + (sn[:, 1] - ky) ** 2
|
||
keep = dxsq <= crop_r2
|
||
tgt = sn[keep]
|
||
if len(tgt) < 200:
|
||
return
|
||
|
||
# Source: current scan in world frame, voxel-downsampled.
|
||
try:
|
||
src_world = to_world_points(current_scan_sensor, pose_arr)
|
||
except Exception:
|
||
return
|
||
if src_world is None or len(src_world) < 200:
|
||
return
|
||
src_ds = voxel_downsample_np(np.asarray(src_world, dtype=np.float32), _lc_icp_voxel)
|
||
tgt_ds = voxel_downsample_np(np.asarray(tgt, dtype=np.float32), _lc_icp_voxel)
|
||
if len(src_ds) < 100 or len(tgt_ds) < 100:
|
||
return
|
||
|
||
# Run ICP. Seed = identity (we already have an estimate of
|
||
# where we are; the ICP correction is small).
|
||
reg = _icp_point_to_point_np(
|
||
np.asarray(src_ds, dtype=np.float64),
|
||
np.asarray(tgt_ds, dtype=np.float64),
|
||
init_tf=np.eye(4, dtype=np.float64),
|
||
max_corr_m=float(_lc_icp_max_corr),
|
||
max_iter=int(_lc_icp_max_iter),
|
||
min_corr=40,
|
||
)
|
||
fit = float(reg.get("fitness", 0.0))
|
||
rmse = float(reg.get("rmse", 9e9))
|
||
T = np.asarray(reg.get("transform", np.eye(4)), dtype=np.float64)
|
||
# Drift = ||T.translation||
|
||
drift = float(np.linalg.norm(T[:3, 3]))
|
||
|
||
if fit < _lc_min_fitness:
|
||
return
|
||
if drift < _lc_min_drift:
|
||
# Within noise — don't snap; nothing meaningful to correct.
|
||
return
|
||
if drift > _lc_max_drift:
|
||
# Too big — likely a false positive (robot relocalized to
|
||
# a similar-looking but distant area). Reject.
|
||
st("WARN", {"LOOP_CLOSURE": {"event": "rejected_huge_drift",
|
||
"drift_m": drift, "fit": fit}})
|
||
return
|
||
|
||
# Apply the correction: pose_new = T @ pose_old.
|
||
# This is the "snap forward" — current pose shifts to align
|
||
# with the historic keyframe's local map. Future scans will
|
||
# accumulate from the corrected pose.
|
||
try:
|
||
pose_corrected = T @ pose_arr
|
||
# Wire the correction back into the SLAM stack. The
|
||
# LocalizationFrameState owns odom_to_map_transform —
|
||
# apply_map_correction is the canonical entry point.
|
||
loc_frames.apply_map_correction(T)
|
||
_sync_loc_vars_from_service()
|
||
# Update last_key_pose so the next frame's pose-delta
|
||
# check doesn't fire spuriously.
|
||
# (last_key_pose / last_guard_pose ARE corrected via
|
||
# nonlocal sync done by _sync_loc_vars_from_service in
|
||
# other paths — but here we're outside that flow, so do
|
||
# the minimum and let the next normal tick re-sync.)
|
||
_lc_closures_applied += 1
|
||
_lc_last_closure_info = {
|
||
"ts": float(now_t),
|
||
"fit": fit,
|
||
"rmse": rmse,
|
||
"drift_m": drift,
|
||
"keyframe_age_sec": float(best_age),
|
||
"applied_count": int(_lc_closures_applied),
|
||
}
|
||
# Reset map_lock counters since we just snapped — the next
|
||
# tick should see a clean alignment.
|
||
map_lock_consecutive_rejects = 0
|
||
map_lock_consecutive_safe_rejects = 0
|
||
st("INFO", {"LOOP_CLOSURE": {"event": "applied",
|
||
"fit": fit,
|
||
"rmse": rmse,
|
||
"drift_m": drift,
|
||
"keyframe_age_sec": float(best_age),
|
||
"total": int(_lc_closures_applied)}})
|
||
except Exception as _e:
|
||
st("WARN", {"LOOP_CLOSURE": {"event": "apply_failed",
|
||
"error": f"{type(_e).__name__}: {_e}"}})
|
||
|
||
# ---------------- localization ----------------
|
||
def do_localize(
|
||
force: bool = False,
|
||
source: str = "MANUAL",
|
||
src_override: Optional[np.ndarray] = None,
|
||
) -> Optional[Dict[str, Any]]:
|
||
nonlocal last_localize_t, last_localize_points
|
||
nonlocal ref_pcd_cache, ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n, ref_cache_path
|
||
nonlocal approx_pose_guess_tf, approx_pose_guess_until_t, approx_pose_guess_fail_count, approx_pose_guess_success_count
|
||
nonlocal last_loc_icp_result
|
||
now = _now()
|
||
src_tag = str(source).upper().strip()
|
||
use_live_src = src_override is not None
|
||
if not loc_cfg.enabled:
|
||
if force:
|
||
st("ERROR", "LOCALIZE disabled in config.")
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
if not ref_map_path:
|
||
if force:
|
||
st("ERROR", "LOCALIZE: load a reference map first.")
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
|
||
try:
|
||
period = max(0.1, float(loc_cfg.period_sec))
|
||
if use_live_src:
|
||
# Live relocalization should be frequent but not every frame to avoid jitter.
|
||
period = min(period, float(loc_live_period_sec))
|
||
if (not force) and ((now - last_localize_t) < period):
|
||
return None
|
||
|
||
req_loc = int(loc_cfg.min_points_for_localize)
|
||
if use_live_src:
|
||
cur = np.asarray(src_override, dtype=np.float32)
|
||
cur_n = int(len(cur))
|
||
req_live = max(80, min(req_loc, 1000))
|
||
if cur_n < req_live:
|
||
if force:
|
||
st("ERROR", f"LOCALIZE: not enough live points ({cur_n}/{req_live}).")
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
else:
|
||
cur = stable.get_save_points()
|
||
cur_n = 0 if cur is None else int(len(cur))
|
||
if cur is None or cur_n < req_loc:
|
||
if force:
|
||
st("ERROR", f"LOCALIZE: not enough stable points ({cur_n}/{req_loc}). Start mapping first.")
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
if (not force) and ((cur_n - int(last_localize_points)) < int(loc_cfg.min_new_points)):
|
||
return None
|
||
|
||
import open3d as o3d
|
||
|
||
if ref_pcd_cache is None or ref_cache_path != ref_map_path:
|
||
ref_pcd_cache = o3d.io.read_point_cloud(ref_map_path)
|
||
if hasattr(ref_pcd_cache, "points"):
|
||
ref_points_cache = np.asarray(ref_pcd_cache.points, dtype=np.float32).copy()
|
||
else:
|
||
ref_points_cache = np.asarray(ref_pcd_cache, dtype=np.float32).reshape((-1, 3)).copy()
|
||
ref_nav_points_cache = None
|
||
ref_nav_cache_src_n = 0
|
||
ref_cache_path = ref_map_path
|
||
if ref_points_cache is None:
|
||
if hasattr(ref_pcd_cache, "points"):
|
||
ref_points_cache = np.asarray(ref_pcd_cache.points, dtype=np.float32).copy()
|
||
else:
|
||
ref_points_cache = np.asarray(ref_pcd_cache, dtype=np.float32).reshape((-1, 3)).copy()
|
||
ref_nav_points_cache = None
|
||
ref_nav_cache_src_n = 0
|
||
ref_np_full = np.asarray(ref_points_cache, dtype=np.float32)
|
||
if len(ref_np_full) < int(loc_cfg.min_points_for_localize):
|
||
if force:
|
||
st("ERROR", "LOCALIZE: reference map has too few points.")
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
if bool(place_cfg.enabled) and _place_profile_allowed():
|
||
if (not bool(place_recog.index_ready)) or (str(place_recog.ref_path or "") != str(ref_map_path)):
|
||
pr_info = place_recog.build(ref_np_full, ref_map_path)
|
||
st("INFO", {"PLACE_RECOG": pr_info})
|
||
|
||
cur_np_full = np.asarray(cur, dtype=np.float32)
|
||
if cur_np_full.ndim != 2 or cur_np_full.shape[1] != 3:
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
|
||
# Once localized, keep tracking in a local window around the last pose
|
||
# to avoid jumping to similar structures elsewhere in a large map.
|
||
tracking_locked = bool(
|
||
use_live_src
|
||
and slam_to_ref_valid
|
||
and src_tag not in ("ON_LOAD", "RECOVERY")
|
||
and str(loc_state.state).upper().strip() in ("TRACKING", "DEGRADED")
|
||
)
|
||
ref_np_track = np.asarray(ref_np_full, dtype=np.float32)
|
||
tracking_window_used = False
|
||
tracking_window_reason = "full_ref"
|
||
if tracking_locked and loc_tracking_window_enabled and len(ref_np_track) > 0:
|
||
try:
|
||
center = np.asarray(last_localize_transform[:3, 3], dtype=np.float32)
|
||
if np.isfinite(center).all():
|
||
rel = ref_np_track - center
|
||
d2 = np.einsum("ij,ij->i", rel, rel)
|
||
win2 = float(loc_tracking_window_radius_m) * float(loc_tracking_window_radius_m)
|
||
keep = d2 <= win2
|
||
n_keep = int(np.count_nonzero(keep))
|
||
if n_keep >= int(loc_tracking_window_min_ref_points):
|
||
ref_np_track = ref_np_track[keep]
|
||
tracking_window_used = True
|
||
tracking_window_reason = "window"
|
||
else:
|
||
tracking_window_reason = "window_too_few"
|
||
except Exception:
|
||
tracking_window_reason = "window_error"
|
||
if len(ref_np_track) > int(loc_tracking_window_max_ref_points):
|
||
step = max(2, int(np.ceil(float(len(ref_np_track)) / float(loc_tracking_window_max_ref_points))))
|
||
ref_np_track = ref_np_track[::step]
|
||
if tracking_window_used:
|
||
tracking_window_reason = "window_decimated"
|
||
else:
|
||
tracking_window_reason = "full_ref_decimated"
|
||
|
||
voxel = float(loc_cfg.voxel_localize)
|
||
if use_live_src:
|
||
voxel = min(voxel, 0.16)
|
||
max_corr = max(0.5, float(loc_cfg.max_corr_mult) * voxel)
|
||
if use_live_src:
|
||
max_corr = min(max_corr, 0.95)
|
||
accept_fit_target = float(loc_cfg.accept_fitness)
|
||
accept_rmse_target = float(loc_cfg.accept_rmse)
|
||
guess_bootstrap = bool(
|
||
approx_pose_guess_tf is not None
|
||
and float(now) <= float(approx_pose_guess_until_t)
|
||
and (not slam_to_ref_valid)
|
||
)
|
||
guess_strict_local_active = bool(
|
||
guess_bootstrap and (not slam_to_ref_valid) and bool(loc_guess_strict_local)
|
||
)
|
||
if use_live_src:
|
||
# Slightly stricter acceptance in live mode to reduce wobble while turning.
|
||
accept_fit_target = max(0.34, min(0.98, accept_fit_target + 0.08))
|
||
accept_rmse_target = min(accept_rmse_target, 0.26)
|
||
if guess_bootstrap:
|
||
# If user provided approximate location, allow slight environmental changes.
|
||
max_corr = min(1.15, max_corr * 1.15)
|
||
accept_fit_target = max(0.22, accept_fit_target - 0.04)
|
||
accept_rmse_target = max(accept_rmse_target, 0.34)
|
||
|
||
init = np.array(last_localize_transform, dtype=np.float64, copy=True)
|
||
approx_guess_used = False
|
||
approx_guess_active = bool(guess_bootstrap)
|
||
if approx_guess_active and not slam_to_ref_valid:
|
||
init = np.asarray(approx_pose_guess_tf, dtype=np.float64).copy()
|
||
approx_guess_used = True
|
||
# Bootstrap with local reference crop around approximate click.
|
||
try:
|
||
center_guess = np.asarray(init[:3, 3], dtype=np.float32)
|
||
rel = ref_np_track - center_guess.reshape((1, 3))
|
||
d2 = np.einsum("ij,ij->i", rel, rel)
|
||
local_r2 = float(loc_guess_local_radius_m) * float(loc_guess_local_radius_m)
|
||
wide_r2 = float(loc_guess_crop_radius_m) * float(loc_guess_crop_radius_m)
|
||
keep_local = d2 <= local_r2
|
||
n_local = int(np.count_nonzero(keep_local))
|
||
min_local_n = int(loc_tracking_window_min_ref_points)
|
||
if n_local >= min_local_n:
|
||
ref_np_track = ref_np_track[keep_local]
|
||
tracking_window_used = True
|
||
tracking_window_reason = "approx_local_crop"
|
||
else:
|
||
keep_wide = d2 <= wide_r2
|
||
n_wide = int(np.count_nonzero(keep_wide))
|
||
min_wide_n = max(120, min_local_n // 2) if guess_strict_local_active else min_local_n
|
||
if n_wide >= min_wide_n:
|
||
ref_np_track = ref_np_track[keep_wide]
|
||
tracking_window_used = True
|
||
tracking_window_reason = "approx_wide_crop"
|
||
else:
|
||
tracking_window_reason = "approx_crop_too_few"
|
||
except Exception:
|
||
tracking_window_reason = "approx_crop_error"
|
||
|
||
src_d = voxel_downsample_np(cur_np_full, voxel)
|
||
tgt_d = voxel_downsample_np(ref_np_track, voxel)
|
||
if len(src_d) < 40 or len(tgt_d) < 40:
|
||
if force:
|
||
st("ERROR", "LOCALIZE: not enough downsampled points for ICP.")
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
return None
|
||
|
||
# Coarse-to-fine ICP improves convergence when the initial offset is large.
|
||
coarse_voxel = max(voxel, 0.35)
|
||
src_c = voxel_downsample_np(cur_np_full, coarse_voxel)
|
||
coarse_tgt_src = ref_np_track if (tracking_locked or approx_guess_used) else ref_np_full
|
||
tgt_c = voxel_downsample_np(coarse_tgt_src, coarse_voxel)
|
||
def _run_local_icp_chain(init_tf: np.ndarray) -> Dict[str, Any]:
|
||
seed = np.asarray(init_tf, dtype=np.float64).copy()
|
||
if len(src_c) > 40 and len(tgt_c) > 40:
|
||
coarse = _icp_point_to_point_np(
|
||
src_c,
|
||
tgt_c,
|
||
seed,
|
||
max_corr_m=max_corr * 1.6,
|
||
max_iter=max(15, int(loc_cfg.icp_max_iter) // 2),
|
||
min_corr=30,
|
||
)
|
||
seed = np.array(coarse.get("transform", seed), dtype=np.float64, copy=True)
|
||
return _icp_point_to_point_np(
|
||
src_d,
|
||
tgt_d,
|
||
seed,
|
||
max_corr_m=max_corr,
|
||
max_iter=max(10, int(loc_cfg.icp_max_iter)),
|
||
min_corr=35,
|
||
)
|
||
|
||
# ── Multi-scale ICP fallback ─────────────────────────────
|
||
# Standard run at the configured voxel size first. If
|
||
# fitness comes in below ~85% of accept_fit_target, fall
|
||
# back to two coarser passes (1.5×, 2.5× voxel) — larger
|
||
# basins of convergence catch the "ICP got stuck in a local
|
||
# minimum" failure mode that triggers map_lock cascades
|
||
# downstream. Inspired by FAST_LIO_LOCALIZATION_HUMANOID's
|
||
# multi-scale init (basic / s1 / s2 voxels).
|
||
#
|
||
# Closure-safety: every variable the helper depends on is
|
||
# bound here as a default argument so a future refactor
|
||
# that hoists the helper out of `do_localize` (or that adds
|
||
# an early return up-stream) can't NameError on a missing
|
||
# `accept_fit_target` / `cur_np_full` etc.
|
||
def _run_multi_scale_icp(
|
||
seed: np.ndarray,
|
||
_fit_target_v: float = float(accept_fit_target) * 0.85,
|
||
_voxel_v: float = float(voxel),
|
||
_max_corr_v: float = float(max_corr),
|
||
_icp_max_iter_v: int = int(loc_cfg.icp_max_iter),
|
||
_cur_v: np.ndarray = cur_np_full,
|
||
_tgt_full_v: np.ndarray = ref_np_full,
|
||
_tgt_win_v: np.ndarray = ref_np_track,
|
||
_use_window: bool = bool(tracking_locked or approx_guess_used),
|
||
) -> Dict[str, Any]:
|
||
_best = _run_local_icp_chain(seed)
|
||
if float(_best.get("fitness", 0.0)) >= _fit_target_v:
|
||
return _best
|
||
_tgt_src = _tgt_win_v if _use_window else _tgt_full_v
|
||
for _scale in (1.5, 2.5):
|
||
_v_coarse = _voxel_v * _scale
|
||
_src_s = voxel_downsample_np(_cur_v, _v_coarse)
|
||
_tgt_s = voxel_downsample_np(_tgt_src, _v_coarse)
|
||
if len(_src_s) < 40 or len(_tgt_s) < 40:
|
||
continue
|
||
_rr = _icp_point_to_point_np(
|
||
np.asarray(_src_s, dtype=np.float64),
|
||
np.asarray(_tgt_s, dtype=np.float64),
|
||
seed,
|
||
max_corr_m=_max_corr_v * (1.2 * _scale),
|
||
max_iter=max(10, _icp_max_iter_v),
|
||
min_corr=30,
|
||
)
|
||
_better = (
|
||
float(_rr.get("fitness", 0.0)) > float(_best.get("fitness", 0.0))
|
||
) or (
|
||
float(_rr.get("fitness", 0.0)) == float(_best.get("fitness", 0.0))
|
||
and float(_rr.get("rmse", 9e9)) < float(_best.get("rmse", 9e9))
|
||
)
|
||
if _better:
|
||
_best = _rr
|
||
# Early-out: if a coarser run hit target, stop.
|
||
if float(_best.get("fitness", 0.0)) >= _fit_target_v:
|
||
break
|
||
return _best
|
||
|
||
_t_icp_start = _now()
|
||
reg = _run_multi_scale_icp(init)
|
||
_icp_ms_delay = float((_now() - _t_icp_start) * 1000.0)
|
||
yaw_hypotheses = 0
|
||
place_query: Dict[str, Any] = {"ok": False, "reason": "disabled"}
|
||
if approx_guess_active and not slam_to_ref_valid and float(loc_guess_yaw_step_deg) < 180.0:
|
||
base_yaw = float(_yaw_deg_from_tf(init))
|
||
yaw_offsets = np.arange(-180.0, 180.0, float(loc_guess_yaw_step_deg), dtype=np.float64)
|
||
best_reg = dict(reg)
|
||
for yaw_off in yaw_offsets:
|
||
if abs(float(yaw_off)) < 1e-6:
|
||
continue
|
||
cand = np.asarray(init, dtype=np.float64).copy()
|
||
rz = _tf_from_xyzyaw(0.0, 0.0, 0.0, base_yaw + float(yaw_off))
|
||
cand[:3, :3] = np.asarray(rz[:3, :3], dtype=np.float64)
|
||
rr = _run_local_icp_chain(cand)
|
||
yaw_hypotheses += 1
|
||
better = (float(rr.get("fitness", 0.0)) > float(best_reg.get("fitness", 0.0))) or (
|
||
float(rr.get("fitness", 0.0)) == float(best_reg.get("fitness", 0.0))
|
||
and float(rr.get("rmse", 9e9)) < float(best_reg.get("rmse", 9e9))
|
||
)
|
||
if better:
|
||
best_reg = rr
|
||
reg = dict(best_reg)
|
||
used_recovery = False
|
||
|
||
def accepted(rr: Dict[str, Any]) -> bool:
|
||
return bool(
|
||
float(rr.get("fitness", 0.0)) >= float(accept_fit_target)
|
||
and float(rr.get("rmse", 9e9)) <= float(accept_rmse_target)
|
||
)
|
||
|
||
# Global relocalization fallback:
|
||
# only for bootstrap/lost states, not for normal LIVE tracking.
|
||
allow_global_reloc = bool(
|
||
(not slam_to_ref_valid)
|
||
or src_tag in ("ON_LOAD", "RECOVERY")
|
||
or str(loc_state.state).upper().strip() in ("LOST", "RECOVERY")
|
||
)
|
||
global_reloc_blocked_by_guess = False
|
||
if guess_strict_local_active and (not slam_to_ref_valid):
|
||
if int(approx_pose_guess_fail_count) < int(loc_guess_global_fallback_after_failures):
|
||
allow_global_reloc = False
|
||
global_reloc_blocked_by_guess = True
|
||
if (not accepted(reg)) and allow_global_reloc:
|
||
src_np = np.asarray(src_d, dtype=np.float64)
|
||
# Search against the full reference map to recover from wrong initial position.
|
||
tgt_global = voxel_downsample_np(ref_np_full, max(float(voxel), 0.24))
|
||
tgt_np = np.asarray(tgt_global, dtype=np.float64)
|
||
if len(src_np) > 20 and len(tgt_np) > 40:
|
||
c_src = np.mean(src_np, axis=0)
|
||
anchors = np.zeros((0, 3), dtype=np.float64)
|
||
use_place_retrieval = bool(place_cfg.enabled and _place_profile_allowed())
|
||
if use_place_retrieval:
|
||
approx_center = None
|
||
if approx_pose_guess_tf is not None:
|
||
try:
|
||
approx_center = np.asarray(approx_pose_guess_tf[:3, 3], dtype=np.float32)
|
||
except Exception:
|
||
approx_center = None
|
||
place_query = place_recog.query(cur_np_full, approx_center=approx_center)
|
||
if bool(place_query.get("ok", False)):
|
||
anchors = np.asarray(
|
||
[c.get("center", [0.0, 0.0, 0.0]) for c in place_query.get("candidates", [])],
|
||
dtype=np.float64,
|
||
).reshape((-1, 3))
|
||
if len(anchors) == 0:
|
||
anchors = voxel_downsample_np(
|
||
np.asarray(tgt_np, dtype=np.float32),
|
||
max(float(loc_global_reloc_anchor_voxel_m), float(voxel) * 3.0),
|
||
)
|
||
anchors = np.asarray(anchors, dtype=np.float64)
|
||
if len(anchors) == 0:
|
||
anchors = np.asarray([np.mean(tgt_np, axis=0)], dtype=np.float64)
|
||
if len(anchors) > int(loc_global_reloc_max_anchors):
|
||
step = max(1, int(np.ceil(float(len(anchors)) / float(loc_global_reloc_max_anchors))))
|
||
anchors = anchors[::step][: int(loc_global_reloc_max_anchors)]
|
||
|
||
yaw_step = float(loc_global_reloc_yaw_step_deg)
|
||
yaw_candidates = list(np.arange(-180.0, 180.0, yaw_step, dtype=np.float64))
|
||
if 0.0 not in yaw_candidates:
|
||
yaw_candidates.append(0.0)
|
||
|
||
best = dict(reg)
|
||
for anchor in anchors:
|
||
anchor_v = np.asarray(anchor, dtype=np.float64)
|
||
for yaw_deg in yaw_candidates:
|
||
yaw = np.deg2rad(float(yaw_deg))
|
||
cy = float(np.cos(yaw))
|
||
sy = float(np.sin(yaw))
|
||
rz = np.array(
|
||
[[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]],
|
||
dtype=np.float64,
|
||
)
|
||
init_global = np.eye(4, dtype=np.float64)
|
||
init_global[:3, :3] = rz
|
||
init_global[:3, 3] = anchor_v - (rz @ c_src)
|
||
rr = _icp_point_to_point_np(
|
||
src_np,
|
||
tgt_np,
|
||
init_global,
|
||
max_corr_m=float(max_corr) * float(loc_global_reloc_corr_mult),
|
||
max_iter=int(loc_global_reloc_coarse_iter),
|
||
min_corr=int(loc_global_reloc_min_corr),
|
||
)
|
||
better = (float(rr.get("fitness", 0.0)) > float(best.get("fitness", 0.0))) or (
|
||
float(rr.get("fitness", 0.0)) == float(best.get("fitness", 0.0))
|
||
and float(rr.get("rmse", 9e9)) < float(best.get("rmse", 9e9))
|
||
)
|
||
if better:
|
||
best = rr
|
||
|
||
# Refine the best global candidate.
|
||
best_tf = np.asarray(best.get("transform", np.eye(4, dtype=np.float64)), dtype=np.float64)
|
||
best_refined = _icp_point_to_point_np(
|
||
src_np,
|
||
tgt_np,
|
||
best_tf,
|
||
max_corr_m=float(max_corr) * max(1.2, float(loc_global_reloc_corr_mult) * 0.8),
|
||
max_iter=int(loc_global_reloc_refine_iter),
|
||
min_corr=max(20, int(loc_global_reloc_min_corr)),
|
||
)
|
||
if (float(best_refined.get("fitness", 0.0)) > float(best.get("fitness", 0.0))) or (
|
||
float(best_refined.get("fitness", 0.0)) == float(best.get("fitness", 0.0))
|
||
and float(best_refined.get("rmse", 9e9)) < float(best.get("rmse", 9e9))
|
||
):
|
||
best = best_refined
|
||
|
||
if (float(best.get("fitness", 0.0)) > float(reg.get("fitness", 0.0))) or (
|
||
float(best.get("fitness", 0.0)) == float(reg.get("fitness", 0.0))
|
||
and float(best.get("rmse", 9e9)) < float(reg.get("rmse", 9e9))
|
||
):
|
||
reg = best
|
||
used_recovery = True
|
||
|
||
reg_tf = np.asarray(reg.get("transform", np.eye(4, dtype=np.float64)), dtype=np.float64)
|
||
fit = float(reg.get("fitness", 0.0))
|
||
rmse = float(reg.get("rmse", 9e9))
|
||
acc = accepted(reg)
|
||
temporal_guard_reject = False
|
||
bootstrap_pending = False
|
||
bootstrap_ok = False
|
||
guess_offset_m = -1.0
|
||
if approx_guess_active and (approx_pose_guess_tf is not None):
|
||
try:
|
||
guess_center = np.asarray(approx_pose_guess_tf[:3, 3], dtype=np.float64)
|
||
guess_offset_m = float(np.linalg.norm(np.asarray(reg_tf[:3, 3], dtype=np.float64) - guess_center))
|
||
except Exception:
|
||
guess_offset_m = -1.0
|
||
if guess_strict_local_active and guess_offset_m >= 0.0:
|
||
if guess_offset_m > float(loc_guess_max_start_offset_m):
|
||
acc = False
|
||
bidir = _bidir_match_stats(src_d, tgt_d, reg_tf, max_corr_m=max_corr)
|
||
if bool(loc_bidir_enabled) and bool(bidir.get("ok", False)):
|
||
if (
|
||
float(bidir.get("bidir_inlier", 0.0)) < float(loc_bidir_min_ratio)
|
||
or float(bidir.get("bidir_rmse", 9e9)) > float(loc_bidir_max_rmse)
|
||
):
|
||
acc = False
|
||
if use_live_src and slam_to_ref_valid and src_tag not in ("RECOVERY", "ON_LOAD"):
|
||
dtr_m, drot_deg = _tf_delta(last_localize_transform, reg_tf)
|
||
max_step_t = float(loc_live_track_max_step_trans_m) if (localize_only_enabled and loc_live_track_strict) else 0.35
|
||
max_step_r = float(loc_live_track_max_step_rot_deg) if (localize_only_enabled and loc_live_track_strict) else 22.0
|
||
if (dtr_m > max_step_t or drot_deg > max_step_r) and (fit < (accept_fit_target + 0.10)):
|
||
acc = False
|
||
temporal_guard_reject = True
|
||
if use_live_src and slam_to_ref_valid and src_tag not in ("RECOVERY", "ON_LOAD") and acc:
|
||
# Keep tracking lock conservative to prevent map-frame drift.
|
||
if localize_only_enabled and loc_live_track_strict:
|
||
live_fit_min = max(float(loc_live_track_min_fit), float(accept_fit_target) + 0.02)
|
||
else:
|
||
live_fit_min = max(0.38, float(accept_fit_target) + 0.04)
|
||
if fit < live_fit_min:
|
||
acc = False
|
||
temporal_guard_reject = True
|
||
elif bool(loc_bidir_enabled) and bool(bidir.get("ok", False)):
|
||
if localize_only_enabled and loc_live_track_strict:
|
||
live_bidir_min = max(float(loc_live_track_min_bidir), float(loc_bidir_min_ratio))
|
||
else:
|
||
live_bidir_min = max(0.24, float(loc_bidir_min_ratio) + 0.06)
|
||
if float(bidir.get("bidir_inlier", 0.0)) < live_bidir_min:
|
||
acc = False
|
||
temporal_guard_reject = True
|
||
if approx_guess_active and (not slam_to_ref_valid):
|
||
bootstrap_ok = bool(acc)
|
||
if bootstrap_ok and (fit < float(loc_guess_bootstrap_min_fitness)):
|
||
bootstrap_ok = False
|
||
if bootstrap_ok and bool(bidir.get("ok", False)):
|
||
if float(bidir.get("bidir_inlier", 0.0)) < float(loc_guess_bootstrap_min_bidir):
|
||
bootstrap_ok = False
|
||
if bootstrap_ok and guess_offset_m >= 0.0:
|
||
if guess_offset_m > float(loc_guess_max_start_offset_m):
|
||
bootstrap_ok = False
|
||
if bootstrap_ok:
|
||
approx_pose_guess_success_count += 1
|
||
# Seed next bootstrap attempt from this best candidate.
|
||
loc_frames.last_alignment = np.array(reg_tf, dtype=np.float64, copy=True)
|
||
_sync_loc_vars_from_service()
|
||
if int(approx_pose_guess_success_count) < int(loc_guess_bootstrap_min_hits):
|
||
bootstrap_pending = True
|
||
acc = False
|
||
else:
|
||
approx_pose_guess_success_count = 0
|
||
fit_floor = max(1e-6, float(loc_state_cfg.lost_fitness))
|
||
fit_span = max(1e-6, float(accept_fit_target) - fit_floor)
|
||
fit_score = float(np.clip((fit - fit_floor) / fit_span, 0.0, 1.0))
|
||
rmse_cap = max(1e-6, float(accept_rmse_target) * 1.5)
|
||
rmse_score = float(np.clip((rmse_cap - rmse) / rmse_cap, 0.0, 1.0))
|
||
confidence = float(np.clip(0.62 * fit_score + 0.38 * rmse_score, 0.0, 1.0))
|
||
if acc:
|
||
confidence = max(confidence, 0.75)
|
||
if acc and use_live_src and slam_to_ref_valid and src_tag != "RECOVERY":
|
||
# Low-pass localization transform in live mode to reduce visual map shake.
|
||
alpha = 0.45 if fit >= (accept_fit_target + 0.08) else 0.30
|
||
reg_tf = _blend_rigid_tf(last_localize_transform, reg_tf, alpha_t=alpha, alpha_r=alpha)
|
||
|
||
sigma_xy = float(max(0.02, rmse * (1.3 - fit_score)))
|
||
sigma_z = float(max(0.03, rmse * (1.6 - fit_score)))
|
||
sigma_yaw = float(max(0.02, (1.0 - fit_score) * 0.35))
|
||
|
||
# Snapshot for the FRAME builder — lidar_api reads this via
|
||
# nav.loc_result, then exposes it as loc_metrics on the
|
||
# status payload (WS + ZMQ).
|
||
last_loc_icp_result = {
|
||
"fitness": float(fit),
|
||
"rmse": float(rmse),
|
||
"inliers": int(reg.get("inliers", 0)),
|
||
"icp_ms_delay": float(_icp_ms_delay),
|
||
"accepted": bool(acc),
|
||
"confidence": float(confidence),
|
||
}
|
||
|
||
result: Dict[str, Any] = {
|
||
"fitness": fit,
|
||
"rmse": rmse,
|
||
"max_corr": float(max_corr),
|
||
"accepted": acc,
|
||
"transform": np.array(reg_tf, dtype=np.float64),
|
||
"source": src_tag,
|
||
"src_points": int(len(src_d)),
|
||
"ref_points": int(len(tgt_d)),
|
||
"ref_points_full": int(len(ref_np_full)),
|
||
"tracking_window_used": bool(tracking_window_used),
|
||
"tracking_window_reason": str(tracking_window_reason),
|
||
"recovery_used": bool(used_recovery),
|
||
"temporal_guard_reject": bool(temporal_guard_reject),
|
||
"yaw_hypotheses": int(yaw_hypotheses),
|
||
"confidence": float(confidence),
|
||
"covariance_diag": [sigma_xy, sigma_xy, sigma_z, sigma_yaw, sigma_yaw, sigma_yaw],
|
||
"live_only": bool(use_live_src),
|
||
"global_reloc_allowed": bool(allow_global_reloc),
|
||
"global_reloc_blocked_by_guess": bool(global_reloc_blocked_by_guess),
|
||
"bidir_ok": bool(bidir.get("ok", False)),
|
||
"inlier_live_to_ref": float(bidir.get("live_to_ref_inlier", 0.0)),
|
||
"inlier_ref_to_live": float(bidir.get("ref_to_live_inlier", 0.0)),
|
||
"inlier_bidir": float(bidir.get("bidir_inlier", 0.0)),
|
||
"bidir_rmse": float(bidir.get("bidir_rmse", 9e9)),
|
||
"approx_guess_used": bool(approx_guess_used),
|
||
"approx_guess_fail_count": int(approx_pose_guess_fail_count),
|
||
"approx_guess_success_count": int(approx_pose_guess_success_count),
|
||
"approx_guess_offset_m": float(guess_offset_m),
|
||
"bootstrap_pending": bool(bootstrap_pending),
|
||
"place_retrieval_ok": bool(place_query.get("ok", False)),
|
||
"place_retrieval_reason": str(place_query.get("reason", "na")),
|
||
"place_retrieval_backend": str(place_query.get("backend", place_recog.backend_active)),
|
||
"place_retrieval_top_score": float(place_query.get("top_score", 0.0)),
|
||
"place_retrieval_candidates": int(len(place_query.get("candidates", []))),
|
||
}
|
||
|
||
last_localize_t = now
|
||
if not use_live_src:
|
||
last_localize_points = cur_n
|
||
if result["accepted"]:
|
||
prev_tf = np.array(slam_to_ref_transform, dtype=np.float64, copy=True) if slam_to_ref_valid else None
|
||
loc_frames.set_reference_alignment(reg_tf, confidence)
|
||
_sync_loc_vars_from_service()
|
||
if use_live_src and prev_tf is not None and submap_mode_enabled and submap_mapper.has_points:
|
||
try:
|
||
delta_tf = np.asarray(slam_to_ref_transform, dtype=np.float64) @ np.linalg.inv(prev_tf)
|
||
dt_m, dr_deg = _tf_delta(np.eye(4, dtype=np.float64), delta_tf)
|
||
if dt_m > 1.20 or dr_deg > 45.0:
|
||
# Large relocalization jump: reset local/global buffer to avoid ghost structures.
|
||
submap_mapper.reset()
|
||
elif dt_m > 1e-4 or dr_deg > 1e-3:
|
||
submap_mapper.apply_correction(delta_tf)
|
||
except Exception as _sub_exc:
|
||
st("WARN", f"[submap] correction after relocalization failed: {_sub_exc}")
|
||
approx_pose_guess_tf = None
|
||
approx_pose_guess_until_t = 0.0
|
||
approx_pose_guess_fail_count = 0
|
||
approx_pose_guess_success_count = 0
|
||
session_store.record_success(
|
||
ref_map_path,
|
||
last_localize_transform,
|
||
float(result["fitness"]),
|
||
float(result["rmse"]),
|
||
)
|
||
else:
|
||
if approx_guess_active and (not slam_to_ref_valid):
|
||
if not bootstrap_pending:
|
||
approx_pose_guess_fail_count += 1
|
||
loc_frames.update_confidence(max(0.0, min(1.0, loc_frames.confidence * 0.85)))
|
||
_sync_loc_vars_from_service()
|
||
safety.mark_localization(bool(result["accepted"]), now)
|
||
loc_state.update(result, now)
|
||
result["state"] = str(loc_state.state)
|
||
st("INFO", {"LOCALIZE": result})
|
||
push_loc_state()
|
||
return result
|
||
|
||
except Exception as e:
|
||
safety.mark_localization(False, now)
|
||
loc_state.update(None, now)
|
||
push_loc_state()
|
||
if diagnostics is not None:
|
||
diagnostics.log_exception("LOCALIZE failed", e, _worker_diag_state())
|
||
st("ERROR", f"LOCALIZE failed: {e}")
|
||
return None
|
||
|
||
def maybe_auto_localize(src_live: Optional[np.ndarray] = None):
|
||
if not loc_cfg.enabled:
|
||
return
|
||
if not ref_map_path:
|
||
return
|
||
if src_live is not None and localize_only_enabled:
|
||
do_localize(force=False, source="LIVE_ONLY", src_override=src_live)
|
||
else:
|
||
do_localize(force=False, source="AUTO")
|
||
t = _now()
|
||
if loc_state.should_recover(t):
|
||
loc_state.enter_recovery(t)
|
||
push_loc_state()
|
||
do_localize(force=True, source="RECOVERY", src_override=src_live if localize_only_enabled else None)
|
||
|
||
def apply_density_mode(mode: str):
|
||
nonlocal density_mode, live_stride
|
||
m = str(mode).upper().strip()
|
||
if m == "LOW":
|
||
density_mode = "LOW"
|
||
live_stride = max(2, base_stride * 4)
|
||
elif m == "HIGH":
|
||
density_mode = "HIGH"
|
||
live_stride = 1
|
||
else:
|
||
density_mode = "MEDIUM"
|
||
live_stride = base_stride
|
||
st("INFO", {"DENSITY": {"mode": density_mode, "stride": int(live_stride)}})
|
||
|
||
def _rebuild_filter_runtime(keep_existing: bool = True) -> None:
|
||
nonlocal filt
|
||
seed_pts = None
|
||
if keep_existing and stable is not None:
|
||
try:
|
||
seed_pts = stable.get_points()
|
||
except Exception:
|
||
seed_pts = None
|
||
|
||
vf_cfg = VFilterCfg(
|
||
voxel_size=float(runtime_filter_voxel),
|
||
hit_threshold=int(runtime_filter_hit_threshold),
|
||
decay_seconds=float(runtime_filter_decay),
|
||
max_voxels=int(filt_cfg.max_voxels),
|
||
)
|
||
new_f = VoxelPersistenceFilter(vf_cfg)
|
||
if seed_pts is not None and len(seed_pts) > 0 and hasattr(new_f, "seed_stable_points"):
|
||
try:
|
||
new_f.seed_stable_points(
|
||
seed_pts,
|
||
now=_now(),
|
||
hit_count=int(runtime_filter_hit_threshold),
|
||
clear_existing=True,
|
||
)
|
||
except Exception:
|
||
pass
|
||
filt = new_f
|
||
|
||
def apply_filter_tuning(payload: Any) -> None:
|
||
nonlocal runtime_filter_voxel, runtime_filter_hit_threshold, runtime_filter_decay
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_FILTER_TUNING ignored: payload must be dict.")
|
||
return
|
||
try:
|
||
runtime_filter_voxel = max(0.02, float(payload.get("voxel_size", runtime_filter_voxel)))
|
||
except Exception:
|
||
pass
|
||
try:
|
||
runtime_filter_hit_threshold = max(1, int(payload.get("hit_threshold", runtime_filter_hit_threshold)))
|
||
except Exception:
|
||
pass
|
||
try:
|
||
runtime_filter_decay = max(0.5, float(payload.get("decay_seconds", runtime_filter_decay)))
|
||
except Exception:
|
||
pass
|
||
|
||
_rebuild_filter_runtime(keep_existing=True)
|
||
st(
|
||
"INFO",
|
||
{
|
||
"FILTER_TUNING": {
|
||
"voxel_size": float(runtime_filter_voxel),
|
||
"hit_threshold": int(runtime_filter_hit_threshold),
|
||
"decay_seconds": float(runtime_filter_decay),
|
||
"profile": str(stability_profile),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_stability_profile(profile_name: Any) -> None:
|
||
nonlocal stability_profile
|
||
p = str(profile_name or "BALANCED").upper().strip()
|
||
presets = {
|
||
# Keep map history much longer during exploration so points do not fade while moving away.
|
||
"MAP_NEW": {"hit_threshold": 2, "decay_seconds": 1800.0, "voxel_size": 0.10, "recommended_density": "MEDIUM"},
|
||
"EXTEND_MAP": {"hit_threshold": 2, "decay_seconds": 1800.0, "voxel_size": 0.10, "recommended_density": "MEDIUM"},
|
||
"LOCALIZE_MAP": {"hit_threshold": 3, "decay_seconds": 45.0, "voxel_size": 0.20, "recommended_density": "MEDIUM"},
|
||
"LIVE_NAV_MAP": {"hit_threshold": 2, "decay_seconds": 18.0, "voxel_size": 0.20, "recommended_density": "MEDIUM"},
|
||
"LIVE_NAV_NO_MAP": {"hit_threshold": 2, "decay_seconds": 14.0, "voxel_size": 0.18, "recommended_density": "HIGH"},
|
||
"BALANCED": {
|
||
"voxel_size": float(max(0.02, filt_cfg.voxel_size)),
|
||
"hit_threshold": int(max(1, filt_cfg.hits_threshold)),
|
||
"decay_seconds": float(max(0.5, filt_cfg.strict_sec if filt_cfg.use_strict else filt_cfg.window_sec)),
|
||
"recommended_density": "MEDIUM",
|
||
},
|
||
}
|
||
if p not in presets:
|
||
st("WARN", f"Unknown stability profile: {p}.")
|
||
return
|
||
stability_profile = p
|
||
preset = presets[p]
|
||
apply_filter_tuning(
|
||
{
|
||
"voxel_size": float(preset.get("voxel_size", runtime_filter_voxel)),
|
||
"hit_threshold": int(preset["hit_threshold"]),
|
||
"decay_seconds": float(preset["decay_seconds"]),
|
||
}
|
||
)
|
||
apply_density_mode(str(preset.get("recommended_density", density_mode)))
|
||
st(
|
||
"INFO",
|
||
{
|
||
"WORKFLOW_PROFILE": {
|
||
"name": str(stability_profile),
|
||
"recommended_density": str(preset.get("recommended_density", density_mode)),
|
||
}
|
||
},
|
||
)
|
||
|
||
def _submap_profile_allowed() -> bool:
|
||
profile = str(stability_profile).upper().strip()
|
||
allowed = {str(x).upper().strip() for x in tuple(submap_cfg.apply_profiles)}
|
||
return profile in allowed
|
||
|
||
def apply_min_stable_points(value: Any) -> None:
|
||
try:
|
||
v = int(value)
|
||
except Exception:
|
||
st("WARN", f"SET_MIN_STABLE_POINTS ignored: invalid value {value!r}")
|
||
return
|
||
v = int(np.clip(v, 50, 5_000_000))
|
||
map_cfg.min_points_to_save = v
|
||
loc_cfg.min_points_for_localize = v
|
||
nav_cfg.min_points = v
|
||
try:
|
||
nav_exporter.update_config(nav_cfg)
|
||
except Exception:
|
||
pass
|
||
st("INFO", {"MIN_STABLE_POINTS": {"value": int(v)}})
|
||
|
||
def apply_loop_closure_mode(enabled: Any) -> None:
|
||
e = _as_bool(enabled, False)
|
||
loop_cfg.enabled = bool(e)
|
||
loop_backend.cfg.enabled = bool(e)
|
||
st("INFO", {"LOOP_MODE": {"enabled": bool(e)}})
|
||
|
||
def apply_loc_state_machine_mode(enabled: Any) -> None:
|
||
e = _as_bool(enabled, True)
|
||
loc_state_cfg.enabled = bool(e)
|
||
loc_state.set_enabled(bool(e))
|
||
st("INFO", {"LOC_MACHINE": {"enabled": bool(e)}})
|
||
push_loc_state()
|
||
|
||
def apply_submap_mode(payload: Any) -> None:
|
||
nonlocal submap_cfg, submap_mode_enabled
|
||
merged = {
|
||
"enabled": bool(submap_mode_enabled),
|
||
"local_window_frames": int(submap_cfg.local_window_frames),
|
||
"local_voxel_m": float(submap_cfg.local_voxel_m),
|
||
"global_voxel_m": float(submap_cfg.global_voxel_m),
|
||
"merge_period_sec": float(submap_cfg.merge_period_sec),
|
||
"merge_min_translation_m": float(submap_cfg.merge_min_translation_m),
|
||
"merge_min_rotation_deg": float(submap_cfg.merge_min_rotation_deg),
|
||
"max_global_points": int(submap_cfg.max_global_points),
|
||
"display_max_points": int(submap_cfg.display_max_points),
|
||
"apply_profiles": list(submap_cfg.apply_profiles),
|
||
}
|
||
if isinstance(payload, dict):
|
||
merged.update(payload)
|
||
elif payload is not None:
|
||
merged["enabled"] = _as_bool(payload, bool(submap_mode_enabled))
|
||
new_cfg = SubmapConfig.from_dict(merged)
|
||
keep_pts = True
|
||
if isinstance(payload, dict):
|
||
keep_pts = _as_bool(payload.get("keep_points", True), True)
|
||
submap_cfg = new_cfg
|
||
submap_mode_enabled = bool(new_cfg.enabled)
|
||
submap_mapper.set_config(new_cfg, keep_points=bool(keep_pts and submap_mode_enabled))
|
||
if not submap_mode_enabled:
|
||
submap_mapper.reset()
|
||
st(
|
||
"INFO",
|
||
{
|
||
"SUBMAP_MODE": {
|
||
"enabled": bool(submap_mode_enabled),
|
||
"profile_allowed": bool(_submap_profile_allowed()),
|
||
"apply_profiles": list(submap_cfg.apply_profiles),
|
||
"local_window_frames": int(submap_cfg.local_window_frames),
|
||
"local_voxel_m": float(submap_cfg.local_voxel_m),
|
||
"global_voxel_m": float(submap_cfg.global_voxel_m),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_approx_pose(payload: Any) -> None:
|
||
nonlocal approx_pose_guess_tf, approx_pose_guess_until_t
|
||
nonlocal approx_pose_guess_fail_count, approx_pose_guess_success_count
|
||
if payload is None:
|
||
approx_pose_guess_tf = None
|
||
approx_pose_guess_until_t = 0.0
|
||
approx_pose_guess_fail_count = 0
|
||
approx_pose_guess_success_count = 0
|
||
st("INFO", {"APPROX_POSE": {"set": False}})
|
||
return
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_APPROX_POSE ignored: payload must be dict.")
|
||
return
|
||
try:
|
||
x = float(payload.get("x"))
|
||
y = float(payload.get("y"))
|
||
except Exception:
|
||
st("WARN", "SET_APPROX_POSE ignored: x,y are required.")
|
||
return
|
||
z = float(payload.get("z", loc_guess_default_z_m))
|
||
yaw_deg = payload.get("yaw_deg", None)
|
||
if yaw_deg is None:
|
||
yaw_deg = _yaw_deg_from_tf(last_localize_transform)
|
||
else:
|
||
yaw_deg = float(yaw_deg)
|
||
tf = _tf_from_xyzyaw(x, y, z, yaw_deg)
|
||
approx_pose_guess_tf = np.asarray(tf, dtype=np.float64)
|
||
approx_pose_guess_until_t = float(_now() + float(loc_guess_ttl_sec))
|
||
approx_pose_guess_fail_count = 0
|
||
approx_pose_guess_success_count = 0
|
||
st(
|
||
"INFO",
|
||
{
|
||
"APPROX_POSE": {
|
||
"set": True,
|
||
"x": float(x),
|
||
"y": float(y),
|
||
"z": float(z),
|
||
"yaw_deg": float(yaw_deg),
|
||
"ttl_sec": float(loc_guess_ttl_sec),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_autosave(payload: Any) -> None:
|
||
nonlocal autosave_enabled, autosave_interval_sec, autosave_base_name
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_AUTOSAVE ignored: payload must be dict.")
|
||
return
|
||
autosave_enabled = _as_bool(payload.get("enabled", autosave_enabled), autosave_enabled)
|
||
try:
|
||
autosave_interval_sec = max(5.0, float(payload.get("interval_sec", autosave_interval_sec)))
|
||
except Exception:
|
||
pass
|
||
base_in = str(payload.get("base_name", autosave_base_name)).strip()
|
||
if base_in:
|
||
autosave_base_name = base_in
|
||
st(
|
||
"INFO",
|
||
{
|
||
"AUTOSAVE": {
|
||
"enabled": bool(autosave_enabled),
|
||
"interval_sec": float(autosave_interval_sec),
|
||
"base_name": str(autosave_base_name),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_nav_config(payload: Any) -> None:
|
||
nonlocal nav_cfg, nav_rt_cfg, nav_runtime, nav_global_planner
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_NAV_CONFIG ignored: payload must be dict.")
|
||
return
|
||
candidate = nav_cfg.patched(payload)
|
||
if float(candidate.z_max_m) <= float(candidate.z_min_m):
|
||
st("WARN", "SET_NAV_CONFIG ignored: z_max_m must be greater than z_min_m.")
|
||
return
|
||
nav_cfg = candidate
|
||
nav_exporter.update_config(nav_cfg)
|
||
nav_rt_cfg = nav_rt_cfg.patched(
|
||
{
|
||
"resolution_m": float(nav_cfg.resolution_m),
|
||
"z_min_m": float(nav_cfg.z_min_m),
|
||
"z_max_m": float(nav_cfg.z_max_m),
|
||
"padding_m": float(nav_cfg.padding_m),
|
||
"inflation_radius_m": float(nav_cfg.inflation_radius_m),
|
||
}
|
||
)
|
||
nav_runtime = LiveCostmapRuntime(nav_rt_cfg)
|
||
nav_global_planner = GlobalAStarPlanner(blocked_cost=int(nav_rt_cfg.blocked_cost))
|
||
st(
|
||
"INFO",
|
||
{
|
||
"NAV_CONFIG": {
|
||
"min_points": int(nav_cfg.min_points),
|
||
"resolution_m": float(nav_cfg.resolution_m),
|
||
"z_min_m": float(nav_cfg.z_min_m),
|
||
"z_max_m": float(nav_cfg.z_max_m),
|
||
"inflation_radius_m": float(nav_cfg.inflation_radius_m),
|
||
"padding_m": float(nav_cfg.padding_m),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_map_quality_config(payload: Any) -> None:
|
||
nonlocal map_quality_cfg, map_quality
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_MAP_QUALITY ignored: payload must be dict.")
|
||
return
|
||
merged = {
|
||
"enabled": map_quality_cfg.enabled,
|
||
"near_min_range_m": map_quality_cfg.near_min_range_m,
|
||
"ray_consistency_enabled": map_quality_cfg.ray_consistency_enabled,
|
||
"ray_bin_deg": map_quality_cfg.ray_bin_deg,
|
||
"ray_elev_bin_deg": map_quality_cfg.ray_elev_bin_deg,
|
||
"ray_max_behind_m": map_quality_cfg.ray_max_behind_m,
|
||
"ray_keep_n": map_quality_cfg.ray_keep_n,
|
||
"world_z_clip_enabled": map_quality_cfg.world_z_clip_enabled,
|
||
"world_z_min_m": map_quality_cfg.world_z_min_m,
|
||
"world_z_max_m": map_quality_cfg.world_z_max_m,
|
||
"outlier_filter_enabled": map_quality_cfg.outlier_filter_enabled,
|
||
"outlier_voxel_m": map_quality_cfg.outlier_voxel_m,
|
||
"outlier_min_points": map_quality_cfg.outlier_min_points,
|
||
}
|
||
merged.update(payload)
|
||
map_quality_cfg = IndoorMapQualityConfig.from_dict(merged)
|
||
map_quality = IndoorMapQualityFilter(map_quality_cfg)
|
||
st(
|
||
"INFO",
|
||
{
|
||
"MAP_QUALITY": {
|
||
"enabled": bool(map_quality_cfg.enabled),
|
||
"near_min_range_m": float(map_quality_cfg.near_min_range_m),
|
||
"ray_consistency_enabled": bool(map_quality_cfg.ray_consistency_enabled),
|
||
"ray_bin_deg": float(map_quality_cfg.ray_bin_deg),
|
||
"ray_elev_bin_deg": float(map_quality_cfg.ray_elev_bin_deg),
|
||
"ray_max_behind_m": float(map_quality_cfg.ray_max_behind_m),
|
||
"ray_keep_n": int(map_quality_cfg.ray_keep_n),
|
||
"world_z_clip_enabled": bool(map_quality_cfg.world_z_clip_enabled),
|
||
"world_z_min_m": float(map_quality_cfg.world_z_min_m),
|
||
"world_z_max_m": float(map_quality_cfg.world_z_max_m),
|
||
"outlier_filter_enabled": bool(map_quality_cfg.outlier_filter_enabled),
|
||
"outlier_voxel_m": float(map_quality_cfg.outlier_voxel_m),
|
||
"outlier_min_points": int(map_quality_cfg.outlier_min_points),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_nav_runtime_config(payload: Any) -> None:
|
||
nonlocal nav_rt_cfg, nav_runtime, nav_global_planner, nav_path_world, nav_last_plan_t, nav_last_update_t
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_NAV_RUNTIME ignored: payload must be dict.")
|
||
return
|
||
nav_rt_cfg = nav_rt_cfg.patched(payload)
|
||
nav_runtime = LiveCostmapRuntime(nav_rt_cfg)
|
||
nav_global_planner = GlobalAStarPlanner(blocked_cost=int(nav_rt_cfg.blocked_cost))
|
||
nav_path_world = []
|
||
nav_last_plan_t = 0.0
|
||
nav_last_update_t = 0.0
|
||
st(
|
||
"INFO",
|
||
{
|
||
"NAV_RUNTIME": {
|
||
"enabled": bool(nav_rt_cfg.enabled),
|
||
"resolution_m": float(nav_rt_cfg.resolution_m),
|
||
"z_min_m": float(nav_rt_cfg.z_min_m),
|
||
"z_max_m": float(nav_rt_cfg.z_max_m),
|
||
"inflation_radius_m": float(nav_rt_cfg.inflation_radius_m),
|
||
"dynamic_decay_sec": float(nav_rt_cfg.dynamic_decay_sec),
|
||
"dynamic_min_hits": int(nav_rt_cfg.dynamic_min_hits),
|
||
"blocked_cost": int(nav_rt_cfg.blocked_cost),
|
||
}
|
||
},
|
||
)
|
||
|
||
# ─── Autonomous wander helpers ───────────────────────────────────
|
||
def _wander_allowed_now() -> bool:
|
||
"""True iff wander is armed AND the current workflow is in the
|
||
config's allowed list. Mapping-workflow operators who pressed
|
||
the toggle once and then switched to a non-allowed workflow get
|
||
a silent no-op instead of unexpected motion."""
|
||
if not bool(wander_active):
|
||
return False
|
||
allowed = _wander_cfg.get("allowed_workflows") or []
|
||
wf = str(stability_profile).upper().strip()
|
||
if not allowed:
|
||
return True
|
||
return wf in {str(a).upper().strip() for a in allowed}
|
||
|
||
def _wander_pick_goal(pose_pub) -> Optional[Tuple[float, float]]:
|
||
"""Sample N random reachable cells in the current costmap and
|
||
return the world-coordinates of the BEST one (lowest score)
|
||
— best = closest to map frontier, least-visited, and not in
|
||
the failed-goal blacklist. Returns None if no candidate
|
||
survives — caller tries again next tick.
|
||
|
||
`pose_pub` is the latest 4×4 pose matrix (or None). Must be
|
||
passed in because this helper lives in the outer worker scope
|
||
and can't close over `update_loop`'s local pose variable.
|
||
"""
|
||
nonlocal _wander_last_pick_t, _blacklisted_goals
|
||
grid = nav_runtime.grid if nav_runtime is not None else None
|
||
if grid is None or pose_pub is None:
|
||
return None
|
||
try:
|
||
cost = np.asarray(grid["costmap"], dtype=np.uint8)
|
||
origin = np.asarray(grid["origin_xy"], dtype=np.float64)
|
||
res = float(grid["resolution_m"])
|
||
except Exception:
|
||
return None
|
||
H, W = cost.shape
|
||
rx, ry = float(pose_pub[0, 3]), float(pose_pub[1, 3])
|
||
|
||
wf = str(stability_profile).upper().strip()
|
||
# Normalize config keys to uppercase so a typo'd `Live_Nav_Map`
|
||
# in SLAM_Config.json doesn't silently fall through to the global
|
||
# default. Build the dict once per call; configs are small.
|
||
raw_per_wf = _wander_cfg.get("per_workflow_max_reach_m") or {}
|
||
try:
|
||
per_wf_norm = {str(k).upper().strip(): v for k, v in raw_per_wf.items()}
|
||
except Exception:
|
||
per_wf_norm = {}
|
||
per_wf_max = per_wf_norm.get(wf)
|
||
max_r = float(per_wf_max if per_wf_max is not None
|
||
else _wander_cfg.get("max_reach_radius_m", 6.0))
|
||
min_r = float(_wander_cfg.get("min_reach_radius_m", 1.5))
|
||
attempts = int(_wander_cfg.get("sample_attempts", 20))
|
||
blocked = int(nav_runtime.cfg.blocked_cost)
|
||
rng = np.random.default_rng()
|
||
|
||
# Exploration intelligence — frontier + coverage + blacklist.
|
||
# (1) FRONTIER: in mapping workflows the GRID BOUNDARY is the
|
||
# edge of the explored area. Goals near the boundary push
|
||
# LiDAR observations into unmapped territory; goals in the
|
||
# interior just re-cover known space. Score each candidate
|
||
# by distance-to-boundary (lower = closer to frontier).
|
||
# (2) COVERAGE: every robot pose increments a coarse 2 m visit
|
||
# grid. Goals in heavily-visited cells get a coverage
|
||
# penalty added to their score — stops the robot from
|
||
# re-mapping the same corner.
|
||
# (3) BLACKLIST: goals that timed out (robot couldn't reach
|
||
# them in `goal_timeout_sec`) are rejected for 2 minutes
|
||
# so the picker doesn't keep trying the locked door.
|
||
is_mapping_wf = wf in ("MAP_NEW", "EXTEND_MAP")
|
||
now_t = _now()
|
||
|
||
# Purge expired blacklist entries (lazy GC; cheap O(N) with
|
||
# small N — blacklists rarely exceed 5-10 entries in practice).
|
||
if _blacklisted_goals:
|
||
_blacklisted_goals[:] = [
|
||
(bx, by, exp) for bx, by, exp in _blacklisted_goals
|
||
if exp > now_t
|
||
]
|
||
|
||
def _in_blacklist(wx: float, wy: float) -> bool:
|
||
for bx, by, _exp in _blacklisted_goals:
|
||
dx = wx - bx
|
||
dy = wy - by
|
||
if dx * dx + dy * dy <= 4.0: # 2 m radius
|
||
return True
|
||
return False
|
||
|
||
def _coverage_count(wx: float, wy: float) -> int:
|
||
cgx = int(math.floor(wx / 2.0))
|
||
cgy = int(math.floor(wy / 2.0))
|
||
return int(_coverage_visits.get((cgx, cgy), 0))
|
||
|
||
candidates: List[Tuple[float, float, float]] = []
|
||
for _ in range(max(1, attempts)):
|
||
# In mapping workflows pull samples toward the outer half
|
||
# of the reach band. Combined with the frontier-distance
|
||
# score below, this makes the picker strongly prefer
|
||
# genuine frontier cells.
|
||
if is_mapping_wf:
|
||
_r_lo = max(min_r, max_r * 0.5)
|
||
r = float(rng.uniform(_r_lo, max_r))
|
||
else:
|
||
r = float(rng.uniform(min_r, max_r))
|
||
th = float(rng.uniform(-math.pi, math.pi))
|
||
wx = rx + r * math.cos(th)
|
||
wy = ry + r * math.sin(th)
|
||
gx = int(math.floor((wx - origin[0]) / res))
|
||
gy = int(math.floor((wy - origin[1]) / res))
|
||
if not (0 <= gx < W and 0 <= gy < H):
|
||
continue
|
||
if int(cost[gy, gx]) >= blocked:
|
||
continue
|
||
if _in_blacklist(wx, wy):
|
||
continue
|
||
# Score = composite of coverage penalty + (in mapping
|
||
# workflows) frontier-distance preference. LOWER is better.
|
||
cov = _coverage_count(wx, wy)
|
||
if is_mapping_wf:
|
||
# min(gx, W-1-gx, gy, H-1-gy) = cells from nearest
|
||
# grid edge. Smaller = closer to frontier.
|
||
frontier_dist = min(gx, W - 1 - gx, gy, H - 1 - gy)
|
||
score = float(frontier_dist) + float(cov) * 0.5
|
||
else:
|
||
# Nav workflows: just penalize revisits, accept any
|
||
# reachable cell otherwise.
|
||
score = float(cov)
|
||
candidates.append((score, float(wx), float(wy)))
|
||
|
||
if not candidates:
|
||
return None
|
||
|
||
# Pick the lowest-scoring candidate (best frontier / least
|
||
# visited). Tie-break is implicit (first in list wins).
|
||
candidates.sort(key=lambda c: c[0])
|
||
_, best_x, best_y = candidates[0]
|
||
_wander_last_pick_t = now_t
|
||
return (best_x, best_y)
|
||
|
||
def _wander_tick(now_t: float, pose_pub) -> None:
|
||
"""Per-tick wander state machine. Three triggers fire a new
|
||
goal pick:
|
||
1. Wander is active but no goal is set
|
||
2. The current wander goal was just reached
|
||
3. The current wander goal has been pending longer than
|
||
`goal_timeout_sec` (robot can't get there — try a new one)
|
||
|
||
Manual operator goals are not touched here — they win until
|
||
cleared. Wander resumes once `_wander_goal_origin == 'operator'`
|
||
is gone.
|
||
|
||
`pose_pub` is forwarded to `_wander_pick_goal` because both
|
||
helpers live in the outer worker scope and can't close over
|
||
update_loop's local `pose_pub`.
|
||
"""
|
||
nonlocal nav_goal_xy, nav_path_world, nav_last_plan_t
|
||
nonlocal _wander_goal_origin, _blacklisted_goals
|
||
nonlocal _explore_budget_deadline, _explore_returning_home
|
||
nonlocal _explore_origin_pose
|
||
if not _wander_allowed_now():
|
||
return
|
||
|
||
# Lazy origin capture for the explore-budget return-home path.
|
||
# The first time _wander_tick runs after arming with a valid
|
||
# pose, snapshot (x, y) as the home position.
|
||
if _explore_origin_pose is None and pose_pub is not None:
|
||
try:
|
||
_explore_origin_pose = (
|
||
float(pose_pub[0, 3]),
|
||
float(pose_pub[1, 3]),
|
||
)
|
||
st("INFO", {"WANDER": {"event": "origin_captured",
|
||
"x": _explore_origin_pose[0],
|
||
"y": _explore_origin_pose[1]}})
|
||
except Exception:
|
||
pass
|
||
|
||
# Operator goal in progress → defer to it.
|
||
if nav_goal_xy is not None and _wander_goal_origin == "operator":
|
||
return
|
||
|
||
# ── BOUNDED EXPLORE-THEN-RETURN (#5) ─────────────────────────
|
||
# If a time budget was armed and has expired, override the
|
||
# normal random-goal pick with a return-to-origin. The robot
|
||
# navigates back, then we auto-disarm wander on arrival.
|
||
if (_explore_budget_deadline is not None
|
||
and now_t >= _explore_budget_deadline
|
||
and _explore_origin_pose is not None
|
||
and not _explore_returning_home):
|
||
_explore_returning_home = True
|
||
nav_goal_xy = (
|
||
float(_explore_origin_pose[0]),
|
||
float(_explore_origin_pose[1]),
|
||
)
|
||
nav_path_world = []
|
||
nav_last_plan_t = 0.0
|
||
_wander_goal_origin = "wander"
|
||
st("INFO", {"EXPLORE_BUDGET": {"event": "expired",
|
||
"returning_home": True,
|
||
"origin": list(_explore_origin_pose)}})
|
||
return
|
||
# If we're already returning home and the goal got cleared
|
||
# (arrival), stand down — wander_stop will fall through if the
|
||
# user didn't manually rearm.
|
||
if _explore_returning_home and nav_goal_xy is None:
|
||
st("INFO", {"EXPLORE_BUDGET": {"event": "home_reached",
|
||
"disarming": True}})
|
||
wander_active_local = False # noqa: F841 — intent marker
|
||
_explore_budget_deadline = None
|
||
_explore_returning_home = False
|
||
_wander_stop()
|
||
return
|
||
|
||
interval = float(_wander_cfg.get("pick_interval_sec", 30.0))
|
||
timeout = float(_wander_cfg.get("goal_timeout_sec", 60.0))
|
||
|
||
need_new_goal = False
|
||
if nav_goal_xy is None:
|
||
need_new_goal = True
|
||
elif (now_t - float(_wander_last_pick_t)) >= timeout:
|
||
# Goal couldn't be reached in `timeout` seconds → blacklist
|
||
# it for 2 minutes so the picker doesn't keep retrying the
|
||
# same unreachable target (locked door, person standing
|
||
# in doorway, navigation dead-end). Self-clears after the
|
||
# blocking situation resolves.
|
||
need_new_goal = True
|
||
if nav_goal_xy is not None:
|
||
_blacklisted_goals.append((
|
||
float(nav_goal_xy[0]),
|
||
float(nav_goal_xy[1]),
|
||
now_t + 120.0,
|
||
))
|
||
st("INFO", {"WANDER": {
|
||
"event": "goal_timeout",
|
||
"blacklisted": True,
|
||
"blacklist_size": int(len(_blacklisted_goals)),
|
||
}})
|
||
# Periodic re-pick: even if we haven't reached/timed out, switch
|
||
# targets every `interval` so we don't fixate on one cell for
|
||
# too long. Keeps coverage broad for mapping workflows.
|
||
elif (now_t - float(_wander_last_pick_t)) >= interval:
|
||
need_new_goal = True
|
||
|
||
if need_new_goal:
|
||
picked = _wander_pick_goal(pose_pub)
|
||
if picked is None:
|
||
# Costmap not ready or all attempts blocked — try next tick.
|
||
return
|
||
nav_goal_xy = picked
|
||
nav_path_world = []
|
||
nav_last_plan_t = 0.0
|
||
_wander_goal_origin = "wander"
|
||
st("INFO", {"WANDER": {"event": "goal_picked",
|
||
"x": float(picked[0]),
|
||
"y": float(picked[1])}})
|
||
|
||
def _wander_start() -> None:
|
||
nonlocal wander_active, _explore_origin_pose, _explore_returning_home
|
||
if bool(wander_active):
|
||
return
|
||
wander_active = True
|
||
# Origin pose is captured LAZILY in _wander_tick on the first
|
||
# tick after arming — at that point pose_pub is in scope.
|
||
# Reset the slot so an old origin from a previous session
|
||
# doesn't bleed into this one.
|
||
_explore_origin_pose = None
|
||
_explore_returning_home = False
|
||
st("INFO", {"WANDER": {"event": "armed",
|
||
"workflow": str(stability_profile)}})
|
||
|
||
def _wander_stop() -> None:
|
||
nonlocal wander_active, nav_goal_xy, nav_path_world, _wander_goal_origin
|
||
nonlocal _explore_budget_deadline, _explore_returning_home
|
||
wander_active = False
|
||
_explore_budget_deadline = None
|
||
_explore_returning_home = False
|
||
# Only clear the goal if WE set it. Don't yank an operator's
|
||
# active goal when they turn wander off.
|
||
if _wander_goal_origin == "wander":
|
||
nav_goal_xy = None
|
||
nav_path_world = []
|
||
_wander_goal_origin = "operator"
|
||
st("INFO", {"WANDER": {"event": "disarmed"}})
|
||
|
||
def _set_explore_budget(minutes: float) -> None:
|
||
"""Arm a time budget for the next wander session. When the
|
||
budget expires the picker overrides its choice with the
|
||
captured origin pose, the robot returns home, and wander
|
||
disarms on arrival. minutes <= 0 clears the budget."""
|
||
nonlocal _explore_budget_deadline, _explore_returning_home
|
||
try:
|
||
_m = float(minutes)
|
||
except Exception:
|
||
_m = 0.0
|
||
if _m <= 0.0:
|
||
_explore_budget_deadline = None
|
||
_explore_returning_home = False
|
||
st("INFO", {"EXPLORE_BUDGET": {"event": "cleared"}})
|
||
return
|
||
_explore_budget_deadline = _now() + _m * 60.0
|
||
_explore_returning_home = False
|
||
st("INFO", {"EXPLORE_BUDGET": {"event": "armed",
|
||
"minutes": _m,
|
||
"deadline_sec_from_now": _m * 60.0}})
|
||
|
||
def apply_nav_goal(payload: Any) -> None:
|
||
nonlocal nav_goal_xy, nav_path_world, nav_last_plan_t, _wander_goal_origin
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "SET_NAV_GOAL ignored: payload must be dict.")
|
||
return
|
||
try:
|
||
x = float(payload.get("x"))
|
||
y = float(payload.get("y"))
|
||
except Exception:
|
||
st("WARN", "SET_NAV_GOAL ignored: x/y required.")
|
||
return
|
||
nav_goal_xy = (x, y)
|
||
nav_path_world = []
|
||
nav_last_plan_t = 0.0
|
||
# An explicit operator goal pre-empts wander — Option 1 from the
|
||
# design (operator wins). Wander resumes after this goal is
|
||
# reached or cleared.
|
||
_wander_goal_origin = "operator"
|
||
st("INFO", {"NAV_GOAL": {"x": float(x), "y": float(y)}})
|
||
|
||
def clear_nav_goal() -> None:
|
||
nonlocal nav_goal_xy, nav_path_world, _wander_goal_origin
|
||
nav_goal_xy = None
|
||
nav_path_world = []
|
||
# Operator-cleared goal → also reset origin so wander knows it
|
||
# may pick again on the next tick.
|
||
_wander_goal_origin = "operator"
|
||
st("INFO", {"NAV_GOAL": None})
|
||
|
||
def mission_start(payload: Any) -> None:
|
||
nonlocal nav_goal_xy, nav_path_world, nav_last_plan_t
|
||
if not isinstance(payload, dict):
|
||
st("WARN", "MISSION_START ignored: payload must be dict.")
|
||
return
|
||
raw = payload.get("waypoints", [])
|
||
waypoints: List[Tuple[float, float]] = []
|
||
if isinstance(raw, list):
|
||
for wp in raw:
|
||
if isinstance(wp, dict):
|
||
try:
|
||
waypoints.append((float(wp.get("x")), float(wp.get("y"))))
|
||
except Exception:
|
||
continue
|
||
elif isinstance(wp, (list, tuple)) and len(wp) >= 2:
|
||
try:
|
||
waypoints.append((float(wp[0]), float(wp[1])))
|
||
except Exception:
|
||
continue
|
||
mission.start(waypoints=waypoints)
|
||
goal = mission.current_goal()
|
||
nav_goal_xy = None if goal is None else (float(goal[0]), float(goal[1]))
|
||
nav_path_world = []
|
||
nav_last_plan_t = 0.0
|
||
st("INFO", {"MISSION": mission.snapshot()})
|
||
|
||
def mission_pause() -> None:
|
||
mission.pause()
|
||
st("INFO", {"MISSION": mission.snapshot()})
|
||
|
||
def mission_resume() -> None:
|
||
mission.resume()
|
||
st("INFO", {"MISSION": mission.snapshot()})
|
||
|
||
def mission_stop() -> None:
|
||
nonlocal nav_goal_xy, nav_path_world
|
||
mission.stop()
|
||
nav_goal_xy = None
|
||
nav_path_world = []
|
||
st("INFO", {"MISSION": mission.snapshot()})
|
||
|
||
def _safe_record_base(name: str) -> str:
|
||
base = (name or "").strip() or "slam_recording"
|
||
for bad in ("/", "\\", ":", "*", "?", "\"", "<", ">", "|"):
|
||
base = base.replace(bad, "_")
|
||
return base
|
||
|
||
def record_start(payload: Any) -> None:
|
||
nonlocal recording_enabled, recording_frames, recording_poses, recording_base_name, recording_drop_count
|
||
base = recording_base_name
|
||
if isinstance(payload, dict):
|
||
b = str(payload.get("base_name", "")).strip()
|
||
if b:
|
||
base = b
|
||
recording_base_name = _safe_record_base(base)
|
||
recording_frames = []
|
||
recording_poses = []
|
||
recording_drop_count = 0
|
||
recording_enabled = True
|
||
st(
|
||
"INFO",
|
||
{
|
||
"RECORDING": {
|
||
"enabled": True,
|
||
"base_name": str(recording_base_name),
|
||
"max_frames": int(recording_max_frames),
|
||
}
|
||
},
|
||
)
|
||
|
||
def record_stop(payload: Any) -> None:
|
||
nonlocal recording_enabled, recording_frames, recording_poses
|
||
save = True
|
||
base = recording_base_name
|
||
if isinstance(payload, dict):
|
||
save = _as_bool(payload.get("save", True), True)
|
||
b = str(payload.get("base_name", "")).strip()
|
||
if b:
|
||
base = b
|
||
recording_enabled = False
|
||
if (not save) or len(recording_frames) == 0:
|
||
st(
|
||
"INFO",
|
||
{
|
||
"RECORDING": {
|
||
"enabled": False,
|
||
"frames": int(len(recording_frames)),
|
||
"saved": False,
|
||
}
|
||
},
|
||
)
|
||
recording_frames = []
|
||
recording_poses = []
|
||
return
|
||
base = _safe_record_base(base)
|
||
stamp = time.strftime("%Y%m%d_%H%M%S", time.localtime())
|
||
out_path = Path(map_cfg.data_folder) / f"{base}_{stamp}.npz"
|
||
try:
|
||
frames_obj = np.empty((len(recording_frames),), dtype=object)
|
||
for i, fr in enumerate(recording_frames):
|
||
frames_obj[i] = np.asarray(fr, dtype=np.float32)
|
||
if len(recording_poses) == len(recording_frames) and len(recording_poses) > 0:
|
||
poses_np = np.stack(recording_poses, axis=0).astype(np.float32)
|
||
np.savez_compressed(out_path, frames=frames_obj, poses=poses_np)
|
||
else:
|
||
np.savez_compressed(out_path, frames=frames_obj)
|
||
st(
|
||
"INFO",
|
||
{
|
||
"RECORD_SAVED": {
|
||
"path": str(out_path),
|
||
"frames": int(len(recording_frames)),
|
||
"dropped": int(recording_drop_count),
|
||
}
|
||
},
|
||
)
|
||
except Exception as e:
|
||
st("ERROR", f"RECORD save failed: {e}")
|
||
recording_frames = []
|
||
recording_poses = []
|
||
|
||
def _queue_lag() -> int:
|
||
try:
|
||
return int(data_q.qsize())
|
||
except Exception:
|
||
return -1
|
||
|
||
def _update_perf_rollup(now_t: float, stable_count: int) -> None:
|
||
nonlocal perf_input_count, perf_publish_count, perf_last_rate_t
|
||
nonlocal perf_last_cpu_wall_t, perf_last_cpu_proc_t
|
||
nonlocal perf_input_fps, perf_publish_fps, perf_cpu_pct
|
||
nonlocal perf_queue_lag, perf_stable_growth, perf_prev_stable_count, perf_prev_stable_t
|
||
|
||
dt_rates = float(now_t - perf_last_rate_t)
|
||
if dt_rates >= 0.8:
|
||
perf_input_fps = float(perf_input_count) / max(dt_rates, 1e-6)
|
||
perf_publish_fps = float(perf_publish_count) / max(dt_rates, 1e-6)
|
||
perf_input_count = 0
|
||
perf_publish_count = 0
|
||
perf_last_rate_t = now_t
|
||
|
||
dt_cpu = float(now_t - perf_last_cpu_wall_t)
|
||
if dt_cpu >= 0.8:
|
||
cpu_now = float(time.process_time())
|
||
perf_cpu_pct = 100.0 * float(cpu_now - perf_last_cpu_proc_t) / max(dt_cpu, 1e-6)
|
||
perf_last_cpu_proc_t = cpu_now
|
||
perf_last_cpu_wall_t = now_t
|
||
|
||
dt_growth = float(now_t - perf_prev_stable_t)
|
||
if dt_growth >= 0.8:
|
||
perf_stable_growth = float(stable_count - perf_prev_stable_count) / max(dt_growth, 1e-6)
|
||
perf_prev_stable_count = int(stable_count)
|
||
perf_prev_stable_t = now_t
|
||
|
||
perf_queue_lag = _queue_lag()
|
||
|
||
def maybe_autosave(now_t: float) -> None:
|
||
nonlocal last_autosave_t
|
||
if not autosave_enabled or not mapping_enabled:
|
||
return
|
||
if (now_t - float(last_autosave_t)) < float(autosave_interval_sec):
|
||
return
|
||
try:
|
||
if cleanup_enabled:
|
||
full_pts = stable.get_points()
|
||
if full_pts is not None and len(full_pts) >= int(cleanup_min_total_points):
|
||
cleaned, _ = cleanup_map_islands(full_pts)
|
||
if cleaned is not None and len(cleaned) >= int(map_cfg.min_points_to_save):
|
||
stable.set_points(cleaned)
|
||
pts = stable.get_save_points()
|
||
n = 0 if pts is None else int(len(pts))
|
||
req = int(map_cfg.min_points_to_save)
|
||
if n < req:
|
||
last_autosave_t = now_t
|
||
return
|
||
out_path = stable.export_map(str(autosave_base_name))
|
||
last_autosave_t = now_t
|
||
st("INFO", {"AUTOSAVED": {"path": str(out_path), "points": int(n)}})
|
||
except Exception as e:
|
||
last_autosave_t = now_t
|
||
st("WARN", f"AUTOSAVE failed: {e}")
|
||
|
||
# Checkpoint the submap alongside autosave
|
||
if submap_ckpt is not None and submap_mode_enabled:
|
||
submap_ckpt.save(submap_mapper)
|
||
|
||
# ---------------- lidar frame processor ----------------
|
||
def process_points(points: np.ndarray):
|
||
nonlocal last_pub, last_key_pose, last_frame_t
|
||
nonlocal last_guard_pose, guard_reject_count, guard_last_warn_t
|
||
nonlocal map_lock_last_t, map_lock_accept_count, map_lock_reject_count, map_lock_last_warn_t
|
||
nonlocal map_lock_consecutive_rejects, map_lock_consecutive_safe_rejects
|
||
nonlocal continuity_prev_pts, continuity_reject_count, continuity_consecutive_rejects, continuity_last_warn_t
|
||
nonlocal rotate_freeze_count, rotate_last_warn_t
|
||
nonlocal cleanup_last_t, cleanup_removed_total, cleanup_last_info
|
||
nonlocal perf_input_count, perf_publish_count, perf_icp_ms
|
||
nonlocal recording_enabled, recording_drop_count
|
||
nonlocal nav_goal_xy, nav_last_update_t, nav_last_plan_t, nav_path_world
|
||
nonlocal latest_live_localize_pts
|
||
nonlocal ref_nav_points_cache, ref_nav_cache_src_n
|
||
# Exploration intelligence state — these are reassigned (not
|
||
# just mutated) by the coverage-tick GC + the pose-quality
|
||
# auto-pause / explore-budget branches below.
|
||
nonlocal _coverage_visits
|
||
nonlocal _pose_quality_low_since, _pose_quality_auto_paused
|
||
nonlocal _explore_origin_pose, _explore_budget_deadline, _explore_returning_home
|
||
# Loop closure: _save_loop_keyframe rebinds _lc_keyframes (FIFO
|
||
# trim slice assign); _check_loop_closure rebinds the closure
|
||
# bookkeeping counters.
|
||
nonlocal _lc_keyframes, _lc_last_check_t
|
||
nonlocal _lc_closures_applied, _lc_last_closure_info
|
||
# process_points writes to _wander_goal_origin on the mission /
|
||
# wander-reset branches; without nonlocal Python treats it as a
|
||
# function-local and the status-payload read raises
|
||
# UnboundLocalError on the (very common) path where neither
|
||
# write fires this tick.
|
||
nonlocal _wander_goal_origin
|
||
|
||
if points is None or len(points) == 0:
|
||
return
|
||
|
||
perf_input_count += 1
|
||
|
||
pts = points.astype(np.float32, copy=True)
|
||
pts[:, 1] *= -1
|
||
pts[:, 2] *= -1
|
||
|
||
# Reject invalid values and out-of-range returns before SLAM.
|
||
finite_mask = np.isfinite(pts).all(axis=1)
|
||
if not np.all(finite_mask):
|
||
pts = pts[finite_mask]
|
||
if len(pts) == 0:
|
||
return
|
||
|
||
r2 = np.einsum("ij,ij->i", pts, pts)
|
||
min_r = float(map_quality_cfg.near_min_range_m if map_quality_cfg.enabled else 0.15)
|
||
min_r2 = min_r * min_r
|
||
max_r = float(eng_cfg.max_range)
|
||
max_r2 = max_r * max_r if max_r > 0 else np.inf
|
||
range_mask = (r2 >= min_r2) & (r2 <= max_r2)
|
||
if not np.all(range_mask):
|
||
pts = pts[range_mask]
|
||
if len(pts) < 40:
|
||
return
|
||
|
||
# pre-downsample stride from config
|
||
stride = int(live_stride)
|
||
if stride > 1:
|
||
pts = pts[::stride]
|
||
if map_quality_cfg.enabled:
|
||
pts = map_quality.apply_sensor(pts)
|
||
if len(pts) < 40:
|
||
return
|
||
latest_live_localize_pts = None
|
||
|
||
pose = None
|
||
stable_pts_display = None
|
||
stable_cols = None
|
||
fusion_info: Dict[str, Any] = {}
|
||
nav_runtime_info = None
|
||
nav_info: Dict[str, Any] = {}
|
||
safety_info: Dict[str, Any] = {}
|
||
map_lock_info: Dict[str, Any] = {"status": "skip", "ran": False}
|
||
continuity_info: Dict[str, Any] = {"status": "na"}
|
||
cleanup_info: Dict[str, Any] = dict(cleanup_last_info) if isinstance(cleanup_last_info, dict) else {"status": "na"}
|
||
rotate_freeze_active = False
|
||
submap_info: Dict[str, Any] = submap_mapper.status(
|
||
active=False,
|
||
reason="idle",
|
||
profile=str(stability_profile),
|
||
profile_allowed=_submap_profile_allowed(),
|
||
)
|
||
ref_match_points_pub = None
|
||
ref_match_colors_pub = None
|
||
ref_match_stats: Dict[str, Any] = {}
|
||
stable_points_in_global_frame = False
|
||
mode_now = str(worker_mode)
|
||
|
||
if mapping_enabled:
|
||
now_t = _now()
|
||
dt = 0.1 if last_frame_t <= 0 else float(np.clip(now_t - last_frame_t, 0.02, 0.25))
|
||
last_frame_t = now_t
|
||
ts = np.linspace(0.0, dt, len(pts), dtype=np.float64)
|
||
|
||
t_icp0 = _now()
|
||
slam.register_frame(pts, ts)
|
||
perf_icp_ms = 1000.0 * (_now() - t_icp0)
|
||
odom_pose = slam.last_pose if hasattr(slam, "last_pose") else getattr(slam, "pose", None)
|
||
if odom_pose is not None:
|
||
odom_pose, fusion_info = sensor_fusion.fuse_pose(np.asarray(odom_pose, dtype=np.float64), now=now_t)
|
||
try:
|
||
pose = np.asarray(odom_to_map_transform, dtype=np.float64) @ np.asarray(odom_pose, dtype=np.float64)
|
||
except Exception:
|
||
pose = np.asarray(odom_pose, dtype=np.float64)
|
||
|
||
# Reject obviously unstable jumps to avoid "wall on wall" artifacts.
|
||
unstable_jump = False
|
||
trans_step_for_mode = 0.0
|
||
rot_step_for_mode = 0.0
|
||
rotating_in_place = False
|
||
if pose_guard_enabled and pose is not None:
|
||
pose_np = np.asarray(pose, dtype=np.float64)
|
||
if last_guard_pose is not None:
|
||
dpos = pose_np[:3, 3] - last_guard_pose[:3, 3]
|
||
trans_step = float(np.linalg.norm(dpos))
|
||
rrel = pose_np[:3, :3] @ last_guard_pose[:3, :3].T
|
||
rot_step = float(np.degrees(np.arccos(np.clip((np.trace(rrel) - 1.0) * 0.5, -1.0, 1.0))))
|
||
trans_step_for_mode = float(trans_step)
|
||
rot_step_for_mode = float(rot_step)
|
||
if rotate_mode_enabled and trans_step_for_mode <= float(rotate_in_place_max_trans_m) and rot_step_for_mode >= float(rotate_in_place_min_rot_deg):
|
||
rotating_in_place = True
|
||
scale = max(1.0, float(dt) / max(1e-3, float(pose_guard_ref_dt)))
|
||
max_t = float(pose_guard_max_trans) * scale
|
||
max_r = float(pose_guard_max_rot_deg) * scale
|
||
if rotating_in_place:
|
||
max_r = max(max_r, float(rotate_pose_guard_max_rot_deg) * scale)
|
||
if trans_step > max_t or rot_step > max_r:
|
||
unstable_jump = True
|
||
guard_reject_count += 1
|
||
pose = np.array(last_guard_pose, dtype=np.float64, copy=True)
|
||
stable_pts_display = stable.get_display_points()
|
||
if (now_t - guard_last_warn_t) > 1.0:
|
||
guard_last_warn_t = now_t
|
||
st(
|
||
"WARN",
|
||
(
|
||
"MOTION_GUARD rejected frame: "
|
||
f"dpos={trans_step:.2f}m>{max_t:.2f}m or drot={rot_step:.1f}deg>{max_r:.1f}deg "
|
||
f"(count={guard_reject_count})"
|
||
),
|
||
)
|
||
else:
|
||
last_guard_pose = np.array(pose_np, dtype=np.float64, copy=True)
|
||
else:
|
||
last_guard_pose = np.array(pose_np, dtype=np.float64, copy=True)
|
||
elif pose is not None and rotate_mode_enabled and last_key_pose is not None:
|
||
# Fallback rotate-in-place detector when pose-guard is disabled.
|
||
try:
|
||
dpos = np.asarray(pose, dtype=np.float64)[:3, 3] - np.asarray(last_key_pose, dtype=np.float64)[:3, 3]
|
||
trans_step_for_mode = float(np.linalg.norm(dpos))
|
||
rrel = np.asarray(pose, dtype=np.float64)[:3, :3] @ np.asarray(last_key_pose, dtype=np.float64)[:3, :3].T
|
||
rot_step_for_mode = float(np.degrees(np.arccos(np.clip((np.trace(rrel) - 1.0) * 0.5, -1.0, 1.0))))
|
||
rotating_in_place = bool(
|
||
trans_step_for_mode <= float(rotate_in_place_max_trans_m)
|
||
and rot_step_for_mode >= float(rotate_in_place_min_rot_deg)
|
||
)
|
||
except Exception:
|
||
rotating_in_place = False
|
||
|
||
if not unstable_jump:
|
||
map_lock_block_frame = False
|
||
# Periodic scan-to-map correction keeps the accumulated map anchored
|
||
# and reduces duplicated structures when traversing long paths.
|
||
if pose is not None:
|
||
ml_overrides = None
|
||
if rotating_in_place and rotate_mode_enabled:
|
||
ml_overrides = {
|
||
"period_sec": float(rotate_map_lock_period_sec),
|
||
"accept_fitness": float(rotate_map_lock_accept_fitness),
|
||
"accept_rmse": float(rotate_map_lock_accept_rmse),
|
||
"max_step_rotation_deg": float(rotate_map_lock_max_step_rot_deg),
|
||
}
|
||
corr_lock, map_lock_info = compute_map_lock_correction(pts, pose, now_t, overrides=ml_overrides)
|
||
if corr_lock is not None:
|
||
corr_np = np.asarray(corr_lock, dtype=np.float64)
|
||
corr_apply = np.asarray(corr_np, dtype=np.float64, copy=True)
|
||
if map_lock_apply_damping:
|
||
alpha_t = float(map_lock_apply_alpha_t)
|
||
alpha_r = float(map_lock_apply_alpha_r)
|
||
max_t_apply = float(map_lock_apply_max_trans_m)
|
||
max_r_apply = float(map_lock_apply_max_rot_deg)
|
||
if rotating_in_place and rotate_mode_enabled:
|
||
alpha_t = float(rotate_map_lock_apply_alpha_t)
|
||
alpha_r = float(rotate_map_lock_apply_alpha_r)
|
||
max_t_apply = float(rotate_map_lock_apply_max_trans_m)
|
||
max_r_apply = float(rotate_map_lock_apply_max_rot_deg)
|
||
corr_apply = _blend_rigid_tf(
|
||
np.eye(4, dtype=np.float64),
|
||
corr_apply,
|
||
alpha_t=float(alpha_t),
|
||
alpha_r=float(alpha_r),
|
||
)
|
||
tvec = np.asarray(corr_apply[:3, 3], dtype=np.float64)
|
||
tnorm = float(np.linalg.norm(tvec))
|
||
if tnorm > float(max_t_apply) and tnorm > 1e-9:
|
||
corr_apply[:3, 3] = tvec * (float(max_t_apply) / tnorm)
|
||
dpos_apply, drot_apply = _tf_delta(np.eye(4, dtype=np.float64), corr_apply)
|
||
if drot_apply > float(max_r_apply) and drot_apply > 1e-6:
|
||
corr_apply = _blend_rigid_tf(
|
||
np.eye(4, dtype=np.float64),
|
||
corr_apply,
|
||
alpha_t=1.0,
|
||
alpha_r=float(max_r_apply) / float(drot_apply),
|
||
)
|
||
else:
|
||
dpos_apply, drot_apply = _tf_delta(np.eye(4, dtype=np.float64), corr_apply)
|
||
map_lock_info["applied_dpos_m"] = float(dpos_apply)
|
||
map_lock_info["applied_drot_deg"] = float(drot_apply)
|
||
map_lock_info["damped"] = bool(map_lock_apply_damping)
|
||
pose = corr_apply @ np.asarray(pose, dtype=np.float64)
|
||
loc_frames.apply_map_correction(corr_apply)
|
||
_sync_loc_vars_from_service()
|
||
if last_guard_pose is not None:
|
||
last_guard_pose = corr_apply @ np.asarray(last_guard_pose, dtype=np.float64)
|
||
if last_key_pose is not None:
|
||
last_key_pose = corr_apply @ np.asarray(last_key_pose, dtype=np.float64)
|
||
map_lock_accept_count += 1
|
||
# A successful map_lock accept means alignment
|
||
# is good — clear BOTH consecutive-reject streaks
|
||
# so the auto-recover threshold restarts fresh.
|
||
map_lock_consecutive_rejects = 0
|
||
map_lock_consecutive_safe_rejects = 0
|
||
elif bool(map_lock_info.get("ran", False)):
|
||
map_lock_reject_count += 1
|
||
map_lock_consecutive_rejects += 1
|
||
# Classify severity NOW so the safe-streak
|
||
# counter is accurate even if `block_on_severe`
|
||
# is disabled. Severe = pose really drifted →
|
||
# safe streak resets so auto-recover can't fire
|
||
# on this run of rejections.
|
||
_fit_r_now = float(map_lock_info.get("fitness", 0.0))
|
||
_rmse_r_now = float(map_lock_info.get("rmse", 9e9))
|
||
_dpos_r_now = float(map_lock_info.get("dpos_m", 0.0))
|
||
_drot_r_now = float(map_lock_info.get("drot_deg", 0.0))
|
||
_is_severe_now = bool(
|
||
_fit_r_now <= float(map_lock_severe_fit)
|
||
or _rmse_r_now >= float(map_lock_severe_rmse)
|
||
or _dpos_r_now >= float(map_lock_severe_trans_m)
|
||
or _drot_r_now >= float(map_lock_severe_rot_deg)
|
||
)
|
||
if _is_severe_now:
|
||
map_lock_consecutive_safe_rejects = 0
|
||
else:
|
||
map_lock_consecutive_safe_rejects += 1
|
||
block_on_reject_now = bool(map_lock_block_on_reject)
|
||
if rotating_in_place and rotate_mode_enabled:
|
||
block_on_reject_now = bool(rotate_map_lock_block_on_reject)
|
||
if block_on_reject_now:
|
||
map_lock_block_frame = True
|
||
# Retry map-lock quickly after a reject to recover alignment faster.
|
||
map_lock_last_t = float(now_t - map_lock_period_sec + map_lock_retry_after_sec)
|
||
if (now_t - map_lock_last_warn_t) > 1.0:
|
||
map_lock_last_warn_t = now_t
|
||
fit_r = float(map_lock_info.get("fitness", 0.0))
|
||
rmse_r = float(map_lock_info.get("rmse", 9e9))
|
||
dpos_r = float(map_lock_info.get("dpos_m", 0.0))
|
||
drot_r = float(map_lock_info.get("drot_deg", 0.0))
|
||
st(
|
||
"WARN",
|
||
(
|
||
"MAP_LOCK rejected; skipping frame integration: "
|
||
f"fit={fit_r:.2f}, rmse={rmse_r:.2f}, "
|
||
f"dpos={dpos_r:.2f}m, drot={drot_r:.1f}deg"
|
||
),
|
||
)
|
||
block_on_severe_now = bool(map_lock_block_on_severe)
|
||
if rotating_in_place and rotate_mode_enabled:
|
||
block_on_severe_now = bool(rotate_map_lock_block_on_severe)
|
||
if block_on_severe_now:
|
||
fit_r = float(map_lock_info.get("fitness", 0.0))
|
||
rmse_r = float(map_lock_info.get("rmse", 9e9))
|
||
dpos_r = float(map_lock_info.get("dpos_m", 0.0))
|
||
drot_r = float(map_lock_info.get("drot_deg", 0.0))
|
||
severe_reject = bool(
|
||
fit_r <= float(map_lock_severe_fit)
|
||
or rmse_r >= float(map_lock_severe_rmse)
|
||
or dpos_r >= float(map_lock_severe_trans_m)
|
||
or drot_r >= float(map_lock_severe_rot_deg)
|
||
)
|
||
if severe_reject:
|
||
map_lock_block_frame = True
|
||
if (now_t - map_lock_last_warn_t) > 1.0:
|
||
map_lock_last_warn_t = now_t
|
||
st(
|
||
"WARN",
|
||
(
|
||
"MAP_LOCK severe reject; skipping frame integration: "
|
||
f"fit={fit_r:.2f}, rmse={rmse_r:.2f}, "
|
||
f"dpos={dpos_r:.2f}m, drot={drot_r:.1f}deg"
|
||
),
|
||
)
|
||
|
||
# ── MAP_LOCK safe auto-recover ───────────────────────
|
||
# Fires only when the gate has been rejecting NON-severe
|
||
# frames for ~1.5 s straight (25 frames @ 20 Hz). A
|
||
# severe rejection during the streak resets the safe
|
||
# counter, so this path can never force-accept a frame
|
||
# the gate flagged as "real pose drift" — that would
|
||
# cause duplicate walls / map ghosting. The conservative
|
||
# branch (the original behavior — block forever) still
|
||
# applies whenever any severe rejection happens.
|
||
if (
|
||
map_lock_block_frame
|
||
and map_lock_consecutive_safe_rejects >= int(map_lock_auto_recover_after)
|
||
):
|
||
map_lock_block_frame = False
|
||
map_lock_consecutive_safe_rejects = 0
|
||
map_lock_consecutive_rejects = 0
|
||
# Trigger a fresh map_lock run on the next tick so
|
||
# the gate's stored reference re-aligns to the
|
||
# newly accepted frame.
|
||
map_lock_last_t = float(now_t - map_lock_period_sec + map_lock_retry_after_sec)
|
||
if (now_t - map_lock_last_warn_t) > 1.0:
|
||
map_lock_last_warn_t = now_t
|
||
st(
|
||
"WARN",
|
||
(
|
||
"MAP_LOCK safe auto-recover after "
|
||
f"{int(map_lock_auto_recover_after)} non-severe rejects; "
|
||
"resuming integration"
|
||
),
|
||
)
|
||
|
||
# keyframe gating from config
|
||
if map_lock_block_frame:
|
||
stable_pts_display = stable.get_display_points()
|
||
elif eng_cfg.keyframe_enabled and pose is not None and not pose_delta_ok(pose, last_key_pose):
|
||
# skip filter/map update
|
||
stable_pts_display = stable.get_display_points()
|
||
else:
|
||
if pose is not None:
|
||
last_key_pose = pose
|
||
|
||
points_world = to_world_points(pts, pose)
|
||
stable_now = stable.get_points()
|
||
stable_now_n = 0 if stable_now is None else int(len(stable_now))
|
||
rotate_freeze_active = bool(
|
||
rotate_mode_enabled
|
||
and rotate_freeze_mapping
|
||
and rotating_in_place
|
||
and stable_now_n >= int(rotate_freeze_min_stable_pts)
|
||
)
|
||
if rotate_freeze_active:
|
||
rotate_freeze_count += 1
|
||
continuity_consecutive_rejects = 0
|
||
continuity_pts_ds = voxel_downsample_np(points_world, float(continuity_voxel_m))
|
||
continuity_prev_pts = (
|
||
np.array(continuity_pts_ds, dtype=np.float32, copy=True)
|
||
if continuity_pts_ds is not None and len(continuity_pts_ds) > 0
|
||
else continuity_prev_pts
|
||
)
|
||
continuity_info = {
|
||
"status": "skip_rotate_freeze",
|
||
"trans_step_m": float(trans_step_for_mode),
|
||
"rot_step_deg": float(rot_step_for_mode),
|
||
"stable_points": int(stable_now_n),
|
||
"freeze_count": int(rotate_freeze_count),
|
||
}
|
||
stable_pts_display = stable.get_display_points()
|
||
if (now_t - rotate_last_warn_t) > 1.0:
|
||
rotate_last_warn_t = now_t
|
||
st(
|
||
"INFO",
|
||
(
|
||
"ROTATE_FREEZE active; skipping map integration during in-place turn "
|
||
f"(stable={stable_now_n}, dpos={trans_step_for_mode:.3f}m, drot={rot_step_for_mode:.1f}deg)"
|
||
),
|
||
)
|
||
else:
|
||
if rotating_in_place and rotate_mode_enabled and rotate_skip_continuity:
|
||
continuity_ok = True
|
||
continuity_pts_ds = voxel_downsample_np(points_world, float(continuity_voxel_m))
|
||
continuity_info = {
|
||
"status": "skip_rotating_in_place",
|
||
"trans_step_m": float(trans_step_for_mode),
|
||
"rot_step_deg": float(rot_step_for_mode),
|
||
}
|
||
else:
|
||
continuity_ok, continuity_info, continuity_pts_ds = check_frame_continuity(points_world)
|
||
if not continuity_ok:
|
||
continuity_reject_count += 1
|
||
continuity_consecutive_rejects += 1
|
||
if continuity_consecutive_rejects >= int(continuity_recover_after_rejects):
|
||
continuity_ok = True
|
||
continuity_consecutive_rejects = 0
|
||
continuity_info = {
|
||
"status": "resync_after_rejects",
|
||
"reject_streak": int(continuity_recover_after_rejects),
|
||
"trans_step_m": float(trans_step_for_mode),
|
||
"rot_step_deg": float(rot_step_for_mode),
|
||
}
|
||
continuity_prev_pts = (
|
||
np.array(continuity_pts_ds, dtype=np.float32, copy=True)
|
||
if continuity_pts_ds is not None and len(continuity_pts_ds) > 0
|
||
else continuity_prev_pts
|
||
)
|
||
st(
|
||
"WARN",
|
||
(
|
||
"CONTINUITY auto-resync after repeated rejects; "
|
||
f"recover_after={int(continuity_recover_after_rejects)}"
|
||
),
|
||
)
|
||
if not continuity_ok:
|
||
stable_pts_display = stable.get_display_points()
|
||
# Trigger a quick map-lock retry after continuity rejection.
|
||
map_lock_last_t = float(now_t - map_lock_period_sec + map_lock_retry_after_sec)
|
||
if (now_t - continuity_last_warn_t) > 1.0:
|
||
continuity_last_warn_t = now_t
|
||
st(
|
||
"WARN",
|
||
(
|
||
"CONTINUITY rejected; skipping frame integration: "
|
||
f"inlier={float(continuity_info.get('inlier_ratio', 0.0)):.2f}, "
|
||
f"med={float(continuity_info.get('median_m', 0.0)):.2f}m, "
|
||
f"p90={float(continuity_info.get('p90_m', 0.0)):.2f}m "
|
||
f"(count={int(continuity_reject_count)})"
|
||
),
|
||
)
|
||
else:
|
||
continuity_consecutive_rejects = 0
|
||
continuity_prev_pts = (
|
||
np.array(continuity_pts_ds, dtype=np.float32, copy=True)
|
||
if continuity_pts_ds is not None and len(continuity_pts_ds) > 0
|
||
else None
|
||
)
|
||
if map_quality_cfg.enabled:
|
||
points_world = map_quality.apply_world(points_world)
|
||
filt.update(points_world)
|
||
stable_pts = filt.get_stable_points()
|
||
stable.set_points(stable_pts)
|
||
if (
|
||
cleanup_enabled
|
||
and stable_pts is not None
|
||
and len(stable_pts) >= int(cleanup_min_total_points)
|
||
and (now_t - float(cleanup_last_t)) >= float(cleanup_period_sec)
|
||
):
|
||
cleanup_last_t = float(now_t)
|
||
cleaned_pts, cleanup_info = cleanup_map_islands(stable_pts)
|
||
cleanup_last_info = dict(cleanup_info)
|
||
removed_now = int(cleanup_info.get("removed", 0))
|
||
if removed_now > 0:
|
||
cleanup_removed_total += int(removed_now)
|
||
stable.set_points(cleaned_pts)
|
||
if hasattr(filt, "seed_stable_points"):
|
||
try:
|
||
filt.seed_stable_points(
|
||
cleaned_pts,
|
||
now=now_t,
|
||
hit_count=int(runtime_filter_hit_threshold),
|
||
clear_existing=True,
|
||
)
|
||
except Exception as _cln_exc:
|
||
st("WARN", f"[cleanup] filter re-seed after cleanup failed: {_cln_exc}")
|
||
st(
|
||
"INFO",
|
||
{
|
||
"MAP_CLEANUP": {
|
||
**cleanup_info,
|
||
"removed_total": int(cleanup_removed_total),
|
||
}
|
||
},
|
||
)
|
||
else:
|
||
cleanup_info = dict(cleanup_last_info) if isinstance(cleanup_last_info, dict) else {"status": "na"}
|
||
stable_pts_display = stable.get_display_points()
|
||
|
||
# Lightweight loop closure + pose-graph optimization.
|
||
if pose is not None and loop_cfg.enabled:
|
||
loop_res = loop_backend.process_frame(pts, np.asarray(pose, dtype=np.float64))
|
||
if loop_res.loop_detected or loop_res.optimized or ("rejected" in loop_res.info):
|
||
st(
|
||
"INFO",
|
||
{
|
||
"LOOP": {
|
||
"detected": bool(loop_res.loop_detected),
|
||
"optimized": bool(loop_res.optimized),
|
||
**loop_res.info,
|
||
}
|
||
},
|
||
)
|
||
if loop_res.optimized:
|
||
corr = np.asarray(loop_res.correction, dtype=np.float64)
|
||
# Apply correction to map and reseed filter for consistency.
|
||
stable_all = stable.get_points()
|
||
corrected = apply_transform_points(stable_all, corr)
|
||
if corrected is not None:
|
||
stable.set_points(corrected)
|
||
if hasattr(filt, "seed_stable_points"):
|
||
filt.seed_stable_points(
|
||
corrected,
|
||
now=_now(),
|
||
hit_count=int(filt_cfg.hits_threshold),
|
||
clear_existing=True,
|
||
)
|
||
stable_pts_display = stable.get_display_points()
|
||
loc_frames.apply_loop_correction(corr, update_reference=True)
|
||
_sync_loc_vars_from_service()
|
||
if last_guard_pose is not None:
|
||
last_guard_pose = corr @ np.asarray(last_guard_pose, dtype=np.float64)
|
||
if last_key_pose is not None:
|
||
last_key_pose = corr @ np.asarray(last_key_pose, dtype=np.float64)
|
||
pose = corr @ np.asarray(pose, dtype=np.float64)
|
||
maybe_auto_localize()
|
||
maybe_autosave(now_t)
|
||
if submap_ckpt is not None and submap_mode_enabled:
|
||
submap_ckpt.maybe_save(submap_mapper)
|
||
elif localize_only_enabled:
|
||
now_t = _now()
|
||
dt = 0.1 if last_frame_t <= 0 else float(np.clip(now_t - last_frame_t, 0.02, 0.25))
|
||
last_frame_t = now_t
|
||
ts = np.linspace(0.0, dt, len(pts), dtype=np.float64)
|
||
t_icp0 = _now()
|
||
slam.register_frame(pts, ts)
|
||
perf_icp_ms = 1000.0 * (_now() - t_icp0)
|
||
odom_pose = slam.last_pose if hasattr(slam, "last_pose") else getattr(slam, "pose", None)
|
||
if odom_pose is not None:
|
||
odom_pose, fusion_info = sensor_fusion.fuse_pose(np.asarray(odom_pose, dtype=np.float64), now=now_t)
|
||
pose = np.asarray(odom_pose, dtype=np.float64)
|
||
else:
|
||
pose = None
|
||
|
||
live_for_localize = np.asarray(pts, dtype=np.float32)
|
||
if pose is not None:
|
||
live_for_localize = to_world_points(pts, pose)
|
||
|
||
if loc_live_stack_enabled:
|
||
frame_ds = voxel_downsample_np(live_for_localize, float(loc_live_stack_voxel_m))
|
||
if frame_ds is not None and len(frame_ds) > 0:
|
||
loc_live_stack.append(np.asarray(frame_ds, dtype=np.float32))
|
||
if len(loc_live_stack) >= int(loc_live_stack_min_frames):
|
||
cat = np.concatenate(list(loc_live_stack), axis=0)
|
||
stack_pts = voxel_downsample_np(cat, float(loc_live_stack_voxel_m))
|
||
if len(stack_pts) > int(loc_live_stack_max_points):
|
||
step = max(2, int(np.ceil(float(len(stack_pts)) / float(loc_live_stack_max_points))))
|
||
stack_pts = stack_pts[::step][: int(loc_live_stack_max_points)]
|
||
if stack_pts is not None and len(stack_pts) > 0:
|
||
live_for_localize = np.asarray(stack_pts, dtype=np.float32)
|
||
|
||
latest_live_localize_pts = np.array(live_for_localize, dtype=np.float32, copy=True)
|
||
maybe_auto_localize(src_live=live_for_localize)
|
||
loc_state_name = str(loc_state.state).upper().strip()
|
||
submap_active = bool(
|
||
submap_mode_enabled
|
||
and _submap_profile_allowed()
|
||
and slam_to_ref_valid
|
||
and loc_state_name in ("TRACKING", "DEGRADED")
|
||
and float(last_localize_confidence) >= 0.45
|
||
)
|
||
if submap_active:
|
||
if pose is not None and np.asarray(pose).shape == (4, 4):
|
||
pose_ref = np.asarray(slam_to_ref_transform, dtype=np.float64) @ np.asarray(pose, dtype=np.float64)
|
||
pts_ref = to_world_points(pts, pose_ref)
|
||
else:
|
||
pts_ref = apply_transform_points(pts, slam_to_ref_transform)
|
||
pose_ref = (
|
||
np.asarray(slam_to_ref_transform, dtype=np.float64)
|
||
if np.asarray(slam_to_ref_transform).shape == (4, 4)
|
||
else None
|
||
)
|
||
if pts_ref is not None and len(pts_ref) > 0:
|
||
submap_info = submap_mapper.integrate(pts_ref, now=now_t, pose_world=pose_ref)
|
||
stable_pts_display = submap_mapper.get_display_points()
|
||
stable_points_in_global_frame = True
|
||
else:
|
||
stable_pts_display = live_for_localize
|
||
submap_info = submap_mapper.status(
|
||
active=False,
|
||
reason="empty_live",
|
||
profile=str(stability_profile),
|
||
profile_allowed=True,
|
||
)
|
||
else:
|
||
if (not submap_mode_enabled) or (not _submap_profile_allowed()) or (not slam_to_ref_valid):
|
||
if submap_mapper.has_points and ((not _submap_profile_allowed()) or (not submap_mode_enabled) or (not slam_to_ref_valid)):
|
||
submap_mapper.reset()
|
||
stable_pts_display = live_for_localize
|
||
submap_info = submap_mapper.status(
|
||
active=False,
|
||
reason="disabled_or_not_localized",
|
||
profile=str(stability_profile),
|
||
profile_allowed=_submap_profile_allowed(),
|
||
)
|
||
# Keep pose in odom frame; publish path maps it to reference when localized.
|
||
else:
|
||
stable_pts_display = stable.get_display_points()
|
||
|
||
if recording_enabled:
|
||
if len(recording_frames) < int(recording_max_frames):
|
||
recording_frames.append(np.array(pts, dtype=np.float32, copy=True))
|
||
if pose is not None and np.asarray(pose).shape == (4, 4):
|
||
recording_poses.append(np.array(pose, dtype=np.float32, copy=True))
|
||
else:
|
||
recording_drop_count += 1
|
||
recording_enabled = False
|
||
st(
|
||
"WARN",
|
||
(
|
||
"RECORDING auto-stopped: reached max frames "
|
||
f"({int(recording_max_frames)})."
|
||
),
|
||
)
|
||
|
||
# publish throttling
|
||
t = _now()
|
||
if pub_hz > 0:
|
||
if (t - last_pub) < (1.0 / pub_hz):
|
||
return
|
||
last_pub = t
|
||
|
||
stable_pts_pub = stable_pts_display
|
||
pose_pub = pose
|
||
if slam_to_ref_valid and not stable_points_in_global_frame:
|
||
stable_pts_pub = apply_transform_points(stable_pts_display, slam_to_ref_transform)
|
||
if pose is not None:
|
||
try:
|
||
pose_pub = slam_to_ref_transform @ np.asarray(pose, dtype=np.float64)
|
||
except Exception:
|
||
pose_pub = pose
|
||
|
||
# LIVE-SCAN FALLBACK for nav workflows that don't grow a stable
|
||
# buffer. Without this the dashboard would render whatever was
|
||
# accumulated up to the first frame and then go silent — looks
|
||
# frozen even while the robot is moving. In LIVE_NAV_NO_MAP we
|
||
# substitute the current frame's points (transformed to world
|
||
# frame) so the operator sees a real sweeping lidar view.
|
||
# Mapping workflows (MAP_NEW, EXTEND_MAP) keep the accumulated
|
||
# cloud because that IS the map; nav-with-ref workflows keep
|
||
# ref_match_points for the same reason.
|
||
try:
|
||
_wf_now = str(stability_profile).upper().strip()
|
||
except Exception:
|
||
_wf_now = ""
|
||
if _wf_now == "LIVE_NAV_NO_MAP":
|
||
try:
|
||
if pts is not None and len(pts) > 0 and pose is not None:
|
||
live_world = to_world_points(pts, pose)
|
||
if live_world is not None and len(live_world) > 0:
|
||
# Downsample to keep the display set ~5k after
|
||
# voxel filter would have run on a stable map.
|
||
step = max(1, len(live_world) // 5000)
|
||
stable_pts_pub = np.ascontiguousarray(
|
||
live_world[::step], dtype=np.float32,
|
||
)
|
||
except Exception:
|
||
pass
|
||
|
||
if stable_pts_pub is not None and len(stable_pts_pub) > 0:
|
||
if localize_only_enabled:
|
||
# Blue live overlay for map localization workflows.
|
||
stable_cols = np.zeros((len(stable_pts_pub), 4), dtype=np.float32)
|
||
stable_cols[:, 0] = 0.18
|
||
stable_cols[:, 1] = 0.58
|
||
stable_cols[:, 2] = 1.00
|
||
stable_cols[:, 3] = 0.72
|
||
else:
|
||
alpha_live = 0.90
|
||
stable_cols = height_colors_fast(stable_pts_pub, alpha=float(alpha_live))
|
||
|
||
live_world_for_nav = None
|
||
if localize_only_enabled:
|
||
if slam_to_ref_valid:
|
||
if pose is not None and np.asarray(pose).shape == (4, 4):
|
||
try:
|
||
pose_ref_live = np.asarray(slam_to_ref_transform, dtype=np.float64) @ np.asarray(pose, dtype=np.float64)
|
||
live_world_for_nav = to_world_points(pts, pose_ref_live)
|
||
except Exception:
|
||
live_world_for_nav = apply_transform_points(pts, slam_to_ref_transform)
|
||
else:
|
||
live_world_for_nav = apply_transform_points(pts, slam_to_ref_transform)
|
||
else:
|
||
if pose_pub is not None:
|
||
live_world_for_nav = to_world_points(pts, np.asarray(pose_pub, dtype=np.float64))
|
||
elif slam_to_ref_valid:
|
||
live_world_for_nav = apply_transform_points(pts, slam_to_ref_transform)
|
||
|
||
if localize_only_enabled:
|
||
pose_ref_for_vis = None
|
||
if slam_to_ref_valid and pose_pub is not None and np.asarray(pose_pub).shape == (4, 4):
|
||
pose_ref_for_vis = np.asarray(pose_pub, dtype=np.float64)
|
||
elif slam_to_ref_valid:
|
||
pose_ref_for_vis = np.asarray(slam_to_ref_transform, dtype=np.float64)
|
||
ref_match_points_pub, ref_match_colors_pub, ref_match_stats = _build_ref_match_visual(
|
||
live_world_for_nav,
|
||
pose_ref_for_vis,
|
||
_now(),
|
||
)
|
||
else:
|
||
ref_match_stats = {"enabled": bool(loc_vis_enabled), "updated": False, "reason": "not_localize_mode"}
|
||
|
||
nav_static_points = stable_pts_pub
|
||
if localize_only_enabled and ref_points_cache is not None and len(ref_points_cache) > 0:
|
||
try:
|
||
src_n = int(len(ref_points_cache))
|
||
if ref_nav_points_cache is None or int(ref_nav_cache_src_n) != src_n:
|
||
nav_voxel = max(0.08, float(nav_rt_cfg.resolution_m) * 1.25)
|
||
nav_pts = voxel_downsample_np(np.asarray(ref_points_cache, dtype=np.float32), float(nav_voxel))
|
||
if nav_pts is None:
|
||
nav_pts = np.zeros((0, 3), dtype=np.float32)
|
||
if len(nav_pts) > 300000:
|
||
step = max(2, int(np.ceil(float(len(nav_pts)) / 300000.0)))
|
||
nav_pts = nav_pts[::step]
|
||
ref_nav_points_cache = np.asarray(nav_pts, dtype=np.float32)
|
||
ref_nav_cache_src_n = src_n
|
||
if ref_nav_points_cache is not None and len(ref_nav_points_cache) > 0:
|
||
nav_static_points = ref_nav_points_cache
|
||
except Exception as _nav_exc:
|
||
st("WARN", f"[nav] static point cache update failed: {_nav_exc}")
|
||
|
||
if (t - nav_last_update_t) >= 0.10:
|
||
nav_runtime_info = nav_runtime.update(nav_static_points, live_world_for_nav, t)
|
||
nav_last_update_t = t
|
||
|
||
mission_snapshot = mission.snapshot()
|
||
if pose_pub is not None and np.asarray(pose_pub).shape == (4, 4):
|
||
mission_snapshot = mission.update_pose(float(pose_pub[0, 3]), float(pose_pub[1, 3]))
|
||
goal_from_mission = mission.current_goal()
|
||
if goal_from_mission is not None:
|
||
nav_goal_xy = (float(goal_from_mission[0]), float(goal_from_mission[1]))
|
||
_wander_goal_origin = "operator" # mission goal pre-empts wander
|
||
elif mission_snapshot.get("completed", False):
|
||
nav_goal_xy = None
|
||
nav_path_world = []
|
||
|
||
# COVERAGE MEMORY tick (#2): record the robot's current 2 m
|
||
# cell so future wander goal picks penalize re-visiting it.
|
||
# Only count when localized (pose_pub is not None) — otherwise
|
||
# we'd update with stale or zero pose. Increment is cheap O(1).
|
||
if pose_pub is not None:
|
||
try:
|
||
_cgx = int(math.floor(float(pose_pub[0, 3]) / 2.0))
|
||
_cgy = int(math.floor(float(pose_pub[1, 3]) / 2.0))
|
||
_coverage_visits[(_cgx, _cgy)] = (
|
||
_coverage_visits.get((_cgx, _cgy), 0) + 1
|
||
)
|
||
# Bound the dict so a multi-hour mapping run doesn't
|
||
# grow unbounded — drop old (low-count) entries when
|
||
# we exceed 10k cells.
|
||
if len(_coverage_visits) > 10000:
|
||
_kept = sorted(
|
||
_coverage_visits.items(),
|
||
key=lambda kv: kv[1],
|
||
reverse=True,
|
||
)[:8000]
|
||
_coverage_visits = dict(_kept)
|
||
except Exception:
|
||
pass
|
||
|
||
# Loop closure: save a keyframe when robot has moved enough,
|
||
# and periodically check for revisits. Both no-op when the
|
||
# config flag is off — gated inside the helpers.
|
||
if pose_pub is not None:
|
||
try:
|
||
_save_loop_keyframe(
|
||
(float(pose_pub[0, 3]), float(pose_pub[1, 3])), t,
|
||
)
|
||
_check_loop_closure(pose_pub, t, pts)
|
||
except Exception:
|
||
pass
|
||
|
||
# Autonomous wander tick — runs only when armed AND the current
|
||
# workflow is in the allowed list. Picks new random goals if no
|
||
# operator goal is in progress. pose_pub must be passed through
|
||
# because _wander_tick / _wander_pick_goal are defined in the
|
||
# outer worker scope and don't see this function's locals.
|
||
_wander_tick(t, pose_pub)
|
||
|
||
if nav_goal_xy is not None and pose_pub is not None and nav_runtime.grid is not None:
|
||
if (t - nav_last_plan_t) >= float(nav_plan_period_sec) or len(nav_path_world) <= 2:
|
||
grid = nav_runtime.grid
|
||
try:
|
||
nav_path_world = nav_global_planner.plan(
|
||
np.asarray(grid["costmap"], dtype=np.uint8),
|
||
np.asarray(grid["origin_xy"], dtype=np.float32),
|
||
float(grid["resolution_m"]),
|
||
(float(pose_pub[0, 3]), float(pose_pub[1, 3])),
|
||
(float(nav_goal_xy[0]), float(nav_goal_xy[1])),
|
||
)
|
||
except Exception as _astar_exc:
|
||
st("WARN", f"[nav] A* planning failed: {_astar_exc}")
|
||
nav_path_world = []
|
||
nav_last_plan_t = t
|
||
elif mission.current_goal() is None:
|
||
nav_path_world = []
|
||
|
||
nav_cmd = {"linear_mps": 0.0, "lateral_mps": 0.0, "angular_rps": 0.0, "goal_reached": False, "blocked": False, "motion": "idle"}
|
||
if pose_pub is not None and len(nav_path_world) > 0:
|
||
# Pass the current workflow profile so the local planner can
|
||
# gate holonomic evasion + stuck-recovery to nav workflows
|
||
# only — MAP_NEW / EXTEND_MAP stay strictly operator-driven.
|
||
nav_cmd = nav_local_planner.compute_command(
|
||
np.asarray(pose_pub, dtype=np.float64),
|
||
nav_path_world,
|
||
nav_runtime,
|
||
workflow=str(stability_profile),
|
||
# Only assert "autonomous override" when wander is allowed
|
||
# AND we have a valid pose — otherwise the planner would
|
||
# think it's free to evade with stale localization.
|
||
autonomous_active=(_wander_allowed_now() and pose_pub is not None),
|
||
)
|
||
if bool(nav_cmd.get("goal_reached", False)):
|
||
mission_snapshot = mission.update_pose(float(pose_pub[0, 3]), float(pose_pub[1, 3]))
|
||
next_goal = mission.current_goal()
|
||
nav_goal_xy = None if next_goal is None else (float(next_goal[0]), float(next_goal[1]))
|
||
nav_path_world = []
|
||
# Wander-set goal completed → clear origin so the next
|
||
# _wander_tick() picks a fresh point. Operator-set goal
|
||
# also clears origin so wander may resume.
|
||
_wander_goal_origin = "operator"
|
||
|
||
# ── POSE-QUALITY AUTO-PAUSE (#11) ─────────────────────────
|
||
# If continuity or map_lock has been rejecting hard for
|
||
# ≥2 s, motion makes the drift worse — zero the velocity
|
||
# and let the worker re-sync before resuming. Auto-resumes
|
||
# the instant the gates stop rejecting.
|
||
_pose_low_now = (
|
||
continuity_consecutive_rejects >= int(continuity_recover_after_rejects)
|
||
or map_lock_consecutive_safe_rejects >= 20
|
||
)
|
||
if _pose_low_now:
|
||
if _pose_quality_low_since is None:
|
||
_pose_quality_low_since = t
|
||
elif (t - _pose_quality_low_since) >= 2.0:
|
||
nav_cmd["linear_mps"] = 0.0
|
||
nav_cmd["lateral_mps"] = 0.0
|
||
nav_cmd["angular_rps"] = 0.0
|
||
nav_cmd["blocked"] = True
|
||
nav_cmd["motion"] = "paused_pose_low"
|
||
if not _pose_quality_auto_paused:
|
||
_pose_quality_auto_paused = True
|
||
st("WARN", "POSE_QUALITY low; auto-pausing motion until tracking recovers")
|
||
else:
|
||
_pose_quality_low_since = None
|
||
if _pose_quality_auto_paused:
|
||
_pose_quality_auto_paused = False
|
||
st("INFO", "POSE_QUALITY recovered; motion resumed")
|
||
|
||
# ── ADAPTIVE SPEED (#4) ───────────────────────────────────
|
||
# Sample the costmap inside a 1.5 m disc around the robot;
|
||
# scale linear+lateral velocity down when local obstacle
|
||
# density is high. Open space → full speed; cluttered
|
||
# space → 0.4× speed minimum. Doesn't affect angular
|
||
# velocity (rotation rate stays unchanged for snappy
|
||
# heading correction).
|
||
_grid_now = getattr(nav_runtime, "grid", None)
|
||
if (isinstance(_grid_now, dict)
|
||
and pose_pub is not None
|
||
and float(nav_cmd.get("linear_mps", 0.0)) != 0.0):
|
||
try:
|
||
_cost_arr = np.asarray(_grid_now["costmap"], dtype=np.uint8)
|
||
_origin = np.asarray(_grid_now["origin_xy"], dtype=np.float64)
|
||
_res = float(_grid_now["resolution_m"])
|
||
_rx = float(pose_pub[0, 3])
|
||
_ry = float(pose_pub[1, 3])
|
||
_gx = int(math.floor((_rx - _origin[0]) / _res))
|
||
_gy = int(math.floor((_ry - _origin[1]) / _res))
|
||
_r_cells = max(2, int(1.5 / max(0.02, _res)))
|
||
_gxmin = max(0, _gx - _r_cells)
|
||
_gxmax = min(_cost_arr.shape[1], _gx + _r_cells + 1)
|
||
_gymin = max(0, _gy - _r_cells)
|
||
_gymax = min(_cost_arr.shape[0], _gy + _r_cells + 1)
|
||
_patch = _cost_arr[_gymin:_gymax, _gxmin:_gxmax]
|
||
if _patch.size > 0:
|
||
_blocked = int(nav_runtime.cfg.blocked_cost)
|
||
_density = float(np.count_nonzero(_patch >= _blocked)) / float(_patch.size)
|
||
# 0% blocked → scale 1.0; 25%+ blocked → scale 0.4.
|
||
_density_clip = min(0.25, _density)
|
||
_speed_scale = max(0.4, 1.0 - _density_clip * 2.4)
|
||
nav_cmd["linear_mps"] = float(nav_cmd.get("linear_mps", 0.0)) * _speed_scale
|
||
nav_cmd["lateral_mps"] = float(nav_cmd.get("lateral_mps", 0.0)) * _speed_scale
|
||
except Exception:
|
||
pass
|
||
|
||
safety_info = safety.evaluate(t, pose_pub, str(loc_state.state), nav_runtime, nav_cmd)
|
||
|
||
# Compute wander-state reason now so it's consistent with the
|
||
# autonomous_active value the planner just saw. The reason is the
|
||
# operator-visible "why isn't wander firing?" explanation.
|
||
_w_allowed_workflow = bool(_wander_allowed_now())
|
||
_w_localized = pose_pub is not None
|
||
if not bool(wander_active):
|
||
_wander_reason = "not_armed"
|
||
elif not _w_allowed_workflow:
|
||
_wander_reason = "workflow_disallowed"
|
||
elif not _w_localized:
|
||
_wander_reason = "not_localized"
|
||
else:
|
||
_wander_reason = ""
|
||
|
||
grid_now = nav_runtime.grid
|
||
nav_info = {
|
||
"mode": mode_now,
|
||
"goal": [float(nav_goal_xy[0]), float(nav_goal_xy[1])] if nav_goal_xy is not None else None,
|
||
"goal_origin": str(_wander_goal_origin),
|
||
"wander_active": bool(wander_active),
|
||
"wander_allowed": _w_allowed_workflow and _w_localized,
|
||
"wander_reason": _wander_reason,
|
||
"path_points": int(len(nav_path_world)),
|
||
"path_preview": (
|
||
[[float(x), float(y)] for (x, y) in nav_path_world[:: max(1, len(nav_path_world) // 60)]]
|
||
if len(nav_path_world) > 0
|
||
else []
|
||
),
|
||
"cmd": dict(safety_info.get("cmd", nav_cmd)),
|
||
"mission": mission_snapshot,
|
||
# Latest ICP outcome (fitness/rmse/inliers/icp_ms_delay).
|
||
# Read by lidar_api → exposed on the status payload as
|
||
# `loc_metrics` for the dashboard's localization card.
|
||
"loc_result": dict(last_loc_icp_result) if last_loc_icp_result else {},
|
||
"costmap": (
|
||
{
|
||
"shape": [int(grid_now["shape_hw"][0]), int(grid_now["shape_hw"][1])],
|
||
"resolution_m": float(grid_now["resolution_m"]),
|
||
"origin_xy": [float(grid_now["origin_xy"][0]), float(grid_now["origin_xy"][1])],
|
||
"static_cells": int(grid_now["static_cells"]),
|
||
"dynamic_cells": int(grid_now["dynamic_cells"]),
|
||
"inflated_cells": int(grid_now["inflated_cells"]),
|
||
}
|
||
if isinstance(grid_now, dict)
|
||
else nav_runtime_info
|
||
),
|
||
}
|
||
|
||
stable_count = 0 if stable_pts_pub is None else int(len(stable_pts_pub))
|
||
_update_perf_rollup(t, stable_count)
|
||
perf_payload = {
|
||
"input_fps": float(perf_input_fps),
|
||
"publish_fps": float(perf_publish_fps),
|
||
"icp_ms": float(perf_icp_ms),
|
||
"queue_lag": int(perf_queue_lag),
|
||
"stable_growth_per_sec": float(perf_stable_growth),
|
||
"cpu_percent": float(perf_cpu_pct),
|
||
}
|
||
|
||
payload = {
|
||
"stable_points": stable_pts_pub,
|
||
"stable_colors": stable_cols,
|
||
"pose": pose_pub,
|
||
"filter": filt.stats() if hasattr(filt, "stats") else {},
|
||
"workflow_profile": str(stability_profile),
|
||
"map_lock": {
|
||
"enabled": bool(map_lock_enabled),
|
||
"accept_count": int(map_lock_accept_count),
|
||
"reject_count": int(map_lock_reject_count),
|
||
"rotating_in_place": bool(rotating_in_place) if mapping_enabled else False,
|
||
"freeze_active": bool(rotate_freeze_active) if mapping_enabled else False,
|
||
"freeze_count": int(rotate_freeze_count),
|
||
"last": dict(map_lock_info) if isinstance(map_lock_info, dict) else {},
|
||
},
|
||
"continuity": {
|
||
"enabled": bool(continuity_enabled),
|
||
"reject_count": int(continuity_reject_count),
|
||
"reject_streak": int(continuity_consecutive_rejects),
|
||
"recover_after_rejects": int(continuity_recover_after_rejects),
|
||
"last": dict(continuity_info) if isinstance(continuity_info, dict) else {},
|
||
},
|
||
"submap": dict(submap_info) if isinstance(submap_info, dict) else {},
|
||
"ref_match_points": ref_match_points_pub,
|
||
"ref_match_colors": ref_match_colors_pub,
|
||
"ref_match": dict(ref_match_stats) if isinstance(ref_match_stats, dict) else {},
|
||
"cleanup": {
|
||
"enabled": bool(cleanup_enabled),
|
||
"removed_total": int(cleanup_removed_total),
|
||
"last": dict(cleanup_info) if isinstance(cleanup_info, dict) else {},
|
||
},
|
||
"localized": bool(slam_to_ref_valid),
|
||
"loc_state": str(loc_state.state),
|
||
"loc_confidence": float(last_localize_confidence),
|
||
"fusion": fusion_info,
|
||
"perf": perf_payload,
|
||
"mode": str(worker_mode),
|
||
"nav": nav_info,
|
||
"safety": safety_info,
|
||
"recording": {
|
||
"enabled": bool(recording_enabled),
|
||
"frames": int(len(recording_frames)),
|
||
"dropped": int(recording_drop_count),
|
||
},
|
||
}
|
||
perf_publish_count += 1
|
||
_safe_put(data_q, ("FRAME", payload), keep_latest=bool(run_cfg.frame_keep_latest))
|
||
|
||
# ---------------- lidar callback (lightweight) ----------------
|
||
# Keep SDK callback minimal: enqueue latest frame and process in worker loop.
|
||
# ---------------- IMU orientation integrator ----------------
|
||
# Complementary filter: gyro integrates at full 200 Hz; accelerometer
|
||
# corrects roll/pitch drift at a low gain (alpha ≈ 0.97).
|
||
# Yaw is gyro-only (no magnetometer) — drifts slowly, but far less than
|
||
# KISS-ICP yaw on fast rotations.
|
||
# Only used when fusion.enabled = true in config.
|
||
|
||
class _ImuIntegrator:
|
||
__slots__ = ("R", "last_t", "alpha", "last_pose_translation")
|
||
|
||
def __init__(self, alpha: float = 0.97) -> None:
|
||
self.R = np.eye(3, dtype=np.float64)
|
||
self.last_t: float = 0.0
|
||
self.alpha = float(np.clip(alpha, 0.80, 0.999))
|
||
self.last_pose_translation = np.zeros(3, dtype=np.float64)
|
||
|
||
def reset(self) -> None:
|
||
self.R = np.eye(3, dtype=np.float64)
|
||
self.last_t = 0.0
|
||
self.last_pose_translation[:] = 0.0
|
||
|
||
def update(self, gyro: np.ndarray, acc: np.ndarray, t: float) -> np.ndarray:
|
||
"""Returns a 4×4 pose. Translation comes from last known LiDAR pose."""
|
||
if self.last_t <= 0.0:
|
||
self.last_t = t
|
||
return np.eye(4, dtype=np.float64)
|
||
dt = float(t - self.last_t)
|
||
if dt <= 0.0 or dt > 0.5:
|
||
self.last_t = t
|
||
return np.eye(4, dtype=np.float64)
|
||
self.last_t = t
|
||
|
||
# 1 — Gyro integration via Rodrigues. Norm is computed once and
|
||
# reused — the IMU runs at 200 Hz so even a free sqrt counted.
|
||
omega = np.asarray(gyro, dtype=np.float64)
|
||
omega_norm = float(np.linalg.norm(omega))
|
||
angle = omega_norm * dt
|
||
if angle > 1e-9:
|
||
axis = omega / (omega_norm + 1e-15)
|
||
K = np.array([[0.0, -axis[2], axis[1]],
|
||
[axis[2], 0.0, -axis[0]],
|
||
[-axis[1], axis[0], 0.0]], dtype=np.float64)
|
||
dR = np.eye(3, dtype=np.float64) + np.sin(angle) * K + (1.0 - np.cos(angle)) * (K @ K)
|
||
self.R = self.R @ dR
|
||
|
||
# 2 — Accelerometer correction for roll/pitch (complementary filter)
|
||
a = np.asarray(acc, dtype=np.float64)
|
||
a_norm = float(np.linalg.norm(a))
|
||
if 7.5 < a_norm < 12.5: # only trust near 1 g; reject dynamic spikes
|
||
a_unit = a / a_norm
|
||
g_in_sensor = self.R.T @ np.array([0.0, 0.0, -1.0]) # expected gravity direction
|
||
cross = np.cross(a_unit, g_in_sensor)
|
||
cross_norm = float(np.linalg.norm(cross))
|
||
if cross_norm > 1e-6:
|
||
corr_angle = float(np.arcsin(np.clip(cross_norm, -1.0, 1.0))) * (1.0 - self.alpha)
|
||
axis_c = cross / cross_norm
|
||
Kc = np.array([[0.0, -axis_c[2], axis_c[1]],
|
||
[axis_c[2], 0.0, -axis_c[0]],
|
||
[-axis_c[1], axis_c[0], 0.0]], dtype=np.float64)
|
||
dR_c = np.eye(3, dtype=np.float64) + np.sin(corr_angle) * Kc + (1.0 - np.cos(corr_angle)) * (Kc @ Kc)
|
||
self.R = dR_c @ self.R
|
||
|
||
# Re-orthogonalise to prevent numerical drift. On SVD failure
|
||
# we DO reset R to identity (the only safe answer for a
|
||
# corrupted rotation matrix), but log loudly — silently
|
||
# zeroing the orientation prior used to surface as random
|
||
# IMU-fusion glitches with no diagnostic trail.
|
||
try:
|
||
U, _, Vt = np.linalg.svd(self.R, full_matrices=False)
|
||
self.R = U @ Vt
|
||
except np.linalg.LinAlgError as svd_err:
|
||
_log.warning(
|
||
"IMU integrator SVD failed (%s); resetting rotation "
|
||
"to identity. Orientation prior will be unreliable "
|
||
"for the next few frames.",
|
||
svd_err,
|
||
)
|
||
self.R = np.eye(3, dtype=np.float64)
|
||
|
||
T = np.eye(4, dtype=np.float64)
|
||
T[:3, :3] = self.R
|
||
T[:3, 3] = self.last_pose_translation # inject LiDAR translation so fusion doesn't shift position
|
||
return T
|
||
|
||
_imu_integrator = _ImuIntegrator(alpha=float(
|
||
((full_cfg.get("fusion", {}) or {}).get("imu_complementary_alpha", 0.97))
|
||
))
|
||
|
||
def on_imu(gyro: np.ndarray, acc: np.ndarray, timestamp: float) -> None:
|
||
if not fusion_cfg.enabled:
|
||
return
|
||
try:
|
||
# Keep the integrator's translation in sync with the latest LiDAR pose
|
||
with state_lock:
|
||
cur_tf = np.array(odom_to_map_transform, dtype=np.float64, copy=True)
|
||
_imu_integrator.last_pose_translation = cur_tf[:3, 3].copy()
|
||
imu_pose = _imu_integrator.update(gyro, acc, timestamp)
|
||
sensor_fusion.update_prior(
|
||
sensor="imu",
|
||
pose=imu_pose,
|
||
confidence=0.85,
|
||
timestamp=timestamp,
|
||
)
|
||
except Exception:
|
||
pass # IMU is advisory; never let it crash the worker
|
||
|
||
# This avoids running heavy ICP/Open3D inside the Livox SDK thread.
|
||
def on_points(points: np.ndarray):
|
||
if points is None or len(points) == 0:
|
||
return
|
||
try:
|
||
pts = np.asarray(points, dtype=np.float32)
|
||
if pts.ndim != 2 or pts.shape[1] != 3:
|
||
return
|
||
except Exception:
|
||
return
|
||
# Single copy: if `np.asarray` already created a fresh contiguous
|
||
# float32 array (SDK delivered a different dtype or non-owning
|
||
# view), reuse it; otherwise force one explicit copy so the queue
|
||
# does not alias the SDK's reusable buffer.
|
||
if pts is points or not pts.flags.owndata:
|
||
pts = np.array(pts, dtype=np.float32, copy=True)
|
||
_safe_put(raw_points_q, pts, keep_latest=True)
|
||
# Tee to ZMQ if raw-frame publishing is enabled (Jetson side).
|
||
# NOBLOCK + SNDHWM=2 means slow subscribers silently drop —
|
||
# we never let a network stall delay the SDK thread.
|
||
if _raw_pub_sock is not None:
|
||
try:
|
||
import zmq as _zmq_pub2
|
||
_raw_pub_sock.send(pts.tobytes(), flags=_zmq_pub2.NOBLOCK)
|
||
except Exception:
|
||
pass
|
||
|
||
# ---------------- commands ----------------
|
||
def _start_remote_input_thread(url: str) -> None:
|
||
"""Subscribe to the brain's raw-frame ZMQ stream and feed
|
||
on_points exactly as the SDK callback would. Runs as a daemon
|
||
thread; stops when _raw_sub_stop is set."""
|
||
nonlocal _raw_sub_thread
|
||
|
||
def _run():
|
||
try:
|
||
import zmq as _zmq_sub
|
||
ctx = _zmq_sub.Context.instance()
|
||
sub = ctx.socket(_zmq_sub.SUB)
|
||
sub.setsockopt(_zmq_sub.RCVHWM, 2)
|
||
sub.setsockopt(_zmq_sub.LINGER, 0)
|
||
sub.setsockopt_string(_zmq_sub.SUBSCRIBE, "")
|
||
sub.connect(url)
|
||
poller = _zmq_sub.Poller()
|
||
poller.register(sub, _zmq_sub.POLLIN)
|
||
while not _raw_sub_stop.is_set():
|
||
events = dict(poller.poll(timeout=300))
|
||
if sub not in events:
|
||
continue
|
||
try:
|
||
raw = sub.recv(flags=_zmq_sub.NOBLOCK)
|
||
except _zmq_sub.Again:
|
||
continue
|
||
if not raw:
|
||
continue
|
||
try:
|
||
arr = np.frombuffer(raw, dtype=np.float32)
|
||
if arr.size % 3 == 0:
|
||
on_points(arr.reshape(-1, 3))
|
||
except Exception:
|
||
pass
|
||
try:
|
||
sub.close(linger=0)
|
||
except Exception:
|
||
pass
|
||
except Exception as _e:
|
||
_log.error("Remote-input SUB thread error: %s", _e)
|
||
|
||
_raw_sub_thread = threading.Thread(
|
||
target=_run, daemon=True, name="slam-remote-input",
|
||
)
|
||
_raw_sub_thread.start()
|
||
|
||
def do_connect():
|
||
nonlocal lidar, connected
|
||
if connected:
|
||
st("INFO", "Already connected.")
|
||
return
|
||
|
||
# ── Remote-input mode (workstation parallel SLAM) ────────────
|
||
# When SLAM_REMOTE_INPUT_URL is set, skip the Livox SDK entirely
|
||
# and source raw frames from the brain's ZMQ relay. Everything
|
||
# downstream (on_points → raw_points_q → ICP → mapping) works
|
||
# identically; only the input edge differs.
|
||
if _remote_input_url:
|
||
st("INFO", f"Connecting via REMOTE INPUT: {_remote_input_url}")
|
||
try:
|
||
_start_remote_input_thread(_remote_input_url)
|
||
connected = True
|
||
st("INFO", f"CONNECTED TO LIVOX (remote input from {_remote_input_url})")
|
||
except Exception as _e:
|
||
connected = False
|
||
st("ERROR", f"Remote input setup failed: {_e}")
|
||
return
|
||
|
||
cfg_path = Path(eng_cfg.config_file)
|
||
if not cfg_path.exists():
|
||
st("ERROR", f"Config missing: {cfg_path}")
|
||
return
|
||
|
||
# Rewrite the per-host IPs inside mid360_config.json before the SDK
|
||
# reads them. G1's shipped file has the workstation IP (.222) hard-
|
||
# coded four times (cmd_data_ip / push_msg_ip / point_data_ip /
|
||
# imu_data_ip); on the Jetson (eth0 = .164) the SDK's bind() fails
|
||
# silently and every send_byte returns -1 forever. eng_cfg.host_ip
|
||
# is the IP resolved by SLAM_engine (config / env var / autodetect),
|
||
# so writing it into the JSON guarantees the SDK binds to a real
|
||
# local interface.
|
||
try:
|
||
_mcfg = json.loads(cfg_path.read_text())
|
||
_hni = _mcfg.get("MID360", {}).get("host_net_info", {})
|
||
for _k in ("cmd_data_ip", "push_msg_ip", "point_data_ip", "imu_data_ip"):
|
||
if _k in _hni:
|
||
_hni[_k] = str(eng_cfg.host_ip)
|
||
cfg_path.write_text(json.dumps(_mcfg, indent=2))
|
||
except Exception as _e:
|
||
st("WARN", f"could not update mid360_config.json host IPs: {_e}")
|
||
|
||
try:
|
||
lidar = Livox2(
|
||
str(cfg_path),
|
||
host_ip=str(eng_cfg.host_ip),
|
||
debug=bool(livox_debug_enabled),
|
||
print_every_n_frames=int(livox_print_every_n),
|
||
tag_filter=bool(getattr(eng_cfg, "tag_filter", True)),
|
||
)
|
||
lidar.handle_points = on_points
|
||
lidar.handle_imu = on_imu
|
||
connected = True
|
||
st("INFO", f"CONNECTED to Livox (host_ip={eng_cfg.host_ip})")
|
||
except Exception as e:
|
||
connected = False
|
||
err = str(e)
|
||
up = err.upper()
|
||
if ("SDKINIT" in up) or ("BIND" in up) or ("SOCKET" in up) or ("ADDRESS ALREADY IN USE" in up):
|
||
err = (
|
||
f"{err}\n"
|
||
"Hint: likely UDP port conflict or wrong host IP.\n"
|
||
"1) Close other Livox/SLAM apps using ports 56101-56501.\n"
|
||
"2) Ensure GUI host IP matches this PC NIC on LiDAR network.\n"
|
||
"3) Verify mid360_config.json host_net_info *_ip and *_port values."
|
||
)
|
||
st("ERROR", f"Connect failed: {err}")
|
||
|
||
def do_start():
|
||
nonlocal mapping_enabled, localize_only_enabled, worker_mode
|
||
nonlocal last_guard_pose, guard_reject_count
|
||
nonlocal continuity_prev_pts, continuity_reject_count, continuity_consecutive_rejects, continuity_last_warn_t
|
||
nonlocal rotate_freeze_count, rotate_last_warn_t
|
||
nonlocal cleanup_last_t, cleanup_last_info
|
||
nonlocal last_frame_t, latest_live_localize_pts
|
||
if not connected:
|
||
st("ERROR", "START denied: not connected.")
|
||
return
|
||
if self_check_report.get("errors"):
|
||
st("ERROR", "START denied: startup self-check has errors. Fix config paths first.")
|
||
return
|
||
mapping_enabled = True
|
||
localize_only_enabled = False
|
||
last_frame_t = 0.0
|
||
loc_live_stack.clear()
|
||
latest_live_localize_pts = None
|
||
last_guard_pose = None
|
||
guard_reject_count = 0
|
||
continuity_prev_pts = None
|
||
continuity_reject_count = 0
|
||
continuity_consecutive_rejects = 0
|
||
continuity_last_warn_t = 0.0
|
||
rotate_freeze_count = 0
|
||
rotate_last_warn_t = 0.0
|
||
cleanup_last_t = 0.0
|
||
cleanup_last_info = {"status": "na"}
|
||
if not _submap_profile_allowed():
|
||
submap_mapper.reset()
|
||
worker_mode = "MAPPING"
|
||
st("INFO", "MAPPING STARTED.")
|
||
push_worker_mode()
|
||
|
||
def do_start_localize_only():
|
||
nonlocal mapping_enabled, localize_only_enabled, worker_mode, last_localize_t
|
||
nonlocal last_key_pose, last_guard_pose, guard_reject_count, last_frame_t
|
||
nonlocal continuity_prev_pts, continuity_consecutive_rejects
|
||
nonlocal continuity_reject_count, rotate_freeze_count
|
||
nonlocal latest_live_localize_pts
|
||
if not connected:
|
||
st("ERROR", "LOCALIZE_ONLY denied: not connected.")
|
||
return
|
||
if not ref_map_path:
|
||
st("ERROR", "LOCALIZE_ONLY denied: load reference map first.")
|
||
return
|
||
# Start localize-only from a fresh odometry frame to avoid stale-map offsets.
|
||
init_slam_stack()
|
||
mapping_enabled = False
|
||
localize_only_enabled = True
|
||
last_frame_t = 0.0
|
||
loc_frames.reset()
|
||
_sync_loc_vars_from_service()
|
||
last_key_pose = None
|
||
last_guard_pose = None
|
||
guard_reject_count = 0
|
||
continuity_prev_pts = None
|
||
continuity_consecutive_rejects = 0
|
||
continuity_reject_count = 0
|
||
rotate_freeze_count = 0
|
||
loc_live_stack.clear()
|
||
latest_live_localize_pts = None
|
||
submap_mapper.reset()
|
||
worker_mode = "LOCALIZE_ONLY"
|
||
last_localize_t = 0.0
|
||
st("INFO", "LOCALIZE_ONLY STARTED.")
|
||
push_worker_mode()
|
||
|
||
def do_pause():
|
||
nonlocal mapping_enabled, localize_only_enabled, worker_mode
|
||
nonlocal latest_live_localize_pts
|
||
mapping_enabled = False
|
||
localize_only_enabled = False
|
||
loc_live_stack.clear()
|
||
latest_live_localize_pts = None
|
||
worker_mode = "PAUSED"
|
||
st("INFO", "MAPPING PAUSED.")
|
||
push_worker_mode()
|
||
|
||
def do_stop():
|
||
nonlocal mapping_enabled, localize_only_enabled, worker_mode
|
||
nonlocal latest_live_localize_pts
|
||
mapping_enabled = False
|
||
localize_only_enabled = False
|
||
loc_live_stack.clear()
|
||
latest_live_localize_pts = None
|
||
worker_mode = "STOPPED"
|
||
st("INFO", "MAPPING STOPPED.")
|
||
push_worker_mode()
|
||
|
||
def do_export(filename_base: str):
|
||
try:
|
||
# Final cleanup pass: flush any stray islands that arrived since the last
|
||
# periodic cleanup (which runs every cleanup_period_sec seconds).
|
||
if cleanup_enabled:
|
||
full_pts = stable.get_points()
|
||
if full_pts is not None and len(full_pts) >= int(cleanup_min_total_points):
|
||
cleaned, _ = cleanup_map_islands(full_pts)
|
||
if cleaned is not None and len(cleaned) >= int(map_cfg.min_points_to_save):
|
||
stable.set_points(cleaned)
|
||
pts = stable.get_save_points()
|
||
n = 0 if pts is None else len(pts)
|
||
req = int(map_cfg.min_points_to_save)
|
||
if pts is None or n < req:
|
||
st("ERROR", f"EXPORT refused: stable points {n}/{req}. Keep mapping and try again.")
|
||
return
|
||
out_path = stable.export_map(filename_base)
|
||
st("INFO", f"SAVED: {out_path}")
|
||
except Exception as e:
|
||
st("ERROR", f"EXPORT failed: {e}")
|
||
|
||
def do_export_nav(filename_base: str):
|
||
try:
|
||
pts = stable.get_save_points()
|
||
n_total = 0 if pts is None else len(pts)
|
||
req = int(nav_cfg.min_points)
|
||
if pts is None or n_total < req:
|
||
st("ERROR", f"EXPORT_NAV refused: stable points {n_total}/{req}.")
|
||
return
|
||
n_band = int(nav_exporter.count_nav_points(pts))
|
||
if n_band < req:
|
||
st(
|
||
"ERROR",
|
||
(
|
||
"EXPORT_NAV refused: nav-band points "
|
||
f"{n_band}/{req} in z[{float(nav_cfg.z_min_m):.2f}, {float(nav_cfg.z_max_m):.2f}] m."
|
||
),
|
||
)
|
||
return
|
||
result = nav_exporter.export(filename_base, pts)
|
||
st("INFO", {"NAV_EXPORTED": result})
|
||
except Exception as e:
|
||
st("ERROR", f"EXPORT_NAV failed: {e}")
|
||
|
||
def do_load_ref(path_value: str):
|
||
nonlocal ref_map_path, ref_pcd_cache, ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n, ref_cache_path
|
||
nonlocal last_localize_t, last_localize_points
|
||
nonlocal approx_pose_guess_tf, approx_pose_guess_until_t
|
||
nonlocal approx_pose_guess_fail_count, approx_pose_guess_success_count
|
||
nonlocal ref_match_points_cache, ref_match_confirm_hits, ref_match_last_pub_t, ref_match_last_stats
|
||
nonlocal last_frame_t, latest_live_localize_pts
|
||
ref_map_path = str(path_value)
|
||
ref_pcd_cache = None
|
||
ref_points_cache = None
|
||
ref_nav_points_cache = None
|
||
ref_nav_cache_src_n = 0
|
||
ref_cache_path = None
|
||
ref_match_points_cache = None
|
||
ref_match_confirm_hits = None
|
||
ref_match_last_pub_t = 0.0
|
||
ref_match_last_stats = {}
|
||
place_recog.reset()
|
||
last_frame_t = 0.0
|
||
loc_live_stack.clear()
|
||
latest_live_localize_pts = None
|
||
submap_mapper.reset()
|
||
last_localize_t = 0.0
|
||
last_localize_points = 0
|
||
loc_frames.reset_reference()
|
||
_sync_loc_vars_from_service()
|
||
approx_pose_guess_tf = None
|
||
approx_pose_guess_until_t = 0.0
|
||
approx_pose_guess_fail_count = 0
|
||
approx_pose_guess_success_count = 0
|
||
loc_state.reset()
|
||
st("INFO", f"REF loaded: {Path(ref_map_path).name}")
|
||
st("INFO", {"PLACE_RECOG": place_recog.snapshot()})
|
||
prior_tf = session_store.get_transform(ref_map_path)
|
||
if prior_tf is not None:
|
||
loc_frames.last_alignment = np.array(prior_tf, dtype=np.float64, copy=True)
|
||
_sync_loc_vars_from_service()
|
||
st("INFO", {"SESSION": {"prior_loaded": True, "ref": Path(ref_map_path).name}})
|
||
# In map-localization workflows we wait for fresh live scans after START,
|
||
# instead of trying to localize immediately from stale/non-live points.
|
||
if mapping_enabled and (not localize_only_enabled):
|
||
do_localize(force=False, source="ON_LOAD")
|
||
push_loc_state()
|
||
|
||
def do_manual_localize():
|
||
src_live = latest_live_localize_pts if localize_only_enabled else None
|
||
if localize_only_enabled and (src_live is None or int(len(src_live)) < 80):
|
||
st("INFO", "LOCALIZE: waiting for live scan...")
|
||
return
|
||
do_localize(force=True, source="MANUAL", src_override=src_live)
|
||
|
||
def do_load_for_extend(path_value: str):
|
||
"""Load a saved .ply AND seed the stable map + filter so the next
|
||
START extends the loaded map instead of building one from scratch.
|
||
"""
|
||
if not str(path_value):
|
||
st("ERROR", "EXTEND denied: no path provided.")
|
||
return
|
||
do_load_ref(str(path_value))
|
||
try:
|
||
import open3d as o3d
|
||
pcd = o3d.io.read_point_cloud(str(path_value))
|
||
pts = np.asarray(pcd.points, dtype=np.float32)
|
||
except Exception as e:
|
||
st("ERROR", f"EXTEND load failed: {e}")
|
||
return
|
||
if pts is None or len(pts) == 0:
|
||
st("ERROR", "EXTEND denied: loaded file has no points.")
|
||
return
|
||
try:
|
||
stable.set_points(pts)
|
||
if hasattr(filt, "seed_stable_points"):
|
||
filt.seed_stable_points(
|
||
pts,
|
||
now=_now(),
|
||
hit_count=int(runtime_filter_hit_threshold),
|
||
clear_existing=True,
|
||
)
|
||
except Exception as e:
|
||
st("WARN", f"EXTEND seed partial failure: {e}")
|
||
st("INFO", {"EXTEND_LOAD": {"path": str(path_value), "points": int(len(pts))}})
|
||
|
||
def do_clear_ref():
|
||
nonlocal ref_map_path, ref_pcd_cache, ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n, ref_cache_path
|
||
nonlocal last_localize_t, last_localize_points
|
||
nonlocal localize_only_enabled, worker_mode
|
||
nonlocal approx_pose_guess_tf, approx_pose_guess_until_t
|
||
nonlocal approx_pose_guess_fail_count, approx_pose_guess_success_count
|
||
nonlocal ref_match_points_cache, ref_match_confirm_hits, ref_match_last_pub_t, ref_match_last_stats
|
||
nonlocal last_frame_t, latest_live_localize_pts
|
||
ref_map_path = None
|
||
ref_pcd_cache = None
|
||
ref_points_cache = None
|
||
ref_nav_points_cache = None
|
||
ref_nav_cache_src_n = 0
|
||
ref_cache_path = None
|
||
ref_match_points_cache = None
|
||
ref_match_confirm_hits = None
|
||
ref_match_last_pub_t = 0.0
|
||
ref_match_last_stats = {}
|
||
place_recog.reset()
|
||
last_frame_t = 0.0
|
||
loc_live_stack.clear()
|
||
latest_live_localize_pts = None
|
||
submap_mapper.reset()
|
||
last_localize_t = 0.0
|
||
last_localize_points = 0
|
||
loc_frames.reset_reference()
|
||
_sync_loc_vars_from_service()
|
||
approx_pose_guess_tf = None
|
||
approx_pose_guess_until_t = 0.0
|
||
approx_pose_guess_fail_count = 0
|
||
approx_pose_guess_success_count = 0
|
||
localize_only_enabled = False
|
||
if not mapping_enabled:
|
||
worker_mode = "IDLE"
|
||
loc_state.reset()
|
||
st("INFO", "REF cleared.")
|
||
push_worker_mode()
|
||
push_loc_state()
|
||
|
||
st("INFO", "Worker ready. CONNECT then START.")
|
||
st("INFO", {"SELF_CHECK": self_check_report, "source": "worker"})
|
||
apply_density_mode("MEDIUM")
|
||
apply_stability_profile("BALANCED")
|
||
apply_min_stable_points(map_cfg.min_points_to_save)
|
||
apply_loop_closure_mode(loop_cfg.enabled)
|
||
apply_loc_state_machine_mode(loc_state_cfg.enabled)
|
||
apply_submap_mode(
|
||
{
|
||
"enabled": submap_mode_enabled,
|
||
"local_window_frames": submap_cfg.local_window_frames,
|
||
"local_voxel_m": submap_cfg.local_voxel_m,
|
||
"global_voxel_m": submap_cfg.global_voxel_m,
|
||
"merge_period_sec": submap_cfg.merge_period_sec,
|
||
"merge_min_translation_m": submap_cfg.merge_min_translation_m,
|
||
"merge_min_rotation_deg": submap_cfg.merge_min_rotation_deg,
|
||
"max_global_points": submap_cfg.max_global_points,
|
||
"display_max_points": submap_cfg.display_max_points,
|
||
"apply_profiles": list(submap_cfg.apply_profiles),
|
||
"keep_points": False,
|
||
}
|
||
)
|
||
apply_autosave(
|
||
{
|
||
"enabled": autosave_enabled,
|
||
"interval_sec": autosave_interval_sec,
|
||
"base_name": autosave_base_name,
|
||
}
|
||
)
|
||
apply_nav_config(
|
||
{
|
||
"min_points": nav_cfg.min_points,
|
||
"resolution_m": nav_cfg.resolution_m,
|
||
"z_min_m": nav_cfg.z_min_m,
|
||
"z_max_m": nav_cfg.z_max_m,
|
||
"inflation_radius_m": nav_cfg.inflation_radius_m,
|
||
"padding_m": nav_cfg.padding_m,
|
||
}
|
||
)
|
||
apply_map_quality_config(
|
||
{
|
||
"enabled": map_quality_cfg.enabled,
|
||
"near_min_range_m": map_quality_cfg.near_min_range_m,
|
||
"ray_consistency_enabled": map_quality_cfg.ray_consistency_enabled,
|
||
"ray_bin_deg": map_quality_cfg.ray_bin_deg,
|
||
"ray_elev_bin_deg": map_quality_cfg.ray_elev_bin_deg,
|
||
"ray_max_behind_m": map_quality_cfg.ray_max_behind_m,
|
||
"ray_keep_n": map_quality_cfg.ray_keep_n,
|
||
"world_z_clip_enabled": map_quality_cfg.world_z_clip_enabled,
|
||
"world_z_min_m": map_quality_cfg.world_z_min_m,
|
||
"world_z_max_m": map_quality_cfg.world_z_max_m,
|
||
"outlier_filter_enabled": map_quality_cfg.outlier_filter_enabled,
|
||
"outlier_voxel_m": map_quality_cfg.outlier_voxel_m,
|
||
"outlier_min_points": map_quality_cfg.outlier_min_points,
|
||
}
|
||
)
|
||
apply_nav_runtime_config(
|
||
{
|
||
"enabled": nav_rt_cfg.enabled,
|
||
"resolution_m": nav_rt_cfg.resolution_m,
|
||
"z_min_m": nav_rt_cfg.z_min_m,
|
||
"z_max_m": nav_rt_cfg.z_max_m,
|
||
"padding_m": nav_rt_cfg.padding_m,
|
||
"inflation_radius_m": nav_rt_cfg.inflation_radius_m,
|
||
"dynamic_decay_sec": nav_rt_cfg.dynamic_decay_sec,
|
||
"dynamic_min_hits": nav_rt_cfg.dynamic_min_hits,
|
||
"blocked_cost": nav_rt_cfg.blocked_cost,
|
||
}
|
||
)
|
||
st("INFO", {"MISSION": mission.snapshot()})
|
||
push_loc_state()
|
||
push_worker_mode()
|
||
|
||
while running:
|
||
try:
|
||
cmd = None
|
||
try:
|
||
cmd = cmd_q.get(timeout=0.02)
|
||
except queue.Empty:
|
||
cmd = None
|
||
|
||
if cmd:
|
||
if isinstance(cmd, tuple):
|
||
name = str(cmd[0]).upper().strip()
|
||
payload = cmd[1] if len(cmd) > 1 else None
|
||
else:
|
||
name = str(cmd).upper().strip()
|
||
payload = None
|
||
|
||
if name == "CONNECT":
|
||
_run_locked(do_connect)
|
||
elif name == "START":
|
||
_run_locked(do_start)
|
||
elif name == "START_LOCALIZE_ONLY":
|
||
_run_locked(do_start_localize_only)
|
||
elif name == "STOP_LOCALIZE_ONLY":
|
||
_run_locked(do_stop)
|
||
elif name == "PAUSE":
|
||
_run_locked(do_pause)
|
||
elif name == "STOP":
|
||
_run_locked(do_stop)
|
||
elif name == "RESET":
|
||
_run_locked(reset_all)
|
||
elif name == "EXPORT":
|
||
_run_locked(do_export, str(payload or "map_robot"))
|
||
elif name == "EXPORT_NAV":
|
||
_run_locked(do_export_nav, str(payload or "map_robot"))
|
||
elif name == "LOAD_REF":
|
||
_run_locked(do_load_ref, str(payload))
|
||
elif name == "LOAD_FOR_EXTEND":
|
||
_run_locked(do_load_for_extend, str(payload))
|
||
elif name == "LOCALIZE":
|
||
_run_locked(do_manual_localize)
|
||
elif name == "CLEAR_REF":
|
||
_run_locked(do_clear_ref)
|
||
elif name == "SET_DENSITY":
|
||
_run_locked(apply_density_mode, str(payload or "MEDIUM"))
|
||
elif name == "SET_MIN_STABLE_POINTS":
|
||
_run_locked(apply_min_stable_points, payload)
|
||
elif name == "SET_LOOP_CLOSURE":
|
||
_run_locked(apply_loop_closure_mode, payload)
|
||
elif name == "SET_LOC_STATE_MACHINE":
|
||
_run_locked(apply_loc_state_machine_mode, payload)
|
||
elif name == "SET_SUBMAP_MODE":
|
||
_run_locked(apply_submap_mode, payload)
|
||
elif name == "SET_APPROX_POSE":
|
||
_run_locked(apply_approx_pose, payload if isinstance(payload, dict) else None)
|
||
elif name == "CLEAR_APPROX_POSE":
|
||
_run_locked(apply_approx_pose, None)
|
||
elif name == "SET_AUTOSAVE":
|
||
_run_locked(apply_autosave, payload if isinstance(payload, dict) else {})
|
||
elif name == "SET_NAV_CONFIG":
|
||
_run_locked(apply_nav_config, payload if isinstance(payload, dict) else {})
|
||
elif name == "SET_NAV_RUNTIME":
|
||
_run_locked(apply_nav_runtime_config, payload if isinstance(payload, dict) else {})
|
||
elif name == "SET_NAV_GOAL":
|
||
_run_locked(apply_nav_goal, payload if isinstance(payload, dict) else {})
|
||
elif name == "CLEAR_NAV_GOAL":
|
||
_run_locked(clear_nav_goal)
|
||
elif name == "SET_MAP_QUALITY":
|
||
_run_locked(apply_map_quality_config, payload if isinstance(payload, dict) else {})
|
||
elif name == "SET_FILTER_TUNING":
|
||
_run_locked(apply_filter_tuning, payload if isinstance(payload, dict) else {})
|
||
elif name == "SET_STABILITY_PROFILE":
|
||
_run_locked(apply_stability_profile, payload)
|
||
elif name == "RECORD_START":
|
||
_run_locked(record_start, payload if isinstance(payload, dict) else {})
|
||
elif name == "RECORD_STOP":
|
||
_run_locked(record_stop, payload if isinstance(payload, dict) else {})
|
||
elif name == "MISSION_START":
|
||
_run_locked(mission_start, payload if isinstance(payload, dict) else {})
|
||
elif name == "MISSION_PAUSE":
|
||
_run_locked(mission_pause)
|
||
elif name == "MISSION_RESUME":
|
||
_run_locked(mission_resume)
|
||
elif name == "MISSION_STOP":
|
||
_run_locked(mission_stop)
|
||
elif name == "WANDER_START":
|
||
_run_locked(_wander_start)
|
||
elif name == "WANDER_STOP":
|
||
_run_locked(_wander_stop)
|
||
elif name == "SET_EXPLORE_BUDGET":
|
||
_mins = 0.0
|
||
if isinstance(payload, dict):
|
||
_mins = float(payload.get("minutes", 0.0) or 0.0)
|
||
elif isinstance(payload, (int, float)):
|
||
_mins = float(payload)
|
||
_run_locked(_set_explore_budget, _mins)
|
||
elif name == "SENSOR_PRIOR":
|
||
if isinstance(payload, dict):
|
||
def _apply_sensor_prior():
|
||
ok = sensor_fusion.update_prior(
|
||
sensor=str(payload.get("sensor", "unknown")),
|
||
pose=payload.get("pose"),
|
||
confidence=float(payload.get("confidence", 1.0)),
|
||
timestamp=float(payload.get("timestamp", _now())),
|
||
)
|
||
if not ok:
|
||
st("WARN", "SENSOR_PRIOR ignored: invalid payload.")
|
||
_run_locked(_apply_sensor_prior)
|
||
else:
|
||
st("WARN", "SENSOR_PRIOR ignored: payload must be dict.")
|
||
elif name == "SHUTDOWN":
|
||
running = False
|
||
else:
|
||
st("WARN", f"Unknown command: {name}")
|
||
|
||
latest_pts = None
|
||
try:
|
||
while True:
|
||
latest_pts = raw_points_q.get_nowait()
|
||
except queue.Empty:
|
||
pass
|
||
|
||
if latest_pts is not None:
|
||
_run_locked(process_points, latest_pts)
|
||
|
||
except KeyboardInterrupt:
|
||
break
|
||
except Exception as e:
|
||
if diagnostics is not None:
|
||
diagnostics.log_exception("Worker loop error", e, _worker_diag_state())
|
||
st("ERROR", f"Loop error: {e}\n{traceback.format_exc()}")
|
||
|
||
try:
|
||
if lidar is not None and hasattr(lidar, "shutdown"):
|
||
lidar.shutdown()
|
||
except Exception:
|
||
pass
|
||
|
||
try:
|
||
if recording_enabled and len(recording_frames) > 0:
|
||
record_stop({"save": True, "base_name": recording_base_name})
|
||
except Exception:
|
||
pass
|
||
|
||
# Stop the remote-input SUB thread (if running) so its socket
|
||
# can release. The thread polls _raw_sub_stop every 300 ms.
|
||
try:
|
||
_raw_sub_stop.set()
|
||
except Exception:
|
||
pass
|
||
# Close the raw-frame PUB socket if we bound one. Without an
|
||
# explicit close, the OS will reap it when the process exits,
|
||
# but on rapid restart cycles port 55600 can stay in TIME_WAIT
|
||
# for tens of seconds, blocking the next worker's bind.
|
||
try:
|
||
if _raw_pub_sock is not None:
|
||
_raw_pub_sock.close(linger=0)
|
||
except Exception:
|
||
pass
|
||
|
||
try:
|
||
if diagnostics is not None:
|
||
diagnostics.close()
|
||
except Exception:
|
||
pass
|
||
|
||
st("INFO", "Worker shutdown.")
|