platform/lms-fe
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: support multiple camera previews and concurrent agent code navigation

Browse Source
This commit is contained in:
chyoo
2026-10-20 10:10:48 +09:00
parent c17763f59d
commit af54e875df
13 changed files with 525 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
resources/slam-ros-bridge/bridge.py
+53 -8
View File
@@ -6,6 +6,7 @@ import json
import logging
import math
import os
import re
import shutil
import sqlite3
import struct
@@ -207,6 +208,8 @@ class SlamNode(Node):
self._fallback_point_cloud: Optional[PointCloud2] = None
self._color_point_cloud: Optional[PointCloud2] = None
self._camera_image: Optional[CompressedImage] = None
self._camera_frames = {}
self._camera_subscriptions = {}
self._icp_odom: Optional[Odometry] = None
self._map: Optional[OccupancyGrid] = None
self._plan: Optional[NavPath] = None
@@ -261,6 +264,8 @@ class SlamNode(Node):
self.create_subscription(CompressedImage, self.camera_image_topic, self._on_camera_image, qos_profile_sensor_data)
if not os.environ.get("CAMERA_IMAGE_TOPIC"):
self.create_subscription(CompressedImage, "/image_raw/compressed", self._on_camera_image, qos_profile_sensor_data)
self._camera_discovery_timer = self.create_timer(
1.0, self._discover_cameras, clock=Clock(clock_type=ClockType.STEADY_TIME))
self.create_subscription(Odometry, self.icp_odom_topic, self._on_icp_odom, best_effort_qos)
self.create_subscription(Odometry, self.icp_odom_topic, self._on_icp_odom, reliable_qos)
self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, map_qos)
@@ -625,11 +630,45 @@ class SlamNode(Node):
self._color_point_cloud_count += 1
self._last_color_point_cloud_wall_time = time.time()
def _on_camera_image(self, msg: CompressedImage):
def _discover_cameras(self):
try:
topics = sorted(name for name, types in self.get_topic_names_and_types()
if re.fullmatch(r'/camera_test/cameras/video[0-9]+/image_jpeg', name)
and 'sensor_msgs/msg/CompressedImage' in types)[:8]
except Exception:
return
for topic in topics:
camera_id = topic.split('/')[-2]
if camera_id not in self._camera_subscriptions:
self._camera_subscriptions[camera_id] = self.create_subscription(
CompressedImage, topic,
lambda msg, key=camera_id, name=topic: self._on_camera_image(msg, key, name),
qos_profile_sensor_data)
active = {topic.split('/')[-2] for topic in topics}
for camera_id in list(self._camera_subscriptions):
if camera_id not in active:
self.destroy_subscription(self._camera_subscriptions.pop(camera_id))
with self._lock:
self._camera_frames.pop(camera_id, None)
def _on_camera_image(self, msg: CompressedImage, camera_id='legacy', topic=None):
with self._lock:
if camera_id != 'legacy' and camera_id not in self._camera_subscriptions:
return
self._camera_image = msg
self._camera_image_count += 1
self._last_camera_image_wall_time = time.time()
self._camera_frames[camera_id] = (msg, self._last_camera_image_wall_time,
topic or self.camera_image_topic)
def camera_sources(self):
now = time.time()
with self._lock:
frames = list(self._camera_frames.items())
return [{"id": key, "name": f'/dev/{key}' if key != 'legacy' else 'Camera',
"topic": topic, "available": now - received <= _CAMERA_FRAME_MAX_AGE_SECONDS
and bool(image.data) and 'jpeg' in image.format.lower()}
for key, (image, received, topic) in sorted(frames)]
def _active_point_cloud(self) -> Optional[PointCloud2]:
with self._lock:
@@ -687,7 +726,6 @@ class SlamNode(Node):
point_cloud_last = self._last_point_cloud_wall_time
fallback_point_cloud_last = self._last_fallback_point_cloud_wall_time
color_point_cloud_last = self._last_color_point_cloud_wall_time
camera_image_last = self._last_camera_image_wall_time
map_count = self._map_count
plan_count = self._plan_count
@@ -727,7 +765,7 @@ class SlamNode(Node):
},
"camera_image": {
"name": self.camera_image_topic,
"available": received_recently(camera_image_last),
"available": any(camera['available'] for camera in self.camera_sources()),
},
"plan": {"name": self.plan_topic, "available": plan_available},
"cmd_vel": {"name": self.cmd_vel_topic, "available": cmd_vel_available},
@@ -1238,6 +1276,7 @@ class SlamNode(Node):
"sensor_point_cloud": sensor_point_cloud_payload,
"color_point_cloud": color_point_cloud_payload,
"camera_image": camera_image_payload,
"cameras": self.camera_sources(),
"map": map_payload,
"tf": {
"map_to_odom": resolve_2d_transform(tf_snapshot, "map", "odom"),
@@ -1302,10 +1341,16 @@ class SlamNode(Node):
payload["received_at"] = received_at
return payload
def camera_frame_jpeg(self) -> Optional[bytes]:
def camera_frame_jpeg(self, camera_id=None) -> Optional[bytes]:
with self._lock:
image = self._camera_image
received_at = self._last_camera_image_wall_time
if camera_id is not None:
entry = self._camera_frames.get(camera_id)
if entry is None:
return None
image, received_at, _topic = entry
else:
image = self._camera_image
received_at = self._last_camera_image_wall_time
if image is None or received_at is None or time.time() - received_at > _CAMERA_FRAME_MAX_AGE_SECONDS:
return None
if not image.data or "jpeg" not in image.format.lower():
@@ -1531,10 +1576,10 @@ def color_point_cloud():
@app.get("/camera-frame")
def camera_frame():
def camera_frame(camera_id: Optional[str] = None):
if not slam_node:
raise HTTPException(status_code=503, detail="ROS node is not ready")
frame = slam_node.camera_frame_jpeg()
frame = slam_node.camera_frame_jpeg(camera_id)
if frame is None:
raise HTTPException(status_code=404, detail="카메라 프레임을 아직 수신하지 못했습니다.")
return Response(content=frame, media_type="image/jpeg", headers={"Cache-Control": "no-store"})