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,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)
|
||||
Reference in New Issue
Block a user