diff --git a/Lidar/SLAM_Config.json b/Lidar/SLAM_Config.json index 6f0ce03..0b301b6 100644 --- a/Lidar/SLAM_Config.json +++ b/Lidar/SLAM_Config.json @@ -5,8 +5,8 @@ }, "network": { - "default_interface": "enp3s0", - "default_host_ip": "192.168.123.222" + "default_interface": "eth0", + "default_host_ip": "auto" }, "livox": { diff --git a/Lidar/SLAM_engine.py b/Lidar/SLAM_engine.py index 4c2e665..a191dce 100644 --- a/Lidar/SLAM_engine.py +++ b/Lidar/SLAM_engine.py @@ -3,6 +3,7 @@ from __future__ import annotations import json import os +import subprocess import time import multiprocessing as mp from dataclasses import dataclass @@ -11,6 +12,29 @@ from typing import Any, Optional from SLAM_Validation import run_startup_self_check + +def _autodetect_g1_host_ip(default: str = "192.168.123.164") -> str: + """ + Return the local IPv4 on the G1's 192.168.123.0/24 network. + + The Livox SDK binds a UDP socket to this address; using the wrong one + (e.g. the workstation IP on the Jetson, or vice versa) produces a + `bind failed` error storm. Auto-detecting lets the same SLAM_Config.json + work on both the Jetson (eth0 = 192.168.123.164) and the workstation + (enp3s0 = 192.168.123.222) without manual edits. + """ + try: + out = subprocess.run( + ["ip", "-4", "-o", "addr"], capture_output=True, text=True, timeout=2, + ).stdout + for line in out.splitlines(): + for tok in line.split(): + if tok.startswith("192.168.123.") and "/" in tok: + return tok.split("/")[0] + except Exception: + pass + return default + # ------------------------- config loader (jsonl) ------------------------- def load_slam_config() -> dict: """ @@ -130,9 +154,25 @@ def _safe_put(q: mp.Queue, item: Any, keep_latest: bool = False) -> None: def build_configs_from_json(cfg: dict) -> tuple[EngineConfig, FilterConfig, MapConfig, LocalizationConfig, RuntimeConfig]: maps_dir = _resolve_from_config_dir(str(cfg["app"]["maps_dir"])) + # Resolve the Livox host IP. Accept the literal "auto" (or an env var) so + # the same config works on the Jetson and the workstation. Also trigger + # auto-detect if the config still holds the old workstation default but + # we're clearly not running on the workstation. + host_ip_cfg = str(cfg["network"]["default_host_ip"]).strip() + env_ip = os.environ.get("LIVOX_HOST_IP", "").strip() + if env_ip: + resolved_host_ip = env_ip + elif host_ip_cfg.lower() == "auto" or not host_ip_cfg: + resolved_host_ip = _autodetect_g1_host_ip() + else: + # If config says 192.168.123.222 but this machine doesn't own that IP, + # fall back to autodetect instead of crashing the Livox SDK. + detected = _autodetect_g1_host_ip(default=host_ip_cfg) + resolved_host_ip = detected if detected != host_ip_cfg else host_ip_cfg + eng = EngineConfig( config_file=_resolve_from_config_dir(str(cfg["livox"]["config_file"])), - host_ip=cfg["network"]["default_host_ip"], + host_ip=resolved_host_ip, max_range=float(cfg["slam"]["max_range"]), slam_voxel_size=float(cfg["slam"]["slam_voxel_size"]), pre_downsample_stride=int(cfg["slam"]["pre_downsample_stride"]), diff --git a/Lidar/SLAM_worker.py b/Lidar/SLAM_worker.py index c79e920..5777ca3 100644 --- a/Lidar/SLAM_worker.py +++ b/Lidar/SLAM_worker.py @@ -65,12 +65,14 @@ def slam_worker( run_cfg: RuntimeConfig, ): # ───────────────────────────────────────────────────────────────────── - # Redirect this subprocess's stderr to logs/lidar_sdk.log. The Livox - # C++ SDK prints directly to stderr (not Python logging) and spams - # thousands of lines/second when the LiDAR is offline or flaky. Keeping - # it in a file lets us still debug Livox issues without the errors - # drowning the interactive terminal. stdout stays attached so our own - # Python `print`s (pose updates, status lines) still reach the terminal. + # Redirect this subprocess's stdout AND stderr to logs/lidar_sdk.log. + # The Livox C++ SDK writes its `[console] [error] ...` stream to both + # streams, and its `bind failed` / `Create socket ...` messages go to + # stdout. Without this dup2, a disconnected or misconfigured LiDAR + # floods Marcus's terminal with thousands of lines/second. The worker + # communicates status upward through the multiprocessing queues, so + # losing stdout/stderr here is fine — nothing the operator needs to + # see is lost. # ───────────────────────────────────────────────────────────────────── import os as _os, sys as _sys try: @@ -81,7 +83,9 @@ def slam_worker( _os.makedirs(_log_dir, exist_ok=True) _err_path = _os.path.join(_log_dir, "lidar_sdk.log") _fd = _os.open(_err_path, _os.O_WRONLY | _os.O_CREAT | _os.O_APPEND, 0o644) - _os.dup2(_fd, 2) # replace stderr FD so even C++ libs are captured + _os.dup2(_fd, 1) # replace stdout FD so C++ printf/puts are captured + _os.dup2(_fd, 2) # replace stderr FD for C++ std::cerr / spdlog + _sys.stdout = _os.fdopen(1, "w", buffering=1) _sys.stderr = _os.fdopen(2, "w", buffering=1) except Exception: pass # never crash just because the log redirect failed @@ -3343,6 +3347,22 @@ def slam_worker( st("ERROR", f"Config missing: {cfg_path}") return + # Rewrite the per-host IPs inside mid360_config.json before the SDK + # reads them. The shipped file has the workstation IP hardcoded + # four times (cmd_data_ip / push_msg_ip / point_data_ip / imu_data_ip), + # which makes Livox's SDK bind() fail on any machine that isn't the + # workstation. host_ip here is the value resolved by SLAM_engine + # (either config, env, or auto-detected off 192.168.123.0/24). + try: + _mcfg = json.loads(cfg_path.read_text()) + _hni = _mcfg.get("MID360", {}).get("host_net_info", {}) + for _k in ("cmd_data_ip", "push_msg_ip", "point_data_ip", "imu_data_ip"): + if _k in _hni: + _hni[_k] = str(eng_cfg.host_ip) + cfg_path.write_text(json.dumps(_mcfg, indent=2)) + except Exception as _e: + st("WARN", f"could not update mid360_config.json host IPs: {_e}") + try: lidar = Livox2( str(cfg_path),