synchronize source..

2026-08-11 14:23:29 +09:00
parent a88f0920cf
commit f6be625a8f
18 changed files with 1485 additions and 273 deletions
@@ -9,6 +9,8 @@ import os
 import shutil
 import sqlite3
 import struct
+import subprocess
+import tempfile
 import threading
 import time
 import traceback
@@ -17,7 +19,7 @@ from pathlib import Path
 from typing import Optional

 import rclpy
-from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect
+from fastapi import FastAPI, HTTPException, Response, WebSocket, WebSocketDisconnect
 from fastapi.middleware.cors import CORSMiddleware
 from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
 from action_msgs.msg import GoalStatus
@@ -25,20 +27,21 @@ from nav_msgs.msg import OccupancyGrid, Odometry, Path as NavPath
 from nav2_msgs.action import NavigateToPose
 from pydantic import BaseModel
 from rclpy.action import ActionClient
-from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
 from rclpy.time import Time
-from sensor_msgs.msg import JointState, LaserScan, PointCloud2
-from std_msgs.msg import Float64
+from sensor_msgs.msg import CompressedImage, JointState, LaserScan, PointCloud2
+from std_msgs.msg import Float64, String
+from std_srvs.srv import Empty
 from tf2_msgs.msg import TFMessage
 import uvicorn

 logging.basicConfig(level=logging.INFO)

 _POINT_CLOUD_LIMIT = max(1_000, int(os.environ.get("POINT_CLOUD_LIMIT", "50000")))
+_CAMERA_FRAME_MAX_AGE_SECONDS = 3.0


 def yaw_from_quaternion(q) -> float:
@@ -159,6 +162,8 @@ class SlamNode(Node):
        self.amcl_pose_topic = os.environ.get("AMCL_POSE_TOPIC", "/amcl_pose")
        self.point_cloud_topic = os.environ.get("POINT_CLOUD_TOPIC", "/cloud_map")
        self.fallback_point_cloud_topic = os.environ.get("FALLBACK_POINT_CLOUD_TOPIC", "/lidar_filtered")
+        self.color_point_cloud_topic = os.environ.get("COLOR_POINT_CLOUD_TOPIC", "/camera_test/colored_points")
+        self.camera_image_topic = os.environ.get("CAMERA_IMAGE_TOPIC", "/camera_test/image_jpeg")
        self.icp_odom_topic = os.environ.get("ICP_ODOM_TOPIC", "/icp_odom")
        self.cmd_vel_topic = os.environ.get("CMD_VEL_TOPIC", "/cmd_vel")
        self.map_topic = os.environ.get("MAP_TOPIC", "/map")
@@ -166,8 +171,10 @@ class SlamNode(Node):
        self.joint_states_topic = os.environ.get("JOINT_STATES_TOPIC", "/joint_states")
        self.steering_topic = os.environ.get("STEERING_TOPIC", "/mkmini/steering_command")
        self.steering_feedback_topic = os.environ.get("STEERING_FEEDBACK_TOPIC", "/mkmini/steering_angle")
+        self.localization_session_topic = os.environ.get("LOCALIZATION_SESSION_TOPIC", "/localization_session_id")
        self.navigate_to_pose_action = os.environ.get("NAVIGATE_TO_POSE_ACTION", "/navigate_to_pose")
        self.read_only = os.environ.get("READ_ONLY", "false").lower() in ("1", "true", "yes")
