3778 lines
175 KiB
Python
3778 lines
175 KiB
Python
# SLAM_worker.py
|
||
from __future__ import annotations
|
||
|
||
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
|
||
|
||
from SLAM_engine import (
|
||
EngineConfig,
|
||
FilterConfig,
|
||
MapConfig,
|
||
LocalizationConfig,
|
||
RuntimeConfig,
|
||
load_slam_config,
|
||
)
|
||
from SLAM_Validation import run_startup_self_check
|
||
|
||
|
||
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,
|
||
):
|
||
# 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
|
||
|
||
def st(level: str, msg: Any):
|
||
_safe_put(status_q, (level, msg))
|
||
|
||
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 = _as_bool(
|
||
(loc_cfg_raw or {}).get("use_point_to_plane", False),
|
||
False,
|
||
) and bool(loc_pt2plane_unsafe)
|
||
if _as_bool((loc_cfg_raw or {}).get("use_point_to_plane", False), False) 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", {})))
|
||
|
||
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
|
||
last_guard_t = 0.0
|
||
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
|
||
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
|
||
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
|
||
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_pts: Optional[np.ndarray] = None
|
||
latest_live_localize_pts: Optional[np.ndarray] = None
|
||
raw_points_q: queue.Queue = queue.Queue(maxsize=max(2, int(run_cfg.frame_queue_maxsize)))
|
||
|
||
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
|
||
|
||
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, last_guard_t, guard_reject_count, guard_last_warn_t
|
||
nonlocal last_frame_t
|
||
nonlocal odom_to_map_transform, map_lock_last_t, map_lock_accept_count, map_lock_reject_count, map_lock_last_warn_t
|
||
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, last_localize_transform, last_localize_confidence
|
||
nonlocal ref_pcd_cache, ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n, ref_cache_path, slam_to_ref_transform, slam_to_ref_valid
|
||
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 loc_live_stack
|
||
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_pts, 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
|
||
last_guard_t = 0.0
|
||
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_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_pts = None
|
||
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}
|
||
|
||
try:
|
||
from scipy.spatial import cKDTree
|
||
except Exception:
|
||
return {"transform": np.eye(4, dtype=np.float64), "fitness": 0.0, "rmse": 9e9, "inliers": 0}
|
||
|
||
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,
|
||
}
|
||
try:
|
||
from scipy.spatial import cKDTree
|
||
except Exception:
|
||
return stats
|
||
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):
|
||
from scipy.spatial import cKDTree
|
||
tree = 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).
|
||
try:
|
||
from scipy.spatial import cKDTree
|
||
except Exception:
|
||
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 = 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
|
||
|
||
try:
|
||
from scipy.spatial import cKDTree
|
||
tree = 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
|
||
|
||
try:
|
||
from scipy.spatial import cKDTree
|
||
tree = 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))
|
||
|
||
# ---------------- localization ----------------
|
||
def do_localize(
|
||
force: bool = False,
|
||
source: str = "MANUAL",
|
||
src_override: Optional[np.ndarray] = None,
|
||
) -> Optional[Dict[str, Any]]:
|
||
nonlocal ref_map_path, last_localize_t, last_localize_points, last_localize_transform, last_localize_confidence
|
||
nonlocal ref_pcd_cache, ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n, ref_cache_path
|
||
nonlocal slam_to_ref_transform, slam_to_ref_valid
|
||
nonlocal approx_pose_guess_tf, approx_pose_guess_until_t, approx_pose_guess_fail_count, approx_pose_guess_success_count
|
||
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,
|
||
)
|
||
|
||
reg = _run_local_icp_chain(init)
|
||
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))
|
||
|
||
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"},
|
||
"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"},
|
||
"QUICK_DEMO": {"hit_threshold": 2, "decay_seconds": 8.0, "voxel_size": 0.20, "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_mapper, 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
|
||
nonlocal last_localize_transform
|
||
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),
|
||
}
|
||
},
|
||
)
|
||
|
||
def apply_nav_goal(payload: Any) -> None:
|
||
nonlocal nav_goal_xy, nav_path_world, nav_last_plan_t
|
||
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
|
||
st("INFO", {"NAV_GOAL": {"x": float(x), "y": float(y)}})
|
||
|
||
def clear_nav_goal() -> None:
|
||
nonlocal nav_goal_xy, nav_path_world
|
||
nav_goal_xy = None
|
||
nav_path_world = []
|
||
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, recording_base_name
|
||
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, mapping_enabled, localize_only_enabled, worker_mode, last_key_pose, last_frame_t
|
||
nonlocal last_guard_pose, last_guard_t, guard_reject_count, guard_last_warn_t
|
||
nonlocal odom_to_map_transform, map_lock_last_t, map_lock_accept_count, map_lock_reject_count, map_lock_last_warn_t
|
||
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 slam_to_ref_transform, slam_to_ref_valid, last_localize_transform
|
||
nonlocal perf_input_count, perf_publish_count, perf_icp_ms
|
||
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
|
||
nonlocal latest_live_pts, latest_live_localize_pts
|
||
nonlocal ref_points_cache, ref_nav_points_cache, ref_nav_cache_src_n
|
||
|
||
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_pts = np.array(pts, dtype=np.float32, copy=True)
|
||
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)
|
||
last_guard_t = now_t
|
||
else:
|
||
last_guard_pose = np.array(pose_np, dtype=np.float64, copy=True)
|
||
last_guard_t = now_t
|
||
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
|
||
elif bool(map_lock_info.get("ran", False)):
|
||
map_lock_reject_count += 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"
|
||
),
|
||
)
|
||
|
||
# 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
|
||
|
||
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]))
|
||
elif mission_snapshot.get("completed", False):
|
||
nav_goal_xy = None
|
||
nav_path_world = []
|
||
|
||
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, "angular_rps": 0.0, "goal_reached": False, "blocked": False}
|
||
if pose_pub is not None and len(nav_path_world) > 0:
|
||
nav_cmd = nav_local_planner.compute_command(np.asarray(pose_pub, dtype=np.float64), nav_path_world, nav_runtime)
|
||
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 = []
|
||
|
||
safety_info = safety.evaluate(t, pose_pub, str(loc_state.state), nav_runtime, nav_cmd)
|
||
|
||
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,
|
||
"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,
|
||
"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
|
||
omega = np.asarray(gyro, dtype=np.float64)
|
||
angle = float(np.linalg.norm(omega)) * dt
|
||
if angle > 1e-9:
|
||
axis = omega / (float(np.linalg.norm(omega)) + 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
|
||
try:
|
||
U, _, Vt = np.linalg.svd(self.R, full_matrices=False)
|
||
self.R = U @ Vt
|
||
except np.linalg.LinAlgError:
|
||
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 as _imu_exc:
|
||
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
|
||
_safe_put(raw_points_q, np.array(pts, dtype=np.float32, copy=True), keep_latest=True)
|
||
|
||
# ---------------- commands ----------------
|
||
def do_connect():
|
||
nonlocal lidar, connected
|
||
if connected:
|
||
st("INFO", "Already connected.")
|
||
return
|
||
|
||
cfg_path = Path(eng_cfg.config_file)
|
||
if not cfg_path.exists():
|
||
st("ERROR", f"Config missing: {cfg_path}")
|
||
return
|
||
|
||
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),
|
||
)
|
||
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, last_guard_t, 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, loc_live_stack, 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
|
||
last_guard_t = 0.0
|
||
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, last_localize_confidence
|
||
nonlocal last_key_pose, last_guard_pose, last_guard_t, guard_reject_count, last_frame_t
|
||
nonlocal odom_to_map_transform, continuity_prev_pts, continuity_consecutive_rejects
|
||
nonlocal continuity_reject_count, rotate_freeze_count, loc_live_stack
|
||
nonlocal slam_to_ref_valid, 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
|
||
last_guard_t = 0.0
|
||
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 loc_live_stack, 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 loc_live_stack, 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, last_localize_transform, last_localize_confidence
|
||
nonlocal slam_to_ref_transform, slam_to_ref_valid
|
||
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 loc_live_stack, 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_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, last_localize_transform, last_localize_confidence
|
||
nonlocal slam_to_ref_transform, slam_to_ref_valid
|
||
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 loc_live_stack, 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 == "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 == "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
|
||
|
||
try:
|
||
if diagnostics is not None:
|
||
diagnostics.close()
|
||
except Exception:
|
||
pass
|
||
|
||
st("INFO", "Worker shutdown.")
|