Update 2026-04-22 12:00:29

Lidar/SLAM_Config.json

@@ -5,8 +5,8 @@
   },
 
   "network": {
-    "default_interface": "enp3s0",
+    "default_interface": "eth0",
-    "default_host_ip": "192.168.123.222"
+    "default_host_ip": "auto"
   },
 
   "livox": {

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"]),

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
+    # Redirect this subprocess's stdout AND stderr to logs/lidar_sdk.log.
-    # C++ SDK prints directly to stderr (not Python logging) and spams
+    # The Livox C++ SDK writes its `[console] [error] ...` stream to both
-    # thousands of lines/second when the LiDAR is offline or flaky. Keeping
+    # streams, and its `bind failed` / `Create socket ...` messages go to
-    # it in a file lets us still debug Livox issues without the errors
+    # stdout. Without this dup2, a disconnected or misconfigured LiDAR
-    # drowning the interactive terminal. stdout stays attached so our own
+    # floods Marcus's terminal with thousands of lines/second. The worker
-    # Python `print`s (pose updates, status lines) still reach the terminal.
+    # 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),