157 lines
5.9 KiB
Python
157 lines
5.9 KiB
Python
from __future__ import annotations
|
|
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
from typing import Any, Dict, List
|
|
|
|
|
|
def _safe_int(v: Any, default: int) -> int:
|
|
try:
|
|
return int(v)
|
|
except Exception:
|
|
return int(default)
|
|
|
|
|
|
def _safe_float(v: Any, default: float) -> float:
|
|
try:
|
|
return float(v)
|
|
except Exception:
|
|
return float(default)
|
|
|
|
|
|
def _resolve_path(base_dir: Path, value: str | None) -> Path:
|
|
raw = (value or "").strip()
|
|
p = Path(raw).expanduser()
|
|
if p.is_absolute():
|
|
return p
|
|
return (base_dir / p).resolve()
|
|
|
|
|
|
def run_startup_self_check(cfg: Dict[str, Any], base_dir: Path | None = None) -> Dict[str, Any]:
|
|
"""
|
|
Validate runtime-critical config and paths.
|
|
Returns a report dict suitable for GUI popup / worker status.
|
|
"""
|
|
if base_dir is None:
|
|
base_dir = Path(__file__).resolve().parent
|
|
base = Path(base_dir).resolve()
|
|
|
|
errors: List[str] = []
|
|
warnings: List[str] = []
|
|
info: List[str] = []
|
|
|
|
app_cfg = cfg.get("app", {}) or {}
|
|
livox_cfg = cfg.get("livox", {}) or {}
|
|
map_cfg = cfg.get("map", {}) or {}
|
|
loc_cfg = cfg.get("localization", {}) or {}
|
|
nav_cfg = cfg.get("navigation_export", {}) or {}
|
|
nav_rt_cfg = cfg.get("navigation_runtime", {}) or {}
|
|
loop_cfg = cfg.get("loop_closure", {}) or {}
|
|
autosave_cfg = cfg.get("autosave", {}) or {}
|
|
|
|
maps_dir = _resolve_path(base, str(app_cfg.get("maps_dir", "DataMap")))
|
|
livox_file = _resolve_path(base, str(livox_cfg.get("config_file", "mid360_config.json")))
|
|
|
|
try:
|
|
maps_dir.mkdir(parents=True, exist_ok=True)
|
|
except Exception as e:
|
|
errors.append(f"maps_dir is not creatable: {maps_dir} ({e})")
|
|
else:
|
|
if not maps_dir.is_dir():
|
|
errors.append(f"maps_dir is not a directory: {maps_dir}")
|
|
else:
|
|
if not maps_dir.exists():
|
|
errors.append(f"maps_dir missing: {maps_dir}")
|
|
if not os.access(maps_dir, os.W_OK):
|
|
warnings.append(f"maps_dir is not writable: {maps_dir}")
|
|
info.append(f"maps_dir: {maps_dir}")
|
|
|
|
if not livox_file.exists():
|
|
errors.append(f"livox.config_file missing: {livox_file}")
|
|
else:
|
|
info.append(f"livox config: {livox_file.name}")
|
|
try:
|
|
livox_data = json.loads(livox_file.read_text())
|
|
lidar_cfgs = livox_data.get("lidar_configs", []) or []
|
|
ext = {}
|
|
if lidar_cfgs and isinstance(lidar_cfgs[0], dict):
|
|
ext = lidar_cfgs[0].get("extrinsic_parameter", {}) or {}
|
|
ext_vals = [
|
|
_safe_float(ext.get("roll", 0.0), 0.0),
|
|
_safe_float(ext.get("pitch", 0.0), 0.0),
|
|
_safe_float(ext.get("yaw", 0.0), 0.0),
|
|
_safe_float(ext.get("x", 0.0), 0.0),
|
|
_safe_float(ext.get("y", 0.0), 0.0),
|
|
_safe_float(ext.get("z", 0.0), 0.0),
|
|
]
|
|
if all(abs(v) <= 1e-6 for v in ext_vals):
|
|
warnings.append(
|
|
"MID360 extrinsics are all zero in mid360_config.json; "
|
|
"verify G1 upside-down mounting correction before trusting localization."
|
|
)
|
|
else:
|
|
info.append(
|
|
"MID360 extrinsics: "
|
|
f"rpy=({ext_vals[0]:.2f}, {ext_vals[1]:.2f}, {ext_vals[2]:.2f}) "
|
|
f"xyz=({ext_vals[3]:.3f}, {ext_vals[4]:.3f}, {ext_vals[5]:.3f})"
|
|
)
|
|
except Exception as e:
|
|
warnings.append(f"Could not parse MID360 extrinsics from {livox_file.name}: {e}")
|
|
|
|
min_map = _safe_int(map_cfg.get("min_points_to_save", 0), 0)
|
|
min_loc = _safe_int(loc_cfg.get("min_points_for_localize", 0), 0)
|
|
min_nav = _safe_int(nav_cfg.get("min_points", 0), 0)
|
|
if min_map <= 0 or min_loc <= 0 or min_nav <= 0:
|
|
errors.append("min points must be > 0 for map/localization/navigation.")
|
|
if not (min_map == min_loc == min_nav):
|
|
warnings.append(
|
|
f"min points mismatch (map/localize/nav): {min_map}/{min_loc}/{min_nav}. "
|
|
"This can confuse export/localization behavior."
|
|
)
|
|
else:
|
|
info.append(f"min points unified: {min_map}")
|
|
|
|
z_min = _safe_float(nav_cfg.get("z_min_m", -0.4), -0.4)
|
|
z_max = _safe_float(nav_cfg.get("z_max_m", 1.2), 1.2)
|
|
if z_min >= z_max:
|
|
errors.append(f"navigation_export z range invalid: z_min({z_min}) >= z_max({z_max})")
|
|
|
|
rt_z_min = _safe_float(nav_rt_cfg.get("z_min_m", z_min), z_min)
|
|
rt_z_max = _safe_float(nav_rt_cfg.get("z_max_m", z_max), z_max)
|
|
if rt_z_min >= rt_z_max:
|
|
errors.append(f"navigation_runtime z range invalid: z_min({rt_z_min}) >= z_max({rt_z_max})")
|
|
|
|
loc_use_p2pl = bool(loc_cfg.get("use_point_to_plane", False))
|
|
loc_unsafe = bool(loc_cfg.get("allow_point_to_plane_unsafe", False))
|
|
if loc_use_p2pl and not loc_unsafe:
|
|
warnings.append(
|
|
"localization.use_point_to_plane=true but allow_point_to_plane_unsafe=false. "
|
|
"Worker will use point-to-point for stability."
|
|
)
|
|
|
|
if bool(loop_cfg.get("enabled", False)):
|
|
max_corr_t = _safe_float(loop_cfg.get("max_correction_translation_m", 2.0), 2.0)
|
|
max_corr_r = _safe_float(loop_cfg.get("max_correction_rotation_deg", 20.0), 20.0)
|
|
if max_corr_t <= 0.0 or max_corr_r <= 0.0:
|
|
warnings.append("loop_closure enabled but safe correction limits are non-positive.")
|
|
else:
|
|
info.append(f"loop safe limits: {max_corr_t:.2f}m / {max_corr_r:.1f}deg")
|
|
|
|
if bool(autosave_cfg.get("enabled", False)):
|
|
iv = _safe_float(autosave_cfg.get("interval_sec", 90.0), 90.0)
|
|
if iv < 5.0:
|
|
warnings.append(f"autosave interval is very low ({iv:.1f}s).")
|
|
|
|
status = "ok"
|
|
if errors:
|
|
status = "error"
|
|
elif warnings:
|
|
status = "warn"
|
|
return {
|
|
"status": status,
|
|
"errors": errors,
|
|
"warnings": warnings,
|
|
"info": info,
|
|
}
|