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)