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"})
resources/slam-ros-bridge/test_cameras.py
+67
View File
@@ -0,0 +1,67 @@
import ast
from pathlib import Path
import re
import threading
from types import SimpleNamespace
from typing import Optional
import unittest
class CameraTests(unittest.TestCase):
def setUp(self):
self.now = 10.0
source = ast.parse(Path(__file__).with_name('bridge.py').read_text())
node = next(item for item in source.body if isinstance(item, ast.ClassDef) and item.name == 'SlamNode')
names = ('_discover_cameras', '_on_camera_image', 'camera_sources', 'camera_frame_jpeg')
methods = [item for item in node.body if isinstance(item, ast.FunctionDef) and item.name in names]
scope = {'re': re, 'Optional': Optional, 'CompressedImage': object,
'time': SimpleNamespace(time=lambda: self.now), '_CAMERA_FRAME_MAX_AGE_SECONDS': 3,
'qos_profile_sensor_data': object()}
exec(compile(ast.Module(body=methods, type_ignores=[]), 'camera-bridge', 'exec'), scope)
self.node = type('CameraNode', (), {name: scope[name] for name in names})()
self.node._lock = threading.Lock()
self.node._camera_frames = {}
self.node._camera_subscriptions = {}
self.node._camera_image = None
self.node._last_camera_image_wall_time = None
self.node._camera_image_count = 0
self.node.camera_image_topic = '/camera_test/image_jpeg'
self.topics = []
self.callbacks = {}
self.node.get_topic_names_and_types = lambda: self.topics
def subscribe(kind, topic, callback, qos):
self.callbacks[topic] = callback
return topic
self.node.create_subscription = subscribe
self.node.destroy_subscription = lambda sub: self.callbacks.pop(sub)
def test_independent_frames_and_disconnect(self):
for key in ['video0', 'video2']:
self.topics.append((f'/camera_test/cameras/{key}/image_jpeg', ['sensor_msgs/msg/CompressedImage']))
self.topics += [('/other/image_jpeg', ['sensor_msgs/msg/CompressedImage']),
('/camera_test/cameras/video9/image_jpeg', ['sensor_msgs/msg/Image'])]
self.node._discover_cameras()
self.assertEqual(len(self.callbacks), 2)
for topic, callback in self.callbacks.items():
callback(SimpleNamespace(data=topic.encode(), format='jpeg'))
self.assertEqual([item['id'] for item in self.node.camera_sources()], ['video0', 'video2'])
self.assertIn(b'video2', self.node.camera_frame_jpeg('video2'))
self.assertIsNone(self.node.camera_frame_jpeg('../video0'))
self.now += 4
self.assertTrue(all(not item['available'] for item in self.node.camera_sources()))
self.assertIsNone(self.node.camera_frame_jpeg('video0'))
self.topics = self.topics[:1]
self.node._discover_cameras()
self.assertNotIn('video2', self.node._camera_frames)
def test_single_camera_compatibility_and_non_jpeg(self):
self.node._on_camera_image(SimpleNamespace(data=b'jpeg', format='jpeg'))
self.assertEqual(self.node.camera_frame_jpeg(), b'jpeg')
self.assertEqual(self.node.camera_frame_jpeg('legacy'), b'jpeg')
self.node._on_camera_image(SimpleNamespace(data=b'png', format='png'))
self.assertIsNone(self.node.camera_frame_jpeg('legacy'))
self.assertFalse(self.node.camera_sources()[0]['available'])
if __name__ == '__main__':
unittest.main()