+        self.saved_map_database_path = Path(os.environ.get("SAVED_MAP_DATABASE_PATH", ""))

        best_effort_qos = QoSProfile(
            depth=1,
@@ -195,6 +202,8 @@ class SlamNode(Node):
        self._amcl_pose: Optional[PoseWithCovarianceStamped] = None
        self._point_cloud: Optional[PointCloud2] = None
        self._fallback_point_cloud: Optional[PointCloud2] = None
+        self._color_point_cloud: Optional[PointCloud2] = None
+        self._camera_image: Optional[CompressedImage] = None
        self._icp_odom: Optional[Odometry] = None
        self._map: Optional[OccupancyGrid] = None
        self._plan: Optional[NavPath] = None
@@ -207,6 +216,8 @@ class SlamNode(Node):
        self._amcl_pose_count = 0
        self._point_cloud_count = 0
        self._fallback_point_cloud_count = 0
+        self._color_point_cloud_count = 0
+        self._camera_image_count = 0
        self._icp_odom_count = 0
        self._map_count = 0
        self._tf_count = 0
@@ -215,6 +226,8 @@ class SlamNode(Node):
        self._last_amcl_pose_wall_time = None
        self._last_point_cloud_wall_time = None
        self._last_fallback_point_cloud_wall_time = None
+        self._last_color_point_cloud_wall_time = None
+        self._last_camera_image_wall_time = None
        self._last_icp_odom_wall_time = None
        self._last_map_wall_time = None
        self._plan_count = 0
@@ -224,17 +237,25 @@ class SlamNode(Node):
        self._navigation = {"status": "idle", "goal_id": None}
        self._active_goal_handle = None
        self._requested_initial_pose: Optional[dict] = None
+        self._initial_pose_state_path = Path(
+            os.environ.get("INITIAL_POSE_STATE_PATH", "/workspace/.nav2-initial-pose-state.json")
+        )
+        self._initial_pose_set, self._initial_pose_session_id = self._load_initial_pose_state()
+        self._localization_session_id: Optional[str] = None
        self._lock = threading.Lock()
+        self._saved_map_point_cloud = self._load_saved_map_point_cloud()

        self.create_subscription(Odometry, self.odom_topic, self._on_odom, reliable_qos)
        self.create_subscription(Odometry, self.odom_topic, self._on_odom, best_effort_qos)
        self.create_subscription(LaserScan, self.scan_topic, self._on_scan, qos_profile_sensor_data)
-        self.create_subscription(LaserScan, self.scan_topic, self._on_scan, reliable_qos)
        self.create_subscription(PoseWithCovarianceStamped, self.amcl_pose_topic, self._on_amcl_pose, reliable_qos)
        self.create_subscription(PoseWithCovarianceStamped, self.amcl_pose_topic, self._on_amcl_pose, best_effort_qos)
        self.create_subscription(PointCloud2, self.point_cloud_topic, self._on_point_cloud, map_qos)
        if self.fallback_point_cloud_topic != self.point_cloud_topic:
            self.create_subscription(PointCloud2, self.fallback_point_cloud_topic, self._on_fallback_point_cloud, qos_profile_sensor_data)
+        if self.color_point_cloud_topic not in (self.point_cloud_topic, self.fallback_point_cloud_topic):
+            self.create_subscription(PointCloud2, self.color_point_cloud_topic, self._on_color_point_cloud, qos_profile_sensor_data)
+        self.create_subscription(CompressedImage, self.camera_image_topic, self._on_camera_image, qos_profile_sensor_data)
        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)
@@ -242,37 +263,159 @@ class SlamNode(Node):
        self.create_subscription(NavPath, self.plan_topic, self._on_plan, reliable_qos)
        self.create_subscription(JointState, self.joint_states_topic, self._on_joint_states, reliable_qos)
        self.create_subscription(Float64, self.steering_feedback_topic, self._on_steering_feedback, reliable_qos)
+        self.create_subscription(String, self.localization_session_topic, self._on_localization_session, map_qos)
        self.create_subscription(TFMessage, "/tf", self._on_tf, qos_profile_sensor_data)
        self.create_subscription(TFMessage, "/tf_static", self._on_tf, tf_static_qos)
        self._cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
        self._steering_pub = self.create_publisher(Float64, self.steering_topic, 10)
        self._initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10)
        self._navigate_to_pose_client = ActionClient(self, NavigateToPose, self.navigate_to_pose_action)
+        self._rtabmap_backup_client = self.create_client(Empty, "/rtabmap/backup")

        self.get_logger().info(
-            f"SLAM bridge started: odom={self.odom_topic}, icp_odom={self.icp_odom_topic}, scan={self.scan_topic}, point_cloud={self.point_cloud_topic}, fallback_point_cloud={self.fallback_point_cloud_topic}, map={self.map_topic}, read_only={self.read_only}"
+            f"SLAM bridge started: odom={self.odom_topic}, icp_odom={self.icp_odom_topic}, scan={self.scan_topic}, point_cloud={self.point_cloud_topic}, fallback_point_cloud={self.fallback_point_cloud_topic}, color_point_cloud={self.color_point_cloud_topic}, camera_image={self.camera_image_topic}, map={self.map_topic}, read_only={self.read_only}"
        )

+    def _load_saved_map_point_cloud(self) -> Optional[dict]:
+        """Create a stable navigation-map cloud from the selected RTAB-Map DB."""
+        if not self.saved_map_database_path.is_file():
+            return None
+
+        try:
+            with tempfile.TemporaryDirectory(prefix="slam-map-cloud-") as output_dir:
+                output_name = "saved-map"
+                subprocess.run(
+                    [
+                        "rtabmap-export",
+                        "--cloud",
+                        "--scan",
+                        "--voxel",
+                        "0.01",
+                        "--output",
+                        output_name,
+                        "--output_dir",
+                        output_dir,
+                        str(self.saved_map_database_path),
+                    ],
+                    check=True,
+                    stdout=subprocess.DEVNULL,
+                    stderr=subprocess.PIPE,
+                    text=True,
+                    timeout=90,
+                )
+                points = self._read_exported_ply_points(
+                    Path(output_dir) / f"{output_name}_cloud.ply",
+                    _POINT_CLOUD_LIMIT,
+                )
+        except (OSError, subprocess.SubprocessError, ValueError) as error:
+            self.get_logger().warning(f"Saved 3D map export failed: {error}")
+            return None
+
+        if not points:
+            self.get_logger().warning("Saved 3D map export did not contain any points.")
+            return None
+        self.get_logger().info(f"Loaded saved 3D map point cloud: {len(points)} points")
+        return {
+            "frame_id": "map",
+            "points": points,
+            "transform": {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0},
+            "saved_map": True,
+        }
+
+    @staticmethod
+    def _read_exported_ply_points(path: Path, limit: int) -> list[list[float]]:
+        with path.open("rb") as source:
+            header = bytearray()
+            while not header.endswith(b"end_header\n"):
+                chunk = source.read(1)
+                if not chunk or len(header) > 16_384:
+                    raise ValueError("Invalid exported PLY header")
+                header.extend(chunk)
+            lines = header.decode("ascii", errors="strict").splitlines()
+            if "format binary_little_endian 1.0" not in lines:
+                raise ValueError("Unsupported exported PLY format")
+            vertex_line = next((line for line in lines if line.startswith("element vertex ")), None)
+            if vertex_line is None:
+                raise ValueError("Exported PLY does not have vertices")
+            vertex_count = int(vertex_line.rsplit(" ", 1)[1])
+            if vertex_count <= 0:
+                return []
+
+            # rtabmap-export writes x/y/z followed by five float fields.
+            data = source.read(vertex_count * 32)
+            if len(data) < vertex_count * 32:
+                raise ValueError("Exported PLY was truncated")
+            stride = max(1, (vertex_count + max(1, limit) - 1) // max(1, limit))
+            return [
+                list(struct.unpack_from("<3f", data, index * 32))
+                for index in range(0, vertex_count, stride)
+            ]
+
+    def _load_initial_pose_state(self) -> tuple[bool, Optional[str]]:
+        try:
+            payload = json.loads(self._initial_pose_state_path.read_text(encoding="utf-8"))
+            session_id = payload.get("localization_session_id")
+            return (
+                payload.get("initial_pose_set") is True,
+                session_id.strip() if isinstance(session_id, str) and session_id.strip() else None,
+            )
+        except (OSError, ValueError, TypeError):
+            return False, None
+
+    def _save_initial_pose_state(self) -> None:
+        try:
+            self._initial_pose_state_path.parent.mkdir(parents=True, exist_ok=True)
+            temporary_path = self._initial_pose_state_path.with_suffix(".tmp")
+            temporary_path.write_text(json.dumps({
+                "initial_pose_set": self._initial_pose_set,
+                "localization_session_id": self._initial_pose_session_id,
+            }), encoding="utf-8")
+            temporary_path.replace(self._initial_pose_state_path)
+        except OSError as error:
+            self.get_logger().warning(f"Failed to persist initial pose state: {error}")
+
+    def _on_localization_session(self, msg: String):
+        session_id = msg.data.strip()
+        if not session_id:
+            return
+
+        should_persist = False
+        with self._lock:
+            if self._localization_session_id == session_id:
+                return
+
+            self._localization_session_id = session_id
+            if self._initial_pose_set and self._initial_pose_session_id == "__pending__":
+                self._initial_pose_session_id = session_id
+                should_persist = True
+            elif self._initial_pose_session_id != session_id:
+                # A localization module restart starts a new estimation session.
+                # Never reuse the previous session's initial pose in it.
+                self._initial_pose_set = False
+                self._initial_pose_session_id = None
+                should_persist = True
+
+        if should_persist:
+            self._save_initial_pose_state()
+            self.get_logger().info("Localization session changed; initial pose state was reset.")
+
    def _on_odom(self, msg: Odometry):
        with self._lock:
            self._odom = msg
            self._odom_count += 1
            self._last_odom_wall_time = time.time()

-    def publish_initial_pose(self, x: float, y: float, yaw: float = 0.0, frame_id: str = "map") -> None:
-        # AMCL transforms the initial pose at the message timestamp. On a real
-        # platform the bridge clock can be a few milliseconds ahead of the
-        # latest odom/TF sample, which makes AMCL reject the pose as a future
-        # extrapolation. Anchor it to the latest received odometry instead.
+    def publish_initial_pose(self, x: float, y: float, yaw: float = 0.0, frame_id: str = "map") -> dict:
+        """Provide a map-frame seed without overwriting live ICP odometry."""
        with self._lock:
-            odom_stamp = self._odom.header.stamp if self._odom is not None else None
+            has_3d_localization = self._icp_odom is not None
+
+        # A zero ROS timestamp means "use the latest available transform".
+        # The bridge and platform receive odometry through different DDS
+        # processes, so an odometry-derived timestamp can still be newer than
+        # AMCL's TF buffer.
        message = PoseWithCovarianceStamped()
-        if odom_stamp:
-            # The odom callback can lead the TF buffer by a few milliseconds.
-            # Keep a small margin so AMCL can always transform map -> odom.
-            message.header.stamp = (Time.from_msg(odom_stamp) - Duration(seconds=0.1)).to_msg()
-        else:
-            message.header.stamp = self.get_clock().now().to_msg()
+        message.header.stamp = Time().to_msg()
        message.header.frame_id = frame_id
        message.pose.pose.position.x = x
        message.pose.pose.position.y = y
@@ -284,20 +427,33 @@ class SlamNode(Node):
        message.pose.covariance[0] = 0.25
        message.pose.covariance[7] = 0.25
        message.pose.covariance[35] = 0.0685
-        with self._lock:
-            # Use this only until AMCL starts publishing map -> odom. It is a
-            # short-lived viewer fallback, not a replacement for live TF.
-            self._requested_initial_pose = {
-                "x": float(x),
-                "y": float(y),
-                "z": 0.0,
-                "yaw": float(yaw),
-                "frame_id": frame_id or "map",
-                "t": time.time(),
-                "expires_at": time.monotonic() + 2.0,
-            }
+        if not has_3d_localization:
+            with self._lock:
+                # Use this only until AMCL starts publishing map -> odom. It
+                # is a short-lived viewer fallback, not a replacement for TF.
+                self._requested_initial_pose = {
+                    "x": float(x),
+                    "y": float(y),
+                    "z": 0.0,
+                    "yaw": float(yaw),
+                    "frame_id": frame_id or "map",
+                    "t": time.time(),
+                    "expires_at": time.monotonic() + 2.0,
+                }
+        # In 3D this is consumed by the localization module's local relay,
+        # which can call ICP odometry without crossing the DDS peer boundary.
        self._initial_pose_pub.publish(message)

+        with self._lock:
+            self._initial_pose_set = True
+            self._initial_pose_session_id = self._localization_session_id or "__pending__"
+        self._save_initial_pose_state()
+        return {
+            "mode": "rtabmap-3d" if has_3d_localization else "amcl-2d",
+            "refining": has_3d_localization,
+        }
+
+
    def save_map(self, name: Optional[str] = None) -> dict:
        with self._lock:
            grid = self._map
@@ -369,6 +525,10 @@ class SlamNode(Node):
        database_path = None
        database_source = Path(os.environ.get("RTABMAP_DATABASE_PATH", ""))
        if os.environ.get("RTABMAP_MAP_SAVE_ENABLED", "").lower() == "true" and database_source.is_file():
+            # RTAB-Map keeps the optimized pose graph in memory until its
+            # backup service flushes it. Copying SQLite directly here leaves a
+            # usable PGM but a DB that cannot localize on the saved map.
+            self._backup_rtabmap_database()
            database_snapshot = maps_dir / f"{safe_name}.db"
            source_connection = None
            destination_connection = None
@@ -386,6 +546,11 @@ class SlamNode(Node):
                    destination_connection.close()
                if source_connection is not None:
                    source_connection.close()
+            # A saved map must start unlocalized. RTAB-Map stores the last
+            # localization pose beside the optimized graph; retaining it
+            # makes a later localization session place the vehicle before the
+            # learner supplies the first pose.
+            self._clear_database_last_localization(database_snapshot)
            database_path = str(database_snapshot)

        return {
@@ -399,6 +564,28 @@ class SlamNode(Node):
            "previewPgm": base64.b64encode(preview_pgm).decode("ascii"),
        }

+    def _backup_rtabmap_database(self, timeout_seconds: float = 30.0) -> None:
+        if not self._rtabmap_backup_client.wait_for_service(timeout_sec=5.0):
+            raise RuntimeError("3D SLAM DB 저장 서비스를 찾지 못했습니다. RTAB-Map 모듈이 실행 중인지 확인하세요.")
+
+        future = self._rtabmap_backup_client.call_async(Empty.Request())
+        deadline = time.monotonic() + timeout_seconds
+        while not future.done() and time.monotonic() < deadline:
+            time.sleep(0.05)
+
+        if not future.done():
+            raise RuntimeError("3D SLAM DB 저장 시간이 초과되었습니다.")
+        if future.exception() is not None:
+            raise RuntimeError(f"3D SLAM DB 저장에 실패했습니다: {future.exception()}")
+
+    @staticmethod
+    def _clear_database_last_localization(database_path: Path) -> None:
+        try:
+            with sqlite3.connect(str(database_path)) as connection:
+                connection.execute("UPDATE Admin SET opt_last_localization = NULL")
+        except sqlite3.Error as error:
+            raise RuntimeError(f"저장한 3D SLAM DB를 정리하지 못했습니다: {error}") from error
+
    def _on_scan(self, msg: LaserScan):
        with self._lock:
            self._scan = msg
@@ -423,6 +610,18 @@ class SlamNode(Node):
            self._fallback_point_cloud_count += 1
            self._last_fallback_point_cloud_wall_time = time.time()

+    def _on_color_point_cloud(self, msg: PointCloud2):
+        with self._lock:
+            self._color_point_cloud = msg
+            self._color_point_cloud_count += 1
+            self._last_color_point_cloud_wall_time = time.time()
+
+    def _on_camera_image(self, msg: CompressedImage):
+        with self._lock:
+            self._camera_image = msg
+            self._camera_image_count += 1
+            self._last_camera_image_wall_time = time.time()
+
    def _active_point_cloud(self) -> Optional[PointCloud2]:
        with self._lock:
            primary = self._point_cloud
@@ -478,6 +677,8 @@ class SlamNode(Node):
            amcl_pose_last = self._last_amcl_pose_wall_time
            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

@@ -500,7 +701,24 @@ class SlamNode(Node):
            "amcl_pose": {"name": self.amcl_pose_topic, "available": received_recently(amcl_pose_last)},
            "point_cloud": {
                "name": self.point_cloud_topic,
-                "available": received_recently(point_cloud_last) or received_recently(fallback_point_cloud_last),
+                # RTAB-Map publishes the reconstructed global cloud with
+                # TRANSIENT_LOCAL durability. In localization it may publish
+                # only once after loading the database, so a received map is
+                # still valid even when it is older than the live-topic
+                # freshness window used for sensors.
+                "available": self._saved_map_point_cloud is not None or point_cloud_last is not None,
+            },
+            "sensor_point_cloud": {
+                "name": self.fallback_point_cloud_topic,
+                "available": received_recently(fallback_point_cloud_last),
+            },
+            "color_point_cloud": {
+                "name": self.color_point_cloud_topic,
+                "available": received_recently(color_point_cloud_last),
+            },
+            "camera_image": {
+                "name": self.camera_image_topic,
+                "available": received_recently(camera_image_last),
            },
            "plan": {"name": self.plan_topic, "available": plan_available},
            "cmd_vel": {"name": self.cmd_vel_topic, "available": cmd_vel_available},
@@ -725,8 +943,14 @@ class SlamNode(Node):
            plan = self._plan
            amcl_pose = self._amcl_pose
            requested_initial_pose = dict(self._requested_initial_pose) if self._requested_initial_pose else None
+            initial_pose_set = self._initial_pose_set
+            localization_session_id = self._localization_session_id
+            initial_pose_session_id = self._initial_pose_session_id
            joint_positions = dict(self._joint_positions)
            steering_feedback = self._steering_feedback
+            fallback_point_cloud = self._fallback_point_cloud
+            color_point_cloud = self._color_point_cloud
+            camera_image = self._camera_image
            tf_snapshot = dict(self._tf)
            navigation = dict(self._navigation)
        effective_map = self._effective_map(grid, scan, odom)
@@ -897,15 +1121,14 @@ class SlamNode(Node):
                "yaw": euler["yaw"],
                "t": icp_odom.header.stamp.sec + icp_odom.header.stamp.nanosec * 1e-9,
            }
-            cloud_frame = point_cloud.header.frame_id if point_cloud is not None else ""
-            map_to_icp_odom = tf_snapshot.get((cloud_frame, icp_odom.header.frame_id)) if cloud_frame else None
+            map_to_icp_odom = resolve_2d_transform(tf_snapshot, "map", icp_odom.header.frame_id)
            if map_to_icp_odom is not None:
-                # RTAB-Map publishes /cloud_map in map, while ICP odometry is
-                # expressed in icp_odom. Render both in the cloud frame.
+                # ICP is local odometry. RTAB-Map localization publishes the
+                # map -> icp_odom correction needed to render it on the saved map.
                pose_3d = {
                    **pose_3d,
                    **compose_2d(map_to_icp_odom, pose_3d),
-                    "frame_id": cloud_frame,
+                    "frame_id": "map",
                    "source_frame_id": icp_odom.header.frame_id,
                }

@@ -943,12 +1166,38 @@ class SlamNode(Node):
                "t": point_cloud.header.stamp.sec + point_cloud.header.stamp.nanosec * 1e-9,
            }

+        sensor_point_cloud_payload = None
+        if fallback_point_cloud is not None:
+            sensor_point_cloud_payload = {
+                "frame_id": fallback_point_cloud.header.frame_id,
+                "count": fallback_point_cloud.width * fallback_point_cloud.height,
+                "t": fallback_point_cloud.header.stamp.sec + fallback_point_cloud.header.stamp.nanosec * 1e-9,
+            }
+
+        color_point_cloud_payload = None
+        if color_point_cloud is not None:
+            color_point_cloud_payload = {
+                "frame_id": color_point_cloud.header.frame_id,
+                "count": color_point_cloud.width * color_point_cloud.height,
+                "t": color_point_cloud.header.stamp.sec + color_point_cloud.header.stamp.nanosec * 1e-9,
+            }
+
+        camera_image_payload = None
+        if camera_image is not None:
+            camera_image_payload = {
+                "frame_id": camera_image.header.frame_id,
+                "t": camera_image.header.stamp.sec + camera_image.header.stamp.nanosec * 1e-9,
+            }
+
        return {
            "ok": True,
            "topics": {
                "odom": self.odom_topic,
                "scan": self.scan_topic,
                "point_cloud": self.point_cloud_topic,
+                "sensor_point_cloud": self.fallback_point_cloud_topic,
+                "color_point_cloud": self.color_point_cloud_topic,
+                "camera_image": self.camera_image_topic,
                "icp_odom": self.icp_odom_topic,
                "map": self.map_topic,
                "cmd_vel": self.cmd_vel_topic,
@@ -962,11 +1211,17 @@ class SlamNode(Node):
            "vehicle": vehicle,
            "scan": scan_payload,
            "point_cloud": point_cloud_payload,
+            "sensor_point_cloud": sensor_point_cloud_payload,
+            "color_point_cloud": color_point_cloud_payload,
+            "camera_image": camera_image_payload,
            "map": map_payload,
            "tf": {
                "map_to_odom": resolve_2d_transform(tf_snapshot, "map", "odom"),
            },
            "navigation": navigation,
+            "initial_pose_set": initial_pose_set,
+            "localization_session_id": localization_session_id,
+            "initial_pose_session_id": initial_pose_session_id,
            "topic_status": self.topic_status(),
            "plan": {
                "frame_id": plan.header.frame_id,
@@ -983,7 +1238,57 @@ class SlamNode(Node):
        }

    def point_cloud_payload(self, limit: int = _POINT_CLOUD_LIMIT) -> Optional[dict]:
-        cloud = self._active_point_cloud()
+        with self._lock:
+            saved_map_point_cloud = self._saved_map_point_cloud
+            # A saved RTAB-Map map may be published in the local ICP frame
+            # during navigation, while a live SLAM map is normally in `map`.
+            # Preserve the source frame here so the viewer can place either
+            # cloud in the common map scene.
+            cloud = self._point_cloud or self._fallback_point_cloud
+            tf_snapshot = dict(self._tf)
+        if saved_map_point_cloud is not None:
+            return saved_map_point_cloud
+        payload = self._serialize_point_cloud(cloud, limit)
+        if payload is None:
+            return None
+        payload["transform"] = resolve_2d_transform(tf_snapshot, "map", payload["frame_id"])
+        return payload
+
+    def sensor_point_cloud_payload(self, limit: int = 10_000) -> Optional[dict]:
+        with self._lock:
+            cloud = self._fallback_point_cloud
+            tf_snapshot = dict(self._tf)
+            received_at = self._last_fallback_point_cloud_wall_time
+        payload = self._serialize_point_cloud(cloud, limit)
+        if payload is None:
+            return None
+        payload["transform"] = resolve_2d_transform(tf_snapshot, "map", payload["frame_id"])
+        payload["received_at"] = received_at
+        return payload
+
+    def color_point_cloud_payload(self, limit: int = 10_000) -> Optional[dict]:
+        with self._lock:
+            cloud = self._color_point_cloud
+            tf_snapshot = dict(self._tf)
+            received_at = self._last_color_point_cloud_wall_time
+        payload = self._serialize_point_cloud(cloud, limit, include_colors=True)
+        if payload is None:
+            return None
+        payload["transform"] = resolve_2d_transform(tf_snapshot, "map", payload["frame_id"])
+        payload["received_at"] = received_at
+        return payload
+
+    def camera_frame_jpeg(self) -> Optional[bytes]:
+        with self._lock:
+            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():
+            return None
+        return bytes(image.data)
+
+    def _serialize_point_cloud(self, cloud: Optional[PointCloud2], limit: int, include_colors: bool = False) -> Optional[dict]:
        if cloud is None or cloud.point_step <= 0 or not cloud.data:
            return None

@@ -998,6 +1303,9 @@ class SlamNode(Node):
        prefix = ">" if cloud.is_bigendian else "<"
        data = cloud.data
        points = []
+        colors = []
+        color_offset = offsets.get("rgb", offsets.get("rgba"))
+        color_field = next((field for field in cloud.fields if field.name in ("rgb", "rgba")), None)
        for index in range(0, total, stride):
            offset = index * cloud.point_step
            if offset + cloud.point_step > len(data):
@@ -1010,12 +1318,27 @@ class SlamNode(Node):
                break
            if math.isfinite(x) and math.isfinite(y) and math.isfinite(z):
                points.append([x, y, z])
+                if include_colors:
+                    rgb = 0xFFFFFF
+                    if color_offset is not None and color_field is not None:
+                        try:
+                            if color_field.datatype == 6:  # PointField.UINT32
+                                rgb = struct.unpack_from(f"{prefix}I", data, offset + color_offset)[0]
+                            elif color_field.datatype == 7:  # PointField.FLOAT32 packed RGB
+                                packed = struct.unpack_from(f"{prefix}f", data, offset + color_offset)[0]
+                                rgb = struct.unpack(f"{prefix}I", struct.pack(f"{prefix}f", packed))[0]
+                        except struct.error:
+                            pass
+                    colors.append([(rgb >> 16) & 0xFF, (rgb >> 8) & 0xFF, rgb & 0xFF])

-        return {
+        payload = {
            "frame_id": cloud.header.frame_id,
            "stamp": cloud.header.stamp.sec + cloud.header.stamp.nanosec * 1e-9,
            "points": points,
        }
+        if include_colors:
+            payload["colors"] = colors
+        return payload

    def map_binary(self) -> Optional[bytes]:
        with self._lock:
@@ -1049,7 +1372,7 @@ class SlamNode(Node):
    def topic_debug(self) -> dict:
        topics = {}
        for name, types in self.get_topic_names_and_types():
-            if name in (self.odom_topic, self.icp_odom_topic, self.scan_topic, self.point_cloud_topic, self.fallback_point_cloud_topic, self.map_topic, self.cmd_vel_topic):
+            if name in (self.odom_topic, self.icp_odom_topic, self.scan_topic, self.point_cloud_topic, self.fallback_point_cloud_topic, self.color_point_cloud_topic, self.camera_image_topic, self.map_topic, self.cmd_vel_topic):
                topics[name] = types

        return {
@@ -1058,6 +1381,8 @@ class SlamNode(Node):
                "scan": self.scan_topic,
                "point_cloud": self.point_cloud_topic,
                "fallback_point_cloud": self.fallback_point_cloud_topic,
+                "color_point_cloud": self.color_point_cloud_topic,
+                "camera_image": self.camera_image_topic,
                "icp_odom": self.icp_odom_topic,
                "map": self.map_topic,
                "cmd_vel": self.cmd_vel_topic,
@@ -1067,6 +1392,8 @@ class SlamNode(Node):
            "scan_publishers": self.count_publishers(self.scan_topic),
            "point_cloud_publishers": self.count_publishers(self.point_cloud_topic),
            "fallback_point_cloud_publishers": self.count_publishers(self.fallback_point_cloud_topic),
+            "color_point_cloud_publishers": self.count_publishers(self.color_point_cloud_topic),
+            "camera_image_publishers": self.count_publishers(self.camera_image_topic),
            "icp_odom_publishers": self.count_publishers(self.icp_odom_topic),
            "odom_publishers": self.count_publishers(self.odom_topic),
            "map_publishers": self.count_publishers(self.map_topic),
@@ -1075,6 +1402,8 @@ class SlamNode(Node):
                "scan_count": self._scan_count,
                "point_cloud_count": self._point_cloud_count,
                "fallback_point_cloud_count": self._fallback_point_cloud_count,
+                "color_point_cloud_count": self._color_point_cloud_count,
+                "camera_image_count": self._camera_image_count,
                "icp_odom_count": self._icp_odom_count,
                "odom_count": self._odom_count,
                "map_count": self._map_count,
@@ -1082,6 +1411,8 @@ class SlamNode(Node):
                "last_scan_wall_time": self._last_scan_wall_time,
                "last_point_cloud_wall_time": self._last_point_cloud_wall_time,
                "last_fallback_point_cloud_wall_time": self._last_fallback_point_cloud_wall_time,
+                "last_color_point_cloud_wall_time": self._last_color_point_cloud_wall_time,
+                "last_camera_image_wall_time": self._last_camera_image_wall_time,
                "last_icp_odom_wall_time": self._last_icp_odom_wall_time,
                "last_odom_wall_time": self._last_odom_wall_time,
                "last_map_wall_time": self._last_map_wall_time,
@@ -1155,6 +1486,36 @@ def point_cloud():
    return payload


+@app.get("/sensor-point-cloud")
+def sensor_point_cloud():
+    if not slam_node:
+        raise HTTPException(status_code=503, detail="ROS node is not ready")
+    payload = slam_node.sensor_point_cloud_payload()
+    if payload is None:
+        raise HTTPException(status_code=404, detail="센서 포인트클라우드를 아직 수신하지 못했습니다.")
+    return payload
+
+
+@app.get("/color-point-cloud")
+def color_point_cloud():
+    if not slam_node:
+        raise HTTPException(status_code=503, detail="ROS node is not ready")
+    payload = slam_node.color_point_cloud_payload()
+    if payload is None:
+        raise HTTPException(status_code=404, detail="컬러 포인트클라우드를 아직 수신하지 못했습니다.")
+    return payload
+
+
+@app.get("/camera-frame")
+def camera_frame():
+    if not slam_node:
+        raise HTTPException(status_code=503, detail="ROS node is not ready")
+    frame = slam_node.camera_frame_jpeg()
+    if frame is None:
+        raise HTTPException(status_code=404, detail="카메라 프레임을 아직 수신하지 못했습니다.")
+    return Response(content=frame, media_type="image/jpeg", headers={"Cache-Control": "no-store"})
+
+
@app.get("/world-layout")
 def world_layout():
    path = Path(os.environ.get("WORLD_LAYOUT_PATH", "/workspace/world-layout.json"))
@@ -1172,7 +1533,7 @@ def save_map(payload: SaveMapPayload):
        raise HTTPException(status_code=503, detail="ROS node is not ready")
    try:
        return {"ok": True, "map": slam_node.save_map(payload.name)}
-    except ValueError as error:
+    except (ValueError, RuntimeError) as error:
        raise HTTPException(status_code=409, detail=str(error)) from error


@@ -1239,8 +1600,11 @@ def initial_pose(payload: InitialPosePayload):
        raise HTTPException(status_code=503, detail="ROS node is not ready")
    if slam_node.read_only:
        raise HTTPException(status_code=403, detail="읽기 전용 3D SLAM 화면에서는 초기 위치를 설정할 수 없습니다.")
-    slam_node.publish_initial_pose(payload.x, payload.y, payload.yaw, payload.frame_id)
-    return {"ok": True}
+    try:
+        result = slam_node.publish_initial_pose(payload.x, payload.y, payload.yaw, payload.frame_id)
+    except RuntimeError as error:
+        raise HTTPException(status_code=409, detail=str(error)) from error
+    return {"ok": True, **result}


@app.websocket("/ws/state")