Initial commit: scan_web integrated recording UI
FastAPI backend + vanilla JS frontend for LiDAR/camera startup, camera settings, and rosbag recording, unifying the previous scan_gui/scan_gui_dual/ scan_gui_triple desktop tools into one web app.
This commit is contained in:
gardentech
@@ -0,0 +1,67 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from contextlib import asynccontextmanager
|
||||
from pathlib import Path
|
||||
|
||||
from fastapi import FastAPI
|
||||
from fastapi.staticfiles import StaticFiles
|
||||
from starlette.types import Scope
|
||||
|
||||
import config
|
||||
from process_manager import ProcessManager
|
||||
from ros_bridge import ROSBridge
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s")
|
||||
logger = logging.getLogger("scan_web.app")
|
||||
|
||||
FRONTEND_DIR = Path(__file__).resolve().parent.parent / "frontend"
|
||||
|
||||
|
||||
class SessionState:
|
||||
def __init__(self):
|
||||
self.camera_count = config.DEFAULT_CAMERA_COUNT
|
||||
self.recording = False
|
||||
self.recording_path: str | None = None
|
||||
self.recording_started_at: float | None = None
|
||||
self.camera_values: dict = {} # cam_id -> camera_params.CameraParamValues
|
||||
|
||||
|
||||
@asynccontextmanager
|
||||
async def lifespan(app: FastAPI):
|
||||
app.state.pm = ProcessManager()
|
||||
app.state.session = SessionState()
|
||||
app.state.ros = ROSBridge(camera_count=app.state.session.camera_count)
|
||||
app.state.ros.start()
|
||||
logger.info("scan_web backend started (camera_count=%s)", app.state.session.camera_count)
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
logger.info("shutting down — stopping all managed processes")
|
||||
app.state.pm.stop_all()
|
||||
app.state.ros.stop()
|
||||
|
||||
|
||||
app = FastAPI(title="scan_web", lifespan=lifespan)
|
||||
|
||||
from routers import system, camera, recording, telemetry # noqa: E402 (needs app.state types defined above)
|
||||
|
||||
app.include_router(system.router, prefix="/api")
|
||||
app.include_router(camera.router)
|
||||
app.include_router(recording.router, prefix="/api")
|
||||
app.include_router(telemetry.router)
|
||||
|
||||
class NoCacheStaticFiles(StaticFiles):
|
||||
"""Frontend is actively edited during field testing — a browser caching a
|
||||
stale index.html/app.js/style.css (no Cache-Control was set before) led
|
||||
to confusing "it's not applying" reports that were actually just the
|
||||
prior version still running. Static assets here are small and local, so
|
||||
the no-caching cost is negligible."""
|
||||
|
||||
async def get_response(self, path: str, scope: Scope):
|
||||
response = await super().get_response(path, scope)
|
||||
response.headers["Cache-Control"] = "no-store"
|
||||
return response
|
||||
|
||||
|
||||
app.mount("/", NoCacheStaticFiles(directory=str(FRONTEND_DIR), html=True), name="frontend")
|
||||
@@ -0,0 +1,55 @@
|
||||
"""Read back what ros2 bag already knows about a recording — duration,
|
||||
message counts, per-topic counts — instead of just listing directory names.
|
||||
Also a plain on-disk size sum (metadata.yaml doesn't carry a byte size in
|
||||
the ros2 bag version installed here)."""
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
def dir_size_bytes(path: Path) -> int:
|
||||
total = 0
|
||||
try:
|
||||
for entry in path.iterdir():
|
||||
if entry.is_file():
|
||||
total += entry.stat().st_size
|
||||
except OSError:
|
||||
pass
|
||||
return total
|
||||
|
||||
|
||||
def read_bag_metadata(bag_dir: Path) -> Optional[dict]:
|
||||
"""Returns None if metadata.yaml is missing (e.g. bag was killed mid-write
|
||||
before rosbag2 flushed it) or unparseable — a real, if incomplete, state
|
||||
the operator should be able to see rather than a crash."""
|
||||
meta_path = bag_dir / "metadata.yaml"
|
||||
if not meta_path.exists():
|
||||
return None
|
||||
try:
|
||||
with open(meta_path, encoding="utf-8") as f:
|
||||
raw = yaml.safe_load(f)
|
||||
info = raw["rosbag2_bagfile_information"]
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
duration_s = info.get("duration", {}).get("nanoseconds", 0) / 1e9
|
||||
message_count = info.get("message_count", 0)
|
||||
topics = []
|
||||
for entry in info.get("topics_with_message_count", []):
|
||||
tm = entry.get("topic_metadata", {})
|
||||
topics.append({
|
||||
"name": tm.get("name", "?"),
|
||||
"type": tm.get("type", "?"),
|
||||
"message_count": entry.get("message_count", 0),
|
||||
})
|
||||
|
||||
return {
|
||||
"duration_s": round(duration_s, 1),
|
||||
"message_count": message_count,
|
||||
"topics": topics,
|
||||
"size_bytes": dir_size_bytes(bag_dir),
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
"""Camera exposure/gain parameter handling.
|
||||
|
||||
Ported from scan_gui_triple.py's per-camera tab logic:
|
||||
- load defaults by reading the base params YAML (ros__parameters block)
|
||||
- "apply": live `ros2 param set <node> <name> <value>` per param, checked
|
||||
(subprocess.run with captured output), reporting structured per-param
|
||||
success/failure — the triple GUI's more robust approach, preferred here
|
||||
over the single/dual GUIs' fire-and-forget version.
|
||||
- "save": regex text-patch of the base YAML, NOT yaml.dump — the source
|
||||
YAML files carry human-authored comments (e.g. timestamp-calibration
|
||||
notes) that a full re-dump would destroy.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import subprocess
|
||||
from dataclasses import dataclass, asdict
|
||||
from typing import Optional
|
||||
|
||||
import yaml
|
||||
|
||||
from config import ROS_SETUP, CAMERA2_WS_SETUP, CameraSpec
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraParamValues:
|
||||
exposure_auto: bool
|
||||
exposure_auto_target_brightness: int
|
||||
exposure_auto_min: float
|
||||
exposure_auto_max: float
|
||||
exposure_time: int
|
||||
gain: float
|
||||
|
||||
def as_dict(self) -> dict:
|
||||
return asdict(self)
|
||||
|
||||
|
||||
def load_defaults(cam: CameraSpec) -> CameraParamValues:
|
||||
p = {}
|
||||
try:
|
||||
with open(cam.base_params_path, encoding="utf-8") as f:
|
||||
cfg = yaml.safe_load(f)
|
||||
p = cfg[f"/{cam.node_name}"]["ros__parameters"]
|
||||
except Exception:
|
||||
pass
|
||||
return CameraParamValues(
|
||||
exposure_auto=bool(p.get("exposure_auto", False)),
|
||||
exposure_auto_target_brightness=int(p.get("exposure_auto_target_brightness", 128)),
|
||||
exposure_auto_min=float(p.get("exposure_auto_min", 100.0)),
|
||||
exposure_auto_max=float(p.get("exposure_auto_max", 10000.0)),
|
||||
exposure_time=int(p.get("exposure_time", 5000)),
|
||||
gain=float(p.get("gain", 8.0)),
|
||||
)
|
||||
|
||||
|
||||
def write_launch_params_yaml(cam: CameraSpec, values: CameraParamValues, tmp_prefix: str = "/tmp") -> str:
|
||||
"""Write a temp YAML (base + overridden values) for passing as a launch arg."""
|
||||
import tempfile
|
||||
|
||||
try:
|
||||
with open(cam.base_params_path, encoding="utf-8") as f:
|
||||
cfg = yaml.safe_load(f)
|
||||
except Exception:
|
||||
cfg = {}
|
||||
node_key = f"/{cam.node_name}"
|
||||
cfg.setdefault(node_key, {}).setdefault("ros__parameters", {})
|
||||
cfg[node_key]["ros__parameters"].update(values.as_dict())
|
||||
|
||||
tf = tempfile.NamedTemporaryFile(
|
||||
mode="w", suffix=".yaml", prefix=f"{tmp_prefix}/{cam.id}_params_", delete=False
|
||||
)
|
||||
yaml.dump(cfg, tf, default_flow_style=False, allow_unicode=True)
|
||||
tf.close()
|
||||
return tf.name
|
||||
|
||||
|
||||
def _format_param_value(value) -> str:
|
||||
if isinstance(value, bool):
|
||||
return "true" if value else "false"
|
||||
if isinstance(value, float):
|
||||
return f"{value:.4f}"
|
||||
return str(value)
|
||||
|
||||
|
||||
def apply_params(cam: CameraSpec, values: CameraParamValues) -> list[dict]:
|
||||
"""Run `ros2 param set` per param, checked. Returns [{name, ok, message}, ...]."""
|
||||
node = f"/{cam.node_name}"
|
||||
src = f"source {ROS_SETUP} && source {CAMERA2_WS_SETUP}"
|
||||
|
||||
order = ["exposure_auto"]
|
||||
if values.exposure_auto:
|
||||
order += ["exposure_auto_target_brightness", "exposure_auto_min", "exposure_auto_max"]
|
||||
else:
|
||||
order += ["exposure_time"]
|
||||
order += ["gain"]
|
||||
|
||||
vals = values.as_dict()
|
||||
results = []
|
||||
for name in order:
|
||||
val_str = _format_param_value(vals[name])
|
||||
cmd = f"{src} && ros2 param set {node} {name} {val_str}"
|
||||
try:
|
||||
proc = subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=10)
|
||||
except subprocess.TimeoutExpired:
|
||||
results.append({"name": name, "ok": False, "message": "timeout"})
|
||||
continue
|
||||
if proc.returncode != 0:
|
||||
reason = (proc.stdout.strip() or proc.stderr.strip() or "unknown error")
|
||||
results.append({"name": name, "ok": False, "message": reason})
|
||||
else:
|
||||
results.append({"name": name, "ok": True, "message": "ok"})
|
||||
return results
|
||||
|
||||
|
||||
def _patch_yaml_value(text: str, key: str, new_value: str) -> tuple[str, bool]:
|
||||
pattern = re.compile(rf"^([ \t]*){re.escape(key)}:([ \t]*)\S+", re.MULTILINE)
|
||||
new_text, n = pattern.subn(
|
||||
lambda m: f"{m.group(1)}{key}:{m.group(2)}{new_value}", text, count=1,
|
||||
)
|
||||
return new_text, n > 0
|
||||
|
||||
|
||||
def _insert_missing_yaml_keys(text: str, missing: dict) -> str:
|
||||
indent = " "
|
||||
anchor_pos: Optional[int] = None
|
||||
for anchor in ("exposure_time", "gain"):
|
||||
m = re.search(rf"^([ \t]*){re.escape(anchor)}:", text, re.MULTILINE)
|
||||
if m:
|
||||
indent = m.group(1)
|
||||
anchor_pos = m.start()
|
||||
break
|
||||
block = "".join(f"{indent}{k}: {v}\n" for k, v in missing.items())
|
||||
if anchor_pos is None:
|
||||
return text.rstrip("\n") + "\n" + block
|
||||
return text[:anchor_pos] + block + text[anchor_pos:]
|
||||
|
||||
|
||||
def save_params(cam: CameraSpec, values: CameraParamValues) -> None:
|
||||
"""Persist values into the base YAML via comment-preserving text patch."""
|
||||
formatted = {
|
||||
"exposure_auto": "true" if values.exposure_auto else "false",
|
||||
"exposure_auto_target_brightness": str(values.exposure_auto_target_brightness),
|
||||
"exposure_auto_min": f"{values.exposure_auto_min:.1f}",
|
||||
"exposure_auto_max": f"{values.exposure_auto_max:.1f}",
|
||||
"exposure_time": str(values.exposure_time),
|
||||
"gain": f"{values.gain:.1f}",
|
||||
}
|
||||
with open(cam.base_params_path, encoding="utf-8") as f:
|
||||
text = f.read()
|
||||
missing = {}
|
||||
for key, val in formatted.items():
|
||||
text, found = _patch_yaml_value(text, key, val)
|
||||
if not found:
|
||||
missing[key] = val
|
||||
if missing:
|
||||
text = _insert_missing_yaml_keys(text, missing)
|
||||
with open(cam.base_params_path, "w", encoding="utf-8") as f:
|
||||
f.write(text)
|
||||
@@ -0,0 +1,235 @@
|
||||
"""Workspace paths, per-camera-count topic/launch config, and tunable thresholds.
|
||||
|
||||
Values here are ported 1:1 from the source of truth in ~/fast_ws/scan_gui.py,
|
||||
scan_gui_dual.py and scan_gui_triple.py — see docstrings in each CameraSpec
|
||||
list for which file a given camera-count profile was copied from.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
HOME = Path.home()
|
||||
|
||||
ROS_SETUP = "/opt/ros/humble/setup.bash"
|
||||
# fast_ws is deprecated (2026-08-05, user decision) — it never had its own
|
||||
# livox_ros_driver2 build, it only chained to lidar2_ws/install as an
|
||||
# underlay (confirmed: COLCON_CURRENT_PREFIX in fast_ws/install/setup.bash
|
||||
# points at lidar2_ws/install). Source lidar2_ws directly instead.
|
||||
LIDAR2_WS_SETUP = str(HOME / "lidar2_ws/install/setup.bash")
|
||||
CAMERA2_WS_SETUP = str(HOME / "camera2_ws/install/setup.bash")
|
||||
FAST_DUAL_WS_SETUP = str(HOME / "fast_dual_ws/install/setup.bash")
|
||||
RTK_WS_SETUP = str(HOME / "rtk_ws/install/setup.bash")
|
||||
# hku-mars/FAST_LIO ROS2 port, LiDAR+IMU-only mapping (no camera) — confirmed
|
||||
# working 2026-08-06. Its install/setup.bash was built with only
|
||||
# /opt/ros/humble as an underlay (not chained to lidar2_ws), but it subscribes
|
||||
# livox_ros_driver2::msg::CustomMsg on /livox/lidar, so lidar2_ws must still
|
||||
# be sourced first at launch time for that message package to resolve.
|
||||
FAST_LIO_WS_SETUP = str(HOME / "fast_lio/install/setup.bash")
|
||||
|
||||
LIDAR_LAUNCH_CMD = (
|
||||
f"source {ROS_SETUP} && source {LIDAR2_WS_SETUP} && "
|
||||
"stdbuf -oL -eL ros2 launch livox_ros_driver2 mid360s_fastlivo_launch.py"
|
||||
)
|
||||
|
||||
CAMERA_LAUNCH_SOURCE = f"source {ROS_SETUP} && source {CAMERA2_WS_SETUP}"
|
||||
|
||||
# rviz:=false — this is launched headlessly as a recording-lifecycle
|
||||
# subprocess, not an interactive session; mapping.launch.py defaults rviz to
|
||||
# true which would otherwise pop an unwanted GUI window.
|
||||
FASTLIO_LAUNCH_CMD = (
|
||||
f"source {ROS_SETUP} && source {LIDAR2_WS_SETUP} && source {FAST_LIO_WS_SETUP} && "
|
||||
"stdbuf -oL -eL ros2 launch fast_lio mapping.launch.py rviz:=false"
|
||||
)
|
||||
|
||||
|
||||
def fastlio_env() -> dict:
|
||||
"""LD_LIBRARY_PATH override for the fastlio subprocess.
|
||||
|
||||
run_scan_web.sh sources camera2_ws (for the Hikvision camera driver)
|
||||
before exec'ing uvicorn, which puts the Hikvision SDK's lib dirs on
|
||||
LD_LIBRARY_PATH for the whole backend process — and therefore, by
|
||||
default, every ManagedProcess it spawns too (env = os.environ.copy()).
|
||||
fastlio_mapping doesn't use the camera SDK at all, but its bundled
|
||||
libusb-1.0.so.0 is old enough to be missing libusb_set_option, a symbol
|
||||
PCL's libpcl_io needs — with that copy resolved first, fastlio_mapping
|
||||
fails to even start (symbol lookup error, confirmed 2026-08-06).
|
||||
Two separate paths carry this old libusb, not just one — /opt/MVS/lib/*
|
||||
(the vendor SDK's own install) AND
|
||||
camera2_ws/install/hik_camera_ros2_driver/lib (which symlinks straight
|
||||
into the same vendored hikSDK/lib/amd64 copy) — both get filtered here.
|
||||
"""
|
||||
bad = ("mvs", "hik_camera", "hiksdk")
|
||||
parts = os.environ.get("LD_LIBRARY_PATH", "").split(":")
|
||||
cleaned = ":".join(p for p in parts if p and not any(b in p.lower() for b in bad))
|
||||
return {"LD_LIBRARY_PATH": cleaned}
|
||||
|
||||
CAMERA_DELAY_SEC = 5 # let LiDAR init before starting cameras — ported from scan_gui*.py
|
||||
|
||||
LOG_DIR = HOME / "scan_web_logs"
|
||||
LIDAR_LOG_PATH = LOG_DIR / "lidar.log"
|
||||
CAMERA_LOG_PATH = LOG_DIR / "camera.log"
|
||||
RECORDING_LOG_PATH = LOG_DIR / "recording.log"
|
||||
FASTLIO_LOG_PATH = LOG_DIR / "fastlio.log"
|
||||
GNSS_LOG_PATH = LOG_DIR / "gnss.log"
|
||||
|
||||
DEFAULT_SAVE_DIR = str(HOME / "bags") # confirmed default from scan_gui.py:460
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraSpec:
|
||||
id: str
|
||||
image_topic: str
|
||||
info_topic: str
|
||||
node_name: str # ROS node name, no leading slash
|
||||
base_params_path: str
|
||||
launch_arg: str # launch-file argument name this camera's params file is passed as
|
||||
|
||||
|
||||
HIK_CFG_DIR = HOME / "camera2_ws/src/hik_camera_ros2_driver/config"
|
||||
|
||||
# camera_count -> (launch_file, [CameraSpec, ...])
|
||||
CAMERA_PROFILES: dict[int, tuple[str, list[CameraSpec]]] = {
|
||||
1: (
|
||||
"hik_camera_launch.py",
|
||||
[
|
||||
CameraSpec(
|
||||
id="cam1",
|
||||
image_topic="/camera/image",
|
||||
info_topic="/camera/camera_info",
|
||||
node_name="hik_camera_ros2_driver",
|
||||
base_params_path=str(HIK_CFG_DIR / "camera_params.yaml"),
|
||||
launch_arg="params_file",
|
||||
),
|
||||
],
|
||||
),
|
||||
2: (
|
||||
"hik_camera_dual_launch.py",
|
||||
[
|
||||
CameraSpec(
|
||||
id="cam1",
|
||||
image_topic="/cam1/image",
|
||||
info_topic="/cam1/camera_info",
|
||||
node_name="hik_camera_cam1",
|
||||
base_params_path=str(HIK_CFG_DIR / "camera_params_cam1.yaml"),
|
||||
launch_arg="cam1_params_file",
|
||||
),
|
||||
CameraSpec(
|
||||
id="cam2",
|
||||
image_topic="/cam2/image",
|
||||
info_topic="/cam2/camera_info",
|
||||
node_name="hik_camera_cam2",
|
||||
base_params_path=str(HIK_CFG_DIR / "camera_params_cam2.yaml"),
|
||||
launch_arg="cam2_params_file",
|
||||
),
|
||||
],
|
||||
),
|
||||
3: (
|
||||
"hik_camera_triple_launch.py",
|
||||
[
|
||||
CameraSpec(
|
||||
id="cam1",
|
||||
image_topic="/cam1/image",
|
||||
info_topic="/cam1/camera_info",
|
||||
node_name="hik_camera_cam1",
|
||||
base_params_path=str(HIK_CFG_DIR / "camera_params_cam1.yaml"),
|
||||
launch_arg="cam1_params_file",
|
||||
),
|
||||
CameraSpec(
|
||||
id="cam2",
|
||||
image_topic="/cam2/image",
|
||||
info_topic="/cam2/camera_info",
|
||||
node_name="hik_camera_cam2",
|
||||
base_params_path=str(HIK_CFG_DIR / "camera_params_cam2.yaml"),
|
||||
launch_arg="cam2_params_file",
|
||||
),
|
||||
CameraSpec(
|
||||
id="cam3",
|
||||
image_topic="/cam3/image",
|
||||
info_topic="/cam3/camera_info",
|
||||
node_name="hik_camera_cam3",
|
||||
base_params_path=str(HIK_CFG_DIR / "camera_params_cam3.yaml"),
|
||||
launch_arg="cam3_params_file",
|
||||
),
|
||||
],
|
||||
),
|
||||
}
|
||||
|
||||
DEFAULT_CAMERA_COUNT = 3 # matches the physical rig described by the user (3 cams + MID360s)
|
||||
|
||||
|
||||
def camera_specs(camera_count: int) -> list[CameraSpec]:
|
||||
if camera_count not in CAMERA_PROFILES:
|
||||
raise ValueError(f"unsupported camera_count={camera_count}, expected one of {sorted(CAMERA_PROFILES)}")
|
||||
return CAMERA_PROFILES[camera_count][1]
|
||||
|
||||
|
||||
def camera_launch_file(camera_count: int) -> str:
|
||||
return CAMERA_PROFILES[camera_count][0]
|
||||
|
||||
|
||||
def base_record_topics(camera_count: int) -> str:
|
||||
parts = ["/livox/lidar", "/livox/imu"]
|
||||
for cam in camera_specs(camera_count):
|
||||
parts += [cam.image_topic, cam.info_topic]
|
||||
return " ".join(parts)
|
||||
|
||||
|
||||
ROSBAG_SOURCE = f"source {ROS_SETUP} && source {LIDAR2_WS_SETUP} && source {CAMERA2_WS_SETUP}"
|
||||
|
||||
# GPS is out of scope for M1 (see plan milestone M2) — topic names kept here only
|
||||
# as forward-reference documentation, not wired to any process/health logic yet.
|
||||
GPS_TOPICS_LEGACY = "/gps/fix /gps/nmea"
|
||||
GPS_TOPICS_RTK = "/ublox_driver/receiver_pvt"
|
||||
|
||||
DISK_LOW_WARNING_GB = 10.0 # badge turns warning below this
|
||||
DISK_LOW_DANGER_GB = 2.0 # badge turns danger below this — a long scan can lose a bag here
|
||||
|
||||
|
||||
def disk_free_gb(path: str) -> float:
|
||||
"""Free space (GiB) on the filesystem backing `path` (walks up to an
|
||||
existing ancestor dir first — the save dir itself may not exist yet)."""
|
||||
import shutil as _shutil
|
||||
p = Path(path)
|
||||
while not p.exists() and p != p.parent:
|
||||
p = p.parent
|
||||
usage = _shutil.disk_usage(p)
|
||||
return usage.free / (1024 ** 3)
|
||||
|
||||
|
||||
# ── Health staleness thresholds (seconds) — proposed defaults, tune against
|
||||
# real observed rates once M1-M3 are running (see plan §Health synthesis). ──
|
||||
HEALTH_THRESHOLDS = {
|
||||
"/livox/lidar": (1.0, 3.0),
|
||||
# No "/livox/imu" entry — not subscribed at all anymore, see
|
||||
# ros_bridge.py's module docstring. imu_health in /api/status mirrors
|
||||
# lidar's own classification instead.
|
||||
"camera_image": (1.5, 4.0), # applied per-camera image topic
|
||||
# /Odometry, not /aft_mapped_to_init — that was FAST-LIVO2/LOAM-family
|
||||
# naming, anticipated before the actual M3 backend (hku-mars FAST_LIO
|
||||
# ROS2 port, confirmed 2026-08-06) was built. Confirmed via
|
||||
# laserMapping.cpp: pubOdomAftMapped_ publishes nav_msgs/Odometry on
|
||||
# "/Odometry".
|
||||
"/Odometry": (1.0, 4.0),
|
||||
}
|
||||
|
||||
# ── M3 live point-cloud view ────────────────────────────────────────────
|
||||
# FAST-LIO's /cloud_registered is the per-scan (not cumulative) downsampled
|
||||
# world-frame cloud — scan_web accumulates it server-side (see
|
||||
# pointcloud_codec.py) so the operator sees full scan coverage, not a
|
||||
# blinking per-scan cloud.
|
||||
POINTCLOUD_TOPIC = "/cloud_registered"
|
||||
ODOMETRY_TOPIC = "/Odometry"
|
||||
|
||||
POINTCLOUD_VOXEL_SIZE_M = 0.05 # accumulation dedup grid
|
||||
POINTCLOUD_MAX_ACCUM_POINTS = 200_000 # hard ceiling; oldest points evicted past this
|
||||
|
||||
# Two output profiles selected at WS-connect time (?profile=desktop|phone) —
|
||||
# phone gets fewer points at a lower rate, reasoned from three.js draw cost
|
||||
# and WS bandwidth on field WiFi. Both sample from the same accumulated
|
||||
# buffer, just decimated differently per frame.
|
||||
POINTCLOUD_PROFILES = {
|
||||
"desktop": {"max_points": 20_000, "hz": 5.0},
|
||||
"phone": {"max_points": 7_000, "hz": 3.5},
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
"""Synthesized topic health (no diagnostics topics exist upstream — see plan).
|
||||
|
||||
Classification per topic:
|
||||
DOWN - backing process not running, OR no message ever received since
|
||||
process start, OR staleness beyond down_threshold
|
||||
STALE - running, >=1 message received, staleness in (stale, down]
|
||||
OK - staleness <= stale_threshold
|
||||
|
||||
Also tracks a rolling message rate (Hz) per topic — a measured number is a
|
||||
more actionable status signal than a traffic light alone (e.g. "cam2 4.1Hz"
|
||||
tells the operator it's alive but degraded, not just "not OK").
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
|
||||
RATE_WINDOW = 150 # samples kept per topic — larger window trades responsiveness for a
|
||||
# stable number: our own callback dispatch has scheduling jitter (see
|
||||
# ros_bridge.py's MultiThreadedExecutor/GIL-contention notes), so a short
|
||||
# window made the displayed Hz visibly jump around between updates even
|
||||
# when the underlying topic rate was steady. 150 samples spans ~25s for a
|
||||
# ~6Hz camera down to ~2s for ~80Hz IMU — smooths out that jitter without
|
||||
# the number going stale-looking on genuinely slow topics.
|
||||
RATE_STALE_S = 2.0 # if last message older than this, report rate as 0 regardless of window
|
||||
|
||||
|
||||
@dataclass
|
||||
class TopicThreshold:
|
||||
stale_s: float
|
||||
down_s: float
|
||||
|
||||
|
||||
class TopicHealthTracker:
|
||||
def __init__(self):
|
||||
self._last_msg_time: dict[str, float] = {}
|
||||
self._recent_times: dict[str, deque] = {}
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def mark_received(self, topic: str) -> None:
|
||||
now = time.monotonic()
|
||||
with self._lock:
|
||||
self._last_msg_time[topic] = now
|
||||
dq = self._recent_times.setdefault(topic, deque(maxlen=RATE_WINDOW))
|
||||
dq.append(now)
|
||||
|
||||
def reset(self, topic: str) -> None:
|
||||
with self._lock:
|
||||
self._last_msg_time.pop(topic, None)
|
||||
self._recent_times.pop(topic, None)
|
||||
|
||||
def classify(self, topic: str, process_running: bool, thresholds: TopicThreshold) -> str:
|
||||
if not process_running:
|
||||
return "DOWN"
|
||||
with self._lock:
|
||||
last = self._last_msg_time.get(topic)
|
||||
if last is None:
|
||||
return "DOWN"
|
||||
staleness = time.monotonic() - last
|
||||
if staleness <= thresholds.stale_s:
|
||||
return "OK"
|
||||
if staleness <= thresholds.down_s:
|
||||
return "STALE"
|
||||
return "DOWN"
|
||||
|
||||
def staleness(self, topic: str) -> float | None:
|
||||
with self._lock:
|
||||
last = self._last_msg_time.get(topic)
|
||||
return None if last is None else time.monotonic() - last
|
||||
|
||||
def rate_hz(self, topic: str) -> float:
|
||||
"""Measured message rate over the last few samples. 0.0 if stale or unseen."""
|
||||
with self._lock:
|
||||
last = self._last_msg_time.get(topic)
|
||||
dq = self._recent_times.get(topic)
|
||||
if last is None or dq is None or len(dq) < 2:
|
||||
return 0.0
|
||||
if time.monotonic() - last > RATE_STALE_S:
|
||||
return 0.0
|
||||
span = dq[-1] - dq[0]
|
||||
if span <= 0:
|
||||
return 0.0
|
||||
return round((len(dq) - 1) / span, 1)
|
||||
@@ -0,0 +1,54 @@
|
||||
"""Tails the lidar/camera/recording log files for the browser log panel.
|
||||
|
||||
The original PyQt5 scan GUIs had a single unified QPlainTextEdit showing all
|
||||
subprocess stdout with a [tag] prefix — this is the web equivalent. Each
|
||||
ManagedProcess re-opens its log file in "w" mode on every start (see
|
||||
process_manager.py), so a tailer must detect truncation (file shrank since
|
||||
last read) and reset to the top rather than seeking past EOF forever.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
|
||||
class LogTailer:
|
||||
def __init__(self, sources: dict[str, Path]):
|
||||
self._sources = sources
|
||||
self._offsets: dict[str, int] = {tag: 0 for tag in sources}
|
||||
|
||||
def read_tail(self, tag: str, max_lines: int = 200) -> list[str]:
|
||||
"""Backlog for a fresh websocket connection — last N lines, and
|
||||
advances this tag's offset to end-of-file so read_new() only
|
||||
reports genuinely new lines afterwards."""
|
||||
path = self._sources.get(tag)
|
||||
if path is None or not path.exists():
|
||||
return []
|
||||
try:
|
||||
with open(path, "r", errors="replace") as f:
|
||||
lines = f.readlines()
|
||||
self._offsets[tag] = f.tell()
|
||||
except OSError:
|
||||
return []
|
||||
return [ln.rstrip("\n") for ln in lines[-max_lines:]]
|
||||
|
||||
def read_new(self) -> Iterable[tuple[str, str]]:
|
||||
"""Yields (tag, line) for every line appended since the last call."""
|
||||
for tag, path in self._sources.items():
|
||||
if not path.exists():
|
||||
continue
|
||||
try:
|
||||
size = path.stat().st_size
|
||||
if size < self._offsets.get(tag, 0):
|
||||
self._offsets[tag] = 0 # file was truncated (process restarted)
|
||||
with open(path, "r", errors="replace") as f:
|
||||
f.seek(self._offsets[tag])
|
||||
new_text = f.read()
|
||||
self._offsets[tag] = f.tell()
|
||||
except OSError:
|
||||
continue
|
||||
if not new_text:
|
||||
continue
|
||||
for line in new_text.splitlines():
|
||||
if line:
|
||||
yield tag, line
|
||||
@@ -0,0 +1,145 @@
|
||||
"""Accumulates FAST-LIO's per-scan /cloud_registered into a running
|
||||
world-frame point cloud (voxel-deduped, capped at a hard point ceiling) and
|
||||
encodes decimated binary frames for the /ws/pointcloud stream.
|
||||
|
||||
Binary frame layout (all little-endian, no padding):
|
||||
4s magic b"PCF1"
|
||||
I frame_seq
|
||||
d timestamp (unix seconds)
|
||||
I pose_flag 1 if a pose follows, else 0 (uint32, not uint8 — keeps
|
||||
everything after it 4-byte aligned so the client can
|
||||
construct a Float32Array view directly on the xyz
|
||||
bytes below instead of copying)
|
||||
7f pose x, y, z, qx, qy, qz, qw (only present if pose_flag)
|
||||
I point_count N
|
||||
Nx3 f4 xyz, world frame, contiguous [x0,y0,z0, x1,y1,z1, ...]
|
||||
Nx1 u1 intensity, normalized to 0-255
|
||||
|
||||
xyz and intensity are separate contiguous arrays (not an interleaved struct)
|
||||
so both ends can hand them straight to a typed array / numpy view.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
MAGIC = b"PCF1"
|
||||
|
||||
|
||||
class PointCloudAccumulator:
|
||||
def __init__(self, voxel_size: float, max_points: int):
|
||||
self.voxel_size = voxel_size
|
||||
self.max_points = max_points
|
||||
self._lock = threading.Lock()
|
||||
self._xyz = np.empty((0, 3), dtype=np.float32)
|
||||
self._intensity = np.empty((0,), dtype=np.float32)
|
||||
self._keys: list[int] = [] # packed voxel key per row, same order as _xyz
|
||||
self._key_set: set[int] = set() # membership test for incoming-scan dedup
|
||||
self._pose: Optional[tuple] = None # (x,y,z,qx,qy,qz,qw)
|
||||
self._frame_seq = 0
|
||||
|
||||
# ── ingest (called from the ROS-side worker thread) ─────────────────
|
||||
|
||||
def _pack_keys(self, xyz: np.ndarray) -> np.ndarray:
|
||||
# Packs a voxel-quantized (ix,iy,iz) into one int64 so it's hashable
|
||||
# for the Python set below. OFFSET/BASE give each axis +-2^19 voxels
|
||||
# of range (with the default 5cm voxel that's +-26km), comfortably
|
||||
# covering FAST-LIO's 100m det_range with headroom to spare, while
|
||||
# staying well inside int64.
|
||||
OFFSET = 1 << 19
|
||||
BASE = 1 << 20
|
||||
idx = np.floor(xyz / self.voxel_size).astype(np.int64) + OFFSET
|
||||
return (idx[:, 0] * BASE + idx[:, 1]) * BASE + idx[:, 2]
|
||||
|
||||
def add_scan(self, xyz: np.ndarray, intensity: np.ndarray) -> None:
|
||||
"""xyz: (K,3) float32 world-frame points from one /cloud_registered message."""
|
||||
if xyz.shape[0] == 0:
|
||||
return
|
||||
keys = self._pack_keys(xyz)
|
||||
# Dedup within this scan first (a downsampled scan can still put
|
||||
# multiple points in one voxel), then keep only voxels not already
|
||||
# in the accumulated buffer.
|
||||
_, first_idx = np.unique(keys, return_index=True)
|
||||
keys = keys[first_idx]
|
||||
xyz = xyz[first_idx]
|
||||
intensity = intensity[first_idx]
|
||||
|
||||
with self._lock:
|
||||
novel = np.fromiter((k not in self._key_set for k in keys), dtype=bool, count=len(keys))
|
||||
if not novel.any():
|
||||
return
|
||||
new_xyz = xyz[novel]
|
||||
new_intensity = intensity[novel]
|
||||
new_keys = keys[novel]
|
||||
|
||||
self._xyz = np.concatenate([self._xyz, new_xyz])
|
||||
self._intensity = np.concatenate([self._intensity, new_intensity])
|
||||
self._keys.extend(int(k) for k in new_keys)
|
||||
self._key_set.update(int(k) for k in new_keys)
|
||||
|
||||
overflow = len(self._keys) - self.max_points
|
||||
if overflow > 0:
|
||||
# FIFO eviction — oldest points dropped first once over budget.
|
||||
evicted, self._keys = self._keys[:overflow], self._keys[overflow:]
|
||||
self._key_set.difference_update(evicted)
|
||||
self._xyz = self._xyz[overflow:]
|
||||
self._intensity = self._intensity[overflow:]
|
||||
|
||||
def set_pose(self, x: float, y: float, z: float, qx: float, qy: float, qz: float, qw: float) -> None:
|
||||
with self._lock:
|
||||
self._pose = (x, y, z, qx, qy, qz, qw)
|
||||
|
||||
def point_count(self) -> int:
|
||||
with self._lock:
|
||||
return self._xyz.shape[0]
|
||||
|
||||
def reset(self) -> None:
|
||||
"""Clears the accumulated map — call when a new recording starts so
|
||||
the previous scan's points don't linger into the next one."""
|
||||
with self._lock:
|
||||
self._xyz = np.empty((0, 3), dtype=np.float32)
|
||||
self._intensity = np.empty((0,), dtype=np.float32)
|
||||
self._keys = []
|
||||
self._key_set = set()
|
||||
self._pose = None
|
||||
self._frame_seq = 0
|
||||
|
||||
# ── output (called from the asyncio WS loop) ─────────────────────────
|
||||
|
||||
def encode_frame(self, max_points: int) -> bytes:
|
||||
with self._lock:
|
||||
n_total = self._xyz.shape[0]
|
||||
if n_total <= max_points:
|
||||
xyz, intensity = self._xyz, self._intensity
|
||||
else:
|
||||
# Stride sample, not random — keeps the decimated view stable
|
||||
# frame-to-frame instead of sparkling with a fresh random
|
||||
# subset every send.
|
||||
idx = np.linspace(0, n_total - 1, max_points).astype(np.int64)
|
||||
xyz, intensity = self._xyz[idx], self._intensity[idx]
|
||||
pose = self._pose
|
||||
self._frame_seq += 1
|
||||
seq = self._frame_seq
|
||||
return _pack_frame(seq, time.time(), pose, xyz, intensity)
|
||||
|
||||
|
||||
def _pack_frame(seq: int, timestamp: float, pose: Optional[tuple], xyz: np.ndarray, intensity: np.ndarray) -> bytes:
|
||||
n = xyz.shape[0]
|
||||
parts = [MAGIC, struct.pack("<Id", seq, timestamp)]
|
||||
if pose is not None:
|
||||
parts.append(struct.pack("<I", 1))
|
||||
parts.append(struct.pack("<7f", *pose))
|
||||
else:
|
||||
parts.append(struct.pack("<I", 0))
|
||||
parts.append(struct.pack("<I", n))
|
||||
parts.append(np.ascontiguousarray(xyz, dtype="<f4").tobytes())
|
||||
# Livox reflectivity/intensity isn't guaranteed to sit in 0-255, but in
|
||||
# practice runs close to it — plain clip+cast is a fine first cut, tune
|
||||
# once real field values are observed.
|
||||
intensity_u8 = np.clip(intensity, 0, 255).astype(np.uint8)
|
||||
parts.append(np.ascontiguousarray(intensity_u8).tobytes())
|
||||
return b"".join(parts)
|
||||
@@ -0,0 +1,141 @@
|
||||
"""Subprocess orchestration for lidar/camera/recording processes.
|
||||
|
||||
ManagedProcess.stop() is a near-literal port of the `_kill_proc` pattern used
|
||||
by scan_gui.py / scan_gui_dual.py / scan_gui_triple.py: SIGINT the whole
|
||||
process group (so `ros2 launch` subtrees and `ros2 bag record` get a chance
|
||||
to shut down / flush cleanly), wait, then SIGKILL stragglers. This must not
|
||||
regress — it's the one piece of logic explicitly called out as load-bearing
|
||||
in the design plan.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import psutil
|
||||
|
||||
logger = logging.getLogger("scan_web.process_manager")
|
||||
|
||||
|
||||
class ManagedProcess:
|
||||
def __init__(self, name: str, cmd: str, log_path: Optional[Path] = None, env: Optional[dict] = None):
|
||||
self.name = name
|
||||
self.cmd = cmd
|
||||
self.log_path = log_path
|
||||
self.env = env
|
||||
self._proc: Optional[subprocess.Popen] = None
|
||||
self._log_file = None
|
||||
self._lock = threading.Lock()
|
||||
self.started_at: Optional[float] = None
|
||||
|
||||
def start(self) -> None:
|
||||
with self._lock:
|
||||
if self.is_running():
|
||||
return
|
||||
env = os.environ.copy()
|
||||
env["PYTHONUNBUFFERED"] = "1"
|
||||
if self.env:
|
||||
env.update(self.env)
|
||||
|
||||
stdout = subprocess.DEVNULL
|
||||
stderr = subprocess.DEVNULL
|
||||
if self.log_path is not None:
|
||||
self.log_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
self._log_file = open(self.log_path, "w")
|
||||
stdout = self._log_file
|
||||
stderr = subprocess.STDOUT
|
||||
|
||||
self._proc = subprocess.Popen(
|
||||
["bash", "-c", self.cmd],
|
||||
stdout=stdout, stderr=stderr,
|
||||
preexec_fn=os.setsid, env=env,
|
||||
)
|
||||
self.started_at = time.monotonic()
|
||||
logger.info("started %s pid=%s", self.name, self._proc.pid)
|
||||
|
||||
def is_running(self) -> bool:
|
||||
return self._proc is not None and self._proc.poll() is None
|
||||
|
||||
def pid(self) -> Optional[int]:
|
||||
return self._proc.pid if self._proc else None
|
||||
|
||||
def stop(self, timeout: float = 3.0) -> None:
|
||||
with self._lock:
|
||||
proc = self._proc
|
||||
if proc is None or proc.poll() is not None:
|
||||
self._cleanup()
|
||||
return
|
||||
try:
|
||||
parent = psutil.Process(proc.pid)
|
||||
children = parent.children(recursive=True)
|
||||
except psutil.NoSuchProcess:
|
||||
self._cleanup()
|
||||
return
|
||||
for p in children + [parent]:
|
||||
try:
|
||||
p.send_signal(signal.SIGINT)
|
||||
except (psutil.NoSuchProcess, psutil.AccessDenied):
|
||||
pass
|
||||
_, alive = psutil.wait_procs(children + [parent], timeout=timeout)
|
||||
for p in alive:
|
||||
try:
|
||||
p.kill()
|
||||
except (psutil.NoSuchProcess, psutil.AccessDenied):
|
||||
pass
|
||||
logger.info("stopped %s", self.name)
|
||||
self._cleanup()
|
||||
|
||||
def _cleanup(self) -> None:
|
||||
self._proc = None
|
||||
self.started_at = None
|
||||
if self._log_file:
|
||||
self._log_file.close()
|
||||
self._log_file = None
|
||||
|
||||
|
||||
class ProcessManager:
|
||||
"""App-lifetime singleton (stored on app.state) tracking all managed processes."""
|
||||
|
||||
def __init__(self):
|
||||
self._procs: dict[str, ManagedProcess] = {}
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def start(self, name: str, cmd: str, log_path: Optional[Path] = None, env: Optional[dict] = None) -> ManagedProcess:
|
||||
with self._lock:
|
||||
mp = self._procs.get(name)
|
||||
if mp is None or not mp.is_running():
|
||||
mp = ManagedProcess(name, cmd, log_path=log_path, env=env)
|
||||
self._procs[name] = mp
|
||||
mp.start()
|
||||
return mp
|
||||
|
||||
def stop(self, name: str, timeout: float = 3.0) -> None:
|
||||
with self._lock:
|
||||
mp = self._procs.get(name)
|
||||
if mp is not None:
|
||||
mp.stop(timeout=timeout)
|
||||
|
||||
def is_running(self, name: str) -> bool:
|
||||
with self._lock:
|
||||
mp = self._procs.get(name)
|
||||
return mp is not None and mp.is_running()
|
||||
|
||||
def status(self) -> dict[str, dict]:
|
||||
with self._lock:
|
||||
items = list(self._procs.items())
|
||||
return {
|
||||
name: {"running": mp.is_running(), "pid": mp.pid()}
|
||||
for name, mp in items
|
||||
}
|
||||
|
||||
def stop_all(self, timeout: float = 3.0) -> None:
|
||||
with self._lock:
|
||||
names = list(self._procs.keys())
|
||||
for name in names:
|
||||
self.stop(name, timeout=timeout)
|
||||
@@ -0,0 +1,5 @@
|
||||
fastapi>=0.110
|
||||
uvicorn[standard]>=0.29
|
||||
websockets>=12.0
|
||||
numpy>=1.22
|
||||
PyYAML>=6.0
|
||||
@@ -0,0 +1,282 @@
|
||||
"""Hosts rclpy inside the FastAPI process.
|
||||
|
||||
Mirrors the isolation pattern used by RosSubscriber in scan_gui_triple.py:
|
||||
a dedicated rclpy.Context + Node, spun in a daemon thread, kept out of the
|
||||
ASGI event loop entirely. One node, one MultiThreadedExecutor, one shared
|
||||
ReentrantCallbackGroup for every subscription.
|
||||
|
||||
/livox/imu (~200Hz) is deliberately NOT subscribed here — it's ~20x every
|
||||
other topic's rate, and every way of grouping it alongside slower topics
|
||||
(shared default group, shared Reentrant group, its own dedicated
|
||||
Context+Node+thread, split-by-cost with the other cheap raw subscriptions)
|
||||
measurably starved the slower ones, cameras worst of all — confirmed
|
||||
2026-08-06 across several configurations, right down to cameras measuring
|
||||
~0Hz against a real ~10Hz feed even after every other topic's callback was
|
||||
made cheap (raw=True). The operator decided camera accuracy matters more
|
||||
than a live IMU Hz reading, so IMU health is now inferred from lidar's
|
||||
health instead of measured directly (see routers/system.py) — they come off
|
||||
the same livox_ros_driver2 process, so lidar being alive is already strong
|
||||
evidence IMU is too. If IMU health/Hz needs to be real again later, give it
|
||||
back its own dedicated Context+Node+executor+thread (not a shared one) and
|
||||
expect to spend real GIL budget on it.
|
||||
|
||||
Subscriptions cover: camera images (JPEG-encoded for MJPEG serving), the raw
|
||||
lidar driver topic (health only), and (M3) FAST-LIO's point cloud +
|
||||
odometry for the live scan view. GPS (M2) is not yet added. All
|
||||
subscriptions register once at startup, never per start/stop call, so
|
||||
health.py can tell "process down" apart from "never subscribed".
|
||||
|
||||
Cross-thread handoff: ROS callbacks (background thread) write into a
|
||||
lock-guarded "latest value" dict; HTTP/WS handlers (asyncio loop) read it.
|
||||
No queues — a slow reader just sees a slightly stale value, never backs up
|
||||
ROS callback processing.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import config
|
||||
from config import camera_specs
|
||||
from health import TopicHealthTracker
|
||||
from pointcloud_codec import PointCloudAccumulator
|
||||
|
||||
logger = logging.getLogger("scan_web.ros_bridge")
|
||||
|
||||
try:
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
|
||||
from sensor_msgs.msg import Image, PointCloud2
|
||||
from nav_msgs.msg import Odometry
|
||||
from livox_ros_driver2.msg import CustomMsg
|
||||
from cv_bridge import CvBridge
|
||||
import sensor_msgs_py.point_cloud2 as pc2
|
||||
import cv2
|
||||
ROS_AVAILABLE = True
|
||||
except ImportError:
|
||||
ROS_AVAILABLE = False
|
||||
|
||||
|
||||
class ROSBridge:
|
||||
def __init__(self, camera_count: int):
|
||||
self.camera_count = camera_count
|
||||
self.health = TopicHealthTracker()
|
||||
self._running = False
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self._ctx = None # set in _spin(); stop() shuts it down to unblock executor.spin()
|
||||
self._bridge = CvBridge() if ROS_AVAILABLE else None
|
||||
self._frame_lock = threading.Lock()
|
||||
self._latest_jpeg: dict[str, bytes] = {}
|
||||
self._raw_lock = threading.Lock()
|
||||
self._latest_raw: dict[str, object] = {}
|
||||
self._frame_events: dict[str, threading.Event] = {}
|
||||
self.pointcloud = PointCloudAccumulator(
|
||||
config.POINTCLOUD_VOXEL_SIZE_M, config.POINTCLOUD_MAX_ACCUM_POINTS,
|
||||
)
|
||||
|
||||
# ── lifecycle ──────────────────────────────────────────────────────────
|
||||
|
||||
def start(self) -> None:
|
||||
if not ROS_AVAILABLE:
|
||||
logger.warning("rclpy not available (ROS overlays not sourced?) — camera preview disabled")
|
||||
return
|
||||
if self._running:
|
||||
return
|
||||
self._running = True
|
||||
self._thread = threading.Thread(target=self._spin, daemon=True, name="ros-bridge")
|
||||
self._thread.start()
|
||||
|
||||
def stop(self) -> None:
|
||||
self._running = False
|
||||
if self._ctx is not None:
|
||||
# Unblocks the executor.spin() call in _spin()'s thread from out
|
||||
# here — spin() only checks context.ok()/is_shutdown, it doesn't
|
||||
# know about self._running.
|
||||
try:
|
||||
self._ctx.shutdown()
|
||||
except Exception:
|
||||
pass
|
||||
if self._thread:
|
||||
self._thread.join(timeout=2.0)
|
||||
self._thread = None
|
||||
|
||||
def _spin(self) -> None:
|
||||
ctx = rclpy.Context()
|
||||
ctx.init()
|
||||
self._ctx = ctx
|
||||
try:
|
||||
node = rclpy.create_node("scan_web_bridge", context=ctx)
|
||||
# One shared ReentrantCallbackGroup for every subscription below
|
||||
# — the node's default MutuallyExclusiveCallbackGroup would
|
||||
# serialize dispatch across all of them regardless of executor
|
||||
# type. No /livox/imu here (see module docstring) — with it
|
||||
# removed, the remaining topics are all roughly the same order
|
||||
# of magnitude (~10Hz), so they coexist fairly under one
|
||||
# Reentrant group without needing to be split further.
|
||||
cbg = ReentrantCallbackGroup()
|
||||
img_qos = QoSProfile(
|
||||
reliability=QoSReliabilityPolicy.BEST_EFFORT,
|
||||
history=QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
for cam in camera_specs(self.camera_count):
|
||||
node.create_subscription(
|
||||
Image, cam.image_topic,
|
||||
lambda msg, c=cam.id, t=cam.image_topic: self._on_image(c, t, msg),
|
||||
img_qos, callback_group=cbg,
|
||||
)
|
||||
# Raw (undeserialized) lidar driver topic — subscribed only for
|
||||
# staleness/health tracking, not decoded/stored. This callback
|
||||
# never touches the payload, so raw=True skips deserialization
|
||||
# cost entirely (mirrors what `ros2 bag record` does
|
||||
# internally). Distinct from the LIO mapping output
|
||||
# (/aft_mapped_to_init etc.), which is M3 scope.
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=QoSReliabilityPolicy.BEST_EFFORT,
|
||||
history=QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
node.create_subscription(
|
||||
CustomMsg, "/livox/lidar",
|
||||
lambda msg: self.health.mark_received("/livox/lidar"), sensor_qos, callback_group=cbg, raw=True,
|
||||
)
|
||||
# M3 — FAST-LIO's live mapping output. /cloud_registered is only
|
||||
# published while the "fastlio" process is running (tied to the
|
||||
# recording lifecycle, see routers/recording.py), same as lidar
|
||||
# is gated on the "lidar" process.
|
||||
node.create_subscription(
|
||||
PointCloud2, config.POINTCLOUD_TOPIC, self._on_pointcloud, sensor_qos, callback_group=cbg,
|
||||
)
|
||||
node.create_subscription(
|
||||
Odometry, config.ODOMETRY_TOPIC, self._on_odometry, sensor_qos, callback_group=cbg,
|
||||
)
|
||||
# MultiThreadedExecutor (not Single) — spin_once() on either type
|
||||
# still only pulls ONE ready entity per call, but Single runs its
|
||||
# callback synchronously inline before the next pull, whereas
|
||||
# MultiThreaded submits it to a worker pool and returns
|
||||
# immediately, so the poll loop keeps up with several ~10Hz
|
||||
# topics instead of falling behind and starving each other.
|
||||
# num_threads is deliberately just above the subscription count
|
||||
# (6: 3 cameras + lidar + pointcloud + odometry) rather than
|
||||
# generously oversized — Python's GIL means more threads doesn't
|
||||
# mean more parallel throughput, only more context-switch
|
||||
# overhead once you're past "enough to avoid queueing".
|
||||
executor = MultiThreadedExecutor(context=ctx, num_threads=6)
|
||||
executor.add_node(node)
|
||||
executor.spin()
|
||||
executor.remove_node(node)
|
||||
node.destroy_node()
|
||||
except Exception:
|
||||
logger.exception("ros bridge spin loop crashed")
|
||||
finally:
|
||||
self._ctx = None
|
||||
try:
|
||||
ctx.shutdown()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# ── callbacks ──────────────────────────────────────────────────────────
|
||||
|
||||
def _on_image(self, cam_id: str, topic: str, msg) -> None:
|
||||
# Record arrival immediately and hand the deserialized msg off to a
|
||||
# per-camera worker thread. cv_bridge/JPEG encoding is too slow to
|
||||
# run inline here — running it directly in the ROS callback would
|
||||
# tie up that callback's executor thread for the encode's duration,
|
||||
# needlessly limiting how many camera frames can be in flight
|
||||
# concurrently.
|
||||
self.health.mark_received(topic)
|
||||
with self._raw_lock:
|
||||
self._latest_raw[cam_id] = msg
|
||||
ev = self._frame_events.get(cam_id)
|
||||
if ev is None:
|
||||
ev = threading.Event()
|
||||
self._frame_events[cam_id] = ev
|
||||
threading.Thread(
|
||||
target=self._encode_loop, args=(cam_id,), daemon=True, name=f"encode-{cam_id}",
|
||||
).start()
|
||||
ev.set()
|
||||
|
||||
def _encode_loop(self, cam_id: str) -> None:
|
||||
ev = self._frame_events[cam_id]
|
||||
while self._running:
|
||||
if not ev.wait(timeout=0.5):
|
||||
continue
|
||||
ev.clear()
|
||||
with self._raw_lock:
|
||||
msg = self._latest_raw.get(cam_id)
|
||||
if msg is None:
|
||||
continue
|
||||
try:
|
||||
frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
|
||||
ok, buf = cv2.imencode(".jpg", frame, [cv2.IMWRITE_JPEG_QUALITY, 80])
|
||||
if not ok:
|
||||
continue
|
||||
with self._frame_lock:
|
||||
self._latest_jpeg[cam_id] = buf.tobytes()
|
||||
except Exception:
|
||||
logger.exception("failed to decode frame for %s", cam_id)
|
||||
|
||||
def _on_pointcloud(self, msg) -> None:
|
||||
# Same reasoning as _on_image: read_points_numpy + voxel-dedup
|
||||
# accumulation is too slow to run inline on the shared spin thread,
|
||||
# so hand the raw msg off to a dedicated worker thread.
|
||||
self.health.mark_received(config.POINTCLOUD_TOPIC)
|
||||
with self._raw_lock:
|
||||
self._latest_raw["pointcloud"] = msg
|
||||
ev = self._frame_events.get("pointcloud")
|
||||
if ev is None:
|
||||
ev = threading.Event()
|
||||
self._frame_events["pointcloud"] = ev
|
||||
threading.Thread(target=self._pointcloud_loop, daemon=True, name="pointcloud-accum").start()
|
||||
ev.set()
|
||||
|
||||
def _pointcloud_loop(self) -> None:
|
||||
ev = self._frame_events["pointcloud"]
|
||||
while self._running:
|
||||
if not ev.wait(timeout=0.5):
|
||||
continue
|
||||
ev.clear()
|
||||
with self._raw_lock:
|
||||
msg = self._latest_raw.get("pointcloud")
|
||||
if msg is None:
|
||||
continue
|
||||
try:
|
||||
arr = pc2.read_points_numpy(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True)
|
||||
if arr.size == 0:
|
||||
continue
|
||||
xyz = np.ascontiguousarray(arr[:, :3], dtype=np.float32)
|
||||
intensity = np.ascontiguousarray(arr[:, 3], dtype=np.float32)
|
||||
self.pointcloud.add_scan(xyz, intensity)
|
||||
except Exception:
|
||||
logger.exception("failed to accumulate point cloud")
|
||||
|
||||
def _on_odometry(self, msg) -> None:
|
||||
# Cheap (a few float unpacks) — fine to run inline on the spin thread.
|
||||
self.health.mark_received(config.ODOMETRY_TOPIC)
|
||||
p = msg.pose.pose.position
|
||||
q = msg.pose.pose.orientation
|
||||
self.pointcloud.set_pose(p.x, p.y, p.z, q.x, q.y, q.z, q.w)
|
||||
|
||||
# ── readers (called from asyncio handlers) ───────────────────────────────
|
||||
|
||||
def latest_jpeg(self, cam_id: str) -> Optional[bytes]:
|
||||
with self._frame_lock:
|
||||
return self._latest_jpeg.get(cam_id)
|
||||
|
||||
def mjpeg_generator(self, cam_id: str, target_hz: float = 12.0):
|
||||
"""Blocking generator (run in a threadpool) yielding multipart MJPEG chunks."""
|
||||
period = 1.0 / target_hz
|
||||
boundary = b"--frame"
|
||||
while self._running:
|
||||
frame = self.latest_jpeg(cam_id)
|
||||
if frame is not None:
|
||||
yield (
|
||||
boundary + b"\r\nContent-Type: image/jpeg\r\nContent-Length: "
|
||||
+ str(len(frame)).encode() + b"\r\n\r\n" + frame + b"\r\n"
|
||||
)
|
||||
time.sleep(period)
|
||||
@@ -0,0 +1,82 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from fastapi import APIRouter, HTTPException, Request
|
||||
from fastapi.responses import StreamingResponse
|
||||
from pydantic import BaseModel
|
||||
from starlette.concurrency import run_in_threadpool
|
||||
|
||||
import camera_params
|
||||
import config
|
||||
|
||||
router = APIRouter(prefix="/api/camera", tags=["camera"])
|
||||
|
||||
|
||||
class ParamValuesIn(BaseModel):
|
||||
exposure_auto: bool
|
||||
exposure_auto_target_brightness: int
|
||||
exposure_auto_min: float
|
||||
exposure_auto_max: float
|
||||
exposure_time: int
|
||||
gain: float
|
||||
|
||||
|
||||
def _get_spec(request: Request, cam_id: str) -> config.CameraSpec:
|
||||
session = request.app.state.session
|
||||
specs = {c.id: c for c in config.camera_specs(session.camera_count)}
|
||||
spec = specs.get(cam_id)
|
||||
if spec is None:
|
||||
raise HTTPException(404, f"unknown camera id '{cam_id}' for camera_count={session.camera_count}")
|
||||
return spec
|
||||
|
||||
|
||||
@router.get("/{cam_id}/params")
|
||||
async def get_params(cam_id: str, request: Request):
|
||||
spec = _get_spec(request, cam_id)
|
||||
session = request.app.state.session
|
||||
values = session.camera_values.get(cam_id) or camera_params.load_defaults(spec)
|
||||
session.camera_values[cam_id] = values
|
||||
return values.as_dict()
|
||||
|
||||
|
||||
@router.post("/{cam_id}/params")
|
||||
async def set_params(cam_id: str, body: ParamValuesIn, request: Request):
|
||||
spec = _get_spec(request, cam_id)
|
||||
session = request.app.state.session
|
||||
values = camera_params.CameraParamValues(**body.model_dump())
|
||||
session.camera_values[cam_id] = values
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/{cam_id}/params/apply")
|
||||
async def apply_params(cam_id: str, request: Request):
|
||||
spec = _get_spec(request, cam_id)
|
||||
session = request.app.state.session
|
||||
pm = request.app.state.pm
|
||||
if not pm.is_running("camera"):
|
||||
raise HTTPException(409, "camera is not running")
|
||||
values = session.camera_values.get(cam_id) or camera_params.load_defaults(spec)
|
||||
results = await run_in_threadpool(camera_params.apply_params, spec, values)
|
||||
ok = all(r["ok"] for r in results)
|
||||
return {"ok": ok, "results": results}
|
||||
|
||||
|
||||
@router.post("/{cam_id}/params/save")
|
||||
async def save_params(cam_id: str, request: Request):
|
||||
spec = _get_spec(request, cam_id)
|
||||
session = request.app.state.session
|
||||
values = session.camera_values.get(cam_id) or camera_params.load_defaults(spec)
|
||||
try:
|
||||
await run_in_threadpool(camera_params.save_params, spec, values)
|
||||
except Exception as e:
|
||||
raise HTTPException(500, f"save failed: {e}")
|
||||
return {"ok": True, "path": spec.base_params_path}
|
||||
|
||||
|
||||
@router.get("/{cam_id}/stream.mjpg")
|
||||
async def stream(cam_id: str, request: Request):
|
||||
_get_spec(request, cam_id) # validate
|
||||
ros = request.app.state.ros
|
||||
return StreamingResponse(
|
||||
ros.mjpeg_generator(cam_id),
|
||||
media_type="multipart/x-mixed-replace; boundary=frame",
|
||||
)
|
||||
@@ -0,0 +1,119 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
from fastapi import APIRouter, HTTPException, Request
|
||||
from pydantic import BaseModel
|
||||
|
||||
import bag_metadata
|
||||
import config
|
||||
|
||||
router = APIRouter(tags=["recording"])
|
||||
|
||||
# Directory browsing/recording is restricted to the user's home tree — the
|
||||
# browser has no server-filesystem access, so /api/fs/browse is a small
|
||||
# custom endpoint standing in for a native folder picker.
|
||||
BROWSE_ROOT = Path.home()
|
||||
|
||||
|
||||
def _safe_path(raw: str) -> Path:
|
||||
p = (BROWSE_ROOT / raw).resolve() if not os.path.isabs(raw) else Path(raw).resolve()
|
||||
try:
|
||||
p.relative_to(BROWSE_ROOT.resolve())
|
||||
except ValueError:
|
||||
raise HTTPException(400, f"path must be under {BROWSE_ROOT}")
|
||||
return p
|
||||
|
||||
|
||||
@router.get("/fs/browse")
|
||||
async def browse(path: str = str(BROWSE_ROOT)):
|
||||
p = _safe_path(path)
|
||||
if not p.exists() or not p.is_dir():
|
||||
raise HTTPException(404, "not a directory")
|
||||
entries = []
|
||||
for child in sorted(p.iterdir()):
|
||||
if child.is_dir():
|
||||
entries.append({"name": child.name, "path": str(child)})
|
||||
return {"path": str(p), "parent": str(p.parent) if p != BROWSE_ROOT.resolve() else None, "dirs": entries}
|
||||
|
||||
|
||||
@router.get("/bags")
|
||||
async def list_bags(save_dir: str = config.DEFAULT_SAVE_DIR, limit: int = 10):
|
||||
d = Path(save_dir)
|
||||
if not d.exists():
|
||||
return {"bags": []}
|
||||
bags = []
|
||||
children = sorted(d.iterdir(), key=lambda p: p.stat().st_mtime, reverse=True)
|
||||
for child in children:
|
||||
if not child.is_dir():
|
||||
continue
|
||||
if len(bags) >= limit:
|
||||
break
|
||||
meta = bag_metadata.read_bag_metadata(child)
|
||||
bags.append({
|
||||
"name": child.name,
|
||||
"path": str(child),
|
||||
"mtime": child.stat().st_mtime,
|
||||
"duration_s": meta["duration_s"] if meta else None,
|
||||
"message_count": meta["message_count"] if meta else None,
|
||||
"topics": meta["topics"] if meta else [],
|
||||
"size_bytes": meta["size_bytes"] if meta else bag_metadata.dir_size_bytes(child),
|
||||
"complete": meta is not None, # False = still recording, or was killed before flush
|
||||
})
|
||||
return {"bags": bags}
|
||||
|
||||
|
||||
@router.get("/fs/disk-usage")
|
||||
async def disk_usage(path: str = config.DEFAULT_SAVE_DIR):
|
||||
return {"free_gb": round(config.disk_free_gb(path), 2)}
|
||||
|
||||
|
||||
class RecordingStartIn(BaseModel):
|
||||
filename: str
|
||||
save_dir: str = config.DEFAULT_SAVE_DIR
|
||||
|
||||
|
||||
@router.post("/recording/start")
|
||||
async def recording_start(body: RecordingStartIn, request: Request):
|
||||
session = request.app.state.session
|
||||
pm = request.app.state.pm
|
||||
|
||||
if not pm.is_running("lidar") or not pm.is_running("camera"):
|
||||
raise HTTPException(409, "start lidar+camera before recording")
|
||||
if session.recording:
|
||||
raise HTTPException(409, "already recording")
|
||||
if not body.filename.strip():
|
||||
raise HTTPException(400, "filename required")
|
||||
|
||||
save_dir = body.save_dir.strip() or config.DEFAULT_SAVE_DIR
|
||||
os.makedirs(save_dir, exist_ok=True)
|
||||
bag_path = os.path.join(save_dir, body.filename.strip())
|
||||
topics = config.base_record_topics(session.camera_count)
|
||||
|
||||
# FAST-LIO tracks the scan (LiDAR+IMU-only mapping) for exactly the
|
||||
# recording's lifetime — started alongside the bag, stopped alongside it.
|
||||
# It only needs /livox/lidar + /livox/imu, both already live since
|
||||
# lidar+camera are required (checked above) before recording can start.
|
||||
request.app.state.ros.pointcloud.reset() # fresh map for this recording, not last one's leftovers
|
||||
pm.start("fastlio", config.FASTLIO_LAUNCH_CMD, log_path=config.FASTLIO_LOG_PATH, env=config.fastlio_env())
|
||||
|
||||
cmd = f"{config.ROSBAG_SOURCE} && stdbuf -oL -eL ros2 bag record -o {bag_path} {topics}"
|
||||
pm.start("recording", cmd, log_path=config.RECORDING_LOG_PATH)
|
||||
session.recording = True
|
||||
session.recording_path = bag_path
|
||||
session.recording_started_at = time.monotonic()
|
||||
return {"ok": True, "path": bag_path}
|
||||
|
||||
|
||||
@router.post("/recording/stop")
|
||||
async def recording_stop(request: Request):
|
||||
session = request.app.state.session
|
||||
pm = request.app.state.pm
|
||||
pm.stop("recording")
|
||||
pm.stop("fastlio")
|
||||
session.recording = False
|
||||
session.recording_path = None
|
||||
session.recording_started_at = None
|
||||
return {"ok": True}
|
||||
@@ -0,0 +1,212 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
from fastapi import APIRouter, HTTPException, Request
|
||||
from pydantic import BaseModel
|
||||
|
||||
import bag_metadata
|
||||
import camera_params
|
||||
import config
|
||||
from ros_bridge import ROSBridge
|
||||
|
||||
router = APIRouter()
|
||||
|
||||
|
||||
class SessionConfig(BaseModel):
|
||||
camera_count: int
|
||||
|
||||
|
||||
@router.post("/session")
|
||||
async def set_session(cfg: SessionConfig, request: Request):
|
||||
if cfg.camera_count not in config.CAMERA_PROFILES:
|
||||
raise HTTPException(400, f"unsupported camera_count, expected one of {sorted(config.CAMERA_PROFILES)}")
|
||||
session = request.app.state.session
|
||||
pm = request.app.state.pm
|
||||
if pm.is_running("lidar") or pm.is_running("camera"):
|
||||
raise HTTPException(409, "stop lidar/camera before changing camera_count")
|
||||
|
||||
session.camera_count = cfg.camera_count
|
||||
# ROSBridge subscriptions are fixed at construction (see ros_bridge.py) —
|
||||
# changing camera_count means tearing down and recreating the bridge.
|
||||
old_ros: ROSBridge = request.app.state.ros
|
||||
old_ros.stop()
|
||||
new_ros = ROSBridge(camera_count=cfg.camera_count)
|
||||
new_ros.start()
|
||||
request.app.state.ros = new_ros
|
||||
return {"ok": True, "camera_count": session.camera_count}
|
||||
|
||||
|
||||
def _camera_launch_cmd(app) -> str:
|
||||
session = app.state.session
|
||||
specs = config.camera_specs(session.camera_count)
|
||||
launch_file = config.camera_launch_file(session.camera_count)
|
||||
args = []
|
||||
for cam in specs:
|
||||
values = session.camera_values.get(cam.id) or camera_params.load_defaults(cam)
|
||||
session.camera_values[cam.id] = values
|
||||
path = camera_params.write_launch_params_yaml(cam, values)
|
||||
args.append(f" {cam.launch_arg}:={path}")
|
||||
return (
|
||||
f"{config.CAMERA_LAUNCH_SOURCE} && "
|
||||
f"stdbuf -oL -eL ros2 launch hik_camera_ros2_driver {launch_file}" + "".join(args)
|
||||
)
|
||||
|
||||
|
||||
async def _auto_start_camera_after_delay(app):
|
||||
await asyncio.sleep(config.CAMERA_DELAY_SEC)
|
||||
pm = app.state.pm
|
||||
if pm.is_running("lidar") and not pm.is_running("camera"):
|
||||
pm.start("camera", _camera_launch_cmd(app), log_path=config.CAMERA_LOG_PATH)
|
||||
|
||||
|
||||
@router.post("/lidar/start")
|
||||
async def lidar_start(request: Request):
|
||||
pm = request.app.state.pm
|
||||
pm.start("lidar", config.LIDAR_LAUNCH_CMD, log_path=config.LIDAR_LOG_PATH)
|
||||
asyncio.create_task(_auto_start_camera_after_delay(request.app))
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/lidar/stop")
|
||||
async def lidar_stop(request: Request):
|
||||
pm = request.app.state.pm
|
||||
session = request.app.state.session
|
||||
pm.stop("lidar")
|
||||
pm.stop("camera")
|
||||
# fastlio has no sensor data source once lidar stops — tear it down here
|
||||
# too, whether it's running because a recording is active or because the
|
||||
# operator started it standalone via the settings test panel.
|
||||
pm.stop("fastlio")
|
||||
if session.recording:
|
||||
pm.stop("recording")
|
||||
session.recording = False
|
||||
session.recording_path = None
|
||||
session.recording_started_at = None
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/camera/start")
|
||||
async def camera_start(request: Request):
|
||||
pm = request.app.state.pm
|
||||
if not pm.is_running("lidar"):
|
||||
raise HTTPException(409, "start lidar first")
|
||||
pm.start("camera", _camera_launch_cmd(request.app), log_path=config.CAMERA_LOG_PATH)
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/camera/stop")
|
||||
async def camera_stop(request: Request):
|
||||
request.app.state.pm.stop("camera")
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/fastlio/start")
|
||||
async def fastlio_start(request: Request):
|
||||
"""Manual FAST-LIO start, independent of recording — lets the operator
|
||||
preview lidar point-cloud quality (rig leveling, coverage) in Settings
|
||||
before committing to an actual recording."""
|
||||
pm = request.app.state.pm
|
||||
if not pm.is_running("lidar"):
|
||||
raise HTTPException(409, "start lidar first")
|
||||
request.app.state.ros.pointcloud.reset()
|
||||
pm.start("fastlio", config.FASTLIO_LAUNCH_CMD, log_path=config.FASTLIO_LOG_PATH, env=config.fastlio_env())
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/fastlio/stop")
|
||||
async def fastlio_stop(request: Request):
|
||||
if request.app.state.session.recording:
|
||||
raise HTTPException(409, "recording is using fastlio — stop the recording instead")
|
||||
request.app.state.pm.stop("fastlio")
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
@router.post("/system/estop")
|
||||
async def estop(request: Request):
|
||||
request.app.state.pm.stop_all()
|
||||
request.app.state.session.recording = False
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
def build_status(app) -> dict:
|
||||
from health import TopicThreshold
|
||||
|
||||
pm = app.state.pm
|
||||
ros: ROSBridge = app.state.ros
|
||||
session = app.state.session
|
||||
specs = config.camera_specs(session.camera_count)
|
||||
|
||||
lidar_running = pm.is_running("lidar")
|
||||
camera_running = pm.is_running("camera")
|
||||
fastlio_running = pm.is_running("fastlio")
|
||||
|
||||
lidar_thr = config.HEALTH_THRESHOLDS["/livox/lidar"]
|
||||
cam_thr = config.HEALTH_THRESHOLDS["camera_image"]
|
||||
map_thr = config.HEALTH_THRESHOLDS["/Odometry"]
|
||||
|
||||
cameras = []
|
||||
for cam in specs:
|
||||
health = ros.health.classify(
|
||||
cam.image_topic, camera_running, TopicThreshold(*cam_thr)
|
||||
)
|
||||
cameras.append({
|
||||
"id": cam.id, "topic": cam.image_topic, "health": health,
|
||||
"hz": ros.health.rate_hz(cam.image_topic),
|
||||
})
|
||||
|
||||
recording_elapsed_s = None
|
||||
recording_size_bytes = None
|
||||
disk_check_dir = config.DEFAULT_SAVE_DIR
|
||||
if session.recording and session.recording_path:
|
||||
if session.recording_started_at is not None:
|
||||
recording_elapsed_s = round(time.monotonic() - session.recording_started_at, 1)
|
||||
recording_size_bytes = bag_metadata.dir_size_bytes(Path(session.recording_path))
|
||||
disk_check_dir = str(Path(session.recording_path).parent)
|
||||
|
||||
lidar_health = ros.health.classify("/livox/lidar", lidar_running, TopicThreshold(*lidar_thr))
|
||||
|
||||
return {
|
||||
"camera_count": session.camera_count,
|
||||
"default_save_dir": config.DEFAULT_SAVE_DIR,
|
||||
"lidar": {
|
||||
"running": lidar_running,
|
||||
"health": lidar_health,
|
||||
# /livox/imu isn't subscribed at all anymore (see ros_bridge.py's
|
||||
# module docstring — its ~200Hz starved the camera topics badly
|
||||
# enough that the operator asked to drop it, 2026-08-06). It
|
||||
# comes off the same livox_ros_driver2 process as the lidar
|
||||
# topic, so lidar's own health is a reasonable stand-in; there's
|
||||
# no real Hz number to report, so imu_hz stays 0 (the HUD badge
|
||||
# just shows the health word with no Hz suffix in that case).
|
||||
"imu_health": lidar_health,
|
||||
"hz": ros.health.rate_hz("/livox/lidar"),
|
||||
"imu_hz": 0.0,
|
||||
},
|
||||
"cameras": cameras,
|
||||
"map": {
|
||||
"health": ros.health.classify(config.ODOMETRY_TOPIC, fastlio_running, TopicThreshold(*map_thr)),
|
||||
"hz": ros.health.rate_hz(config.ODOMETRY_TOPIC),
|
||||
"points": ros.pointcloud.point_count(),
|
||||
},
|
||||
"recording": {
|
||||
"active": session.recording,
|
||||
"path": session.recording_path,
|
||||
"elapsed_s": recording_elapsed_s,
|
||||
"size_bytes": recording_size_bytes,
|
||||
},
|
||||
"disk": {
|
||||
"path": disk_check_dir,
|
||||
"free_gb": round(config.disk_free_gb(disk_check_dir), 2),
|
||||
"low_warning_gb": config.DISK_LOW_WARNING_GB,
|
||||
"low_danger_gb": config.DISK_LOW_DANGER_GB,
|
||||
},
|
||||
"process_status": pm.status(),
|
||||
}
|
||||
|
||||
|
||||
@router.get("/status")
|
||||
async def status(request: Request):
|
||||
return build_status(request.app)
|
||||
@@ -0,0 +1,74 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
|
||||
from fastapi import APIRouter, WebSocket, WebSocketDisconnect
|
||||
|
||||
import config
|
||||
from log_tail import LogTailer
|
||||
from routers.system import build_status
|
||||
|
||||
router = APIRouter()
|
||||
|
||||
STATUS_PUSH_HZ = 1.0 # slowed from 2.0 — paired with health.py's wider RATE_WINDOW,
|
||||
# the operator asked for a calmer/more trustworthy number over a
|
||||
# snappier one (2026-08-06).
|
||||
LOG_POLL_HZ = 4.0
|
||||
|
||||
|
||||
@router.websocket("/ws/status")
|
||||
async def ws_status(ws: WebSocket):
|
||||
await ws.accept()
|
||||
period = 1.0 / STATUS_PUSH_HZ
|
||||
try:
|
||||
while True:
|
||||
await ws.send_json(build_status(ws.app))
|
||||
await asyncio.sleep(period)
|
||||
except WebSocketDisconnect:
|
||||
pass
|
||||
|
||||
|
||||
@router.websocket("/ws/pointcloud")
|
||||
async def ws_pointcloud(ws: WebSocket, profile: str = "desktop"):
|
||||
"""M3 live scan view — binary-framed decimated point cloud, see
|
||||
pointcloud_codec.py for the wire format. `profile` picks desktop vs.
|
||||
phone point/Hz budgets rather than trusting client-supplied numbers
|
||||
directly (a client could otherwise ask for an unbounded frame size)."""
|
||||
await ws.accept()
|
||||
prof = config.POINTCLOUD_PROFILES.get(profile, config.POINTCLOUD_PROFILES["desktop"])
|
||||
period = 1.0 / prof["hz"]
|
||||
ros = ws.app.state.ros
|
||||
try:
|
||||
while True:
|
||||
await ws.send_bytes(ros.pointcloud.encode_frame(max_points=prof["max_points"]))
|
||||
await asyncio.sleep(period)
|
||||
except WebSocketDisconnect:
|
||||
pass
|
||||
|
||||
|
||||
@router.websocket("/ws/logs")
|
||||
async def ws_logs(ws: WebSocket):
|
||||
"""Unified log tail — mirrors the old PyQt GUIs' single scrollback pane.
|
||||
Sends recent backlog once on connect, then only newly-appended lines."""
|
||||
await ws.accept()
|
||||
tailer = LogTailer({
|
||||
"lidar": config.LIDAR_LOG_PATH,
|
||||
"camera": config.CAMERA_LOG_PATH,
|
||||
"recording": config.RECORDING_LOG_PATH,
|
||||
"fastlio": config.FASTLIO_LOG_PATH,
|
||||
})
|
||||
backlog = []
|
||||
for tag in ("lidar", "camera", "recording", "fastlio"):
|
||||
backlog += [{"tag": tag, "line": ln} for ln in tailer.read_tail(tag, max_lines=80)]
|
||||
if backlog:
|
||||
await ws.send_json({"backlog": backlog})
|
||||
|
||||
period = 1.0 / LOG_POLL_HZ
|
||||
try:
|
||||
while True:
|
||||
new_lines = [{"tag": tag, "line": line} for tag, line in tailer.read_new()]
|
||||
if new_lines:
|
||||
await ws.send_json({"lines": new_lines})
|
||||
await asyncio.sleep(period)
|
||||
except WebSocketDisconnect:
|
||||
pass
|
||||
Reference in New Issue
Block a user