diff --git a/package.json b/package.json index 28b88f4..edb8617 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "lms", - "version": "1.1.7", + "version": "1.1.8", "description": "Autonomy Studio LMS Electron app", "main": "./out/main/index.js", "author": { diff --git a/resources/slam-ros-bridge/Dockerfile b/resources/slam-ros-bridge/Dockerfile index cc50371..7f27bd3 100644 --- a/resources/slam-ros-bridge/Dockerfile +++ b/resources/slam-ros-bridge/Dockerfile @@ -6,7 +6,7 @@ COPY requirements.txt /app/requirements.txt COPY bridge.py /app/bridge.py RUN apt-get update && \ - apt-get install -y --no-install-recommends python3-pip ros-humble-nav2-msgs && \ + apt-get install -y --no-install-recommends python3-pip ros-humble-nav2-msgs ros-humble-rtabmap-msgs ros-humble-rtabmap-util && \ pip3 install --no-cache-dir --retries 5 --timeout 180 -r /app/requirements.txt && \ rm -rf /var/lib/apt/lists/* diff --git a/resources/slam-ros-bridge/__pycache__/bridge.cpython-310.pyc b/resources/slam-ros-bridge/__pycache__/bridge.cpython-310.pyc index 21bc8ed..01b985a 100644 Binary files a/resources/slam-ros-bridge/__pycache__/bridge.cpython-310.pyc and b/resources/slam-ros-bridge/__pycache__/bridge.cpython-310.pyc differ diff --git a/resources/slam-ros-bridge/bridge.py b/resources/slam-ros-bridge/bridge.py index 05cdb2e..6d4a329 100644 --- a/resources/slam-ros-bridge/bridge.py +++ b/resources/slam-ros-bridge/bridge.py @@ -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") diff --git a/src/main/index.ts b/src/main/index.ts index ee67962..dfb7cd1 100644 --- a/src/main/index.ts +++ b/src/main/index.ts @@ -352,7 +352,11 @@ async function syncWorkspaceSavedMapArtifacts( } await Promise.all(artifacts.map(async (artifact) => { const normalized = validateSavedMapDownload(artifact) - await downloadArtifact(normalized.url, resolve(mapsDirectory, normalized.fileName)) + const targetPath = resolve(mapsDirectory, normalized.fileName) + // SLAM containers can leave an artifact owned by root. Removing it first + // lets the download recreate the file with the desktop user's ownership. + await rm(targetPath, { force: true }) + await downloadArtifact(normalized.url, targetPath) })) return { ok: true } } @@ -1335,6 +1339,45 @@ async function readRemoteSavedMapArtifacts( })) } +async function copyRemoteActiveRtabmapDatabaseToLocal( + targetInput: RemoteWorkspaceTarget, + localWorkspaceRoot: string | null | undefined, + destinationPath: string, +): Promise<{ databasePath: string }> { + const target = normalizeRemoteTarget(targetInput) + const destinationFileName = savedMapArtifactFileName('database', destinationPath) + const sourcePath = `${target.workspaceRoot}/maps/rtabmap-active.db` + const snapshotScript = [ + 'import sqlite3, sys', + 'source, destination = sys.argv[1:3]', + 'source_db = sqlite3.connect(f"file:{source}?mode=ro", uri=True)', + 'destination_db = sqlite3.connect(destination)', + 'source_db.backup(destination_db)', + 'destination_db.close()', + 'source_db.close()', + ].join('; ') + const remoteCommand = [ + 'set -e', + `SOURCE=${quoteShellSingle(sourcePath)}`, + 'test -f "$SOURCE" || { echo "3D SLAM DB를 찾을 수 없습니다: $SOURCE" >&2; exit 1; }', + 'SNAPSHOT=$(mktemp)', + `python3 -c ${quoteShellSingle(snapshotScript)} "$SOURCE" "$SNAPSHOT"`, + 'base64 -w0 "$SNAPSHOT"', + 'rm -f "$SNAPSHOT"', + ].join('\n') + const base64 = (await runSshOutput(target, remoteCommand, 120_000)).trim() + if (!base64) throw new Error('3D SLAM DB 스냅샷을 읽지 못했습니다.') + + await syncWorkspaceSavedMapArtifacts(localWorkspaceRoot, []) + const root = expandUserPath(localWorkspaceRoot) + if (!root) throw new Error('로컬 워크스페이스 경로가 설정되어 있지 않습니다.') + const mapsDirectory = resolve(root, 'maps') + const destination = resolve(mapsDirectory, destinationFileName) + if (!destination.startsWith(`${mapsDirectory}/`)) throw new Error('저장된 3D 맵 경로가 올바르지 않습니다.') + await writeFile(destination, Buffer.from(base64, 'base64')) + return { databasePath: `/workspace/maps/${destinationFileName}` } +} + async function syncRemoteSavedMapArtifacts( targetInput: RemoteWorkspaceTarget, artifacts: SavedMapArtifactDownload[], @@ -1347,7 +1390,13 @@ async function syncRemoteSavedMapArtifacts( `ROOT=${quoteShellSingle(target.workspaceRoot)}`, `MAPS_DIRECTORY=${quoteShellSingle(mapsDirectory)}`, 'if [ ! -d "$MAPS_DIRECTORY" ] || [ ! -w "$MAPS_DIRECTORY" ]; then', - ' IMAGE="$(docker image inspect future-nav2:humble-jetson_orin --format {{.Id}} >/dev/null 2>&1 && printf future-nav2:humble-jetson_orin || docker image inspect future-nav2:humble --format {{.Id}} >/dev/null 2>&1 && printf future-nav2:humble || true)"', + ' if docker image inspect future-nav2:humble-jetson_orin >/dev/null 2>&1; then', + ' IMAGE="future-nav2:humble-jetson_orin"', + ' elif docker image inspect future-nav2:humble >/dev/null 2>&1; then', + ' IMAGE="future-nav2:humble"', + ' else', + ' IMAGE=""', + ' fi', ' if [ -z "$IMAGE" ]; then echo "저장된 맵 디렉터리에 쓸 권한이 없고 권한 복구용 NAV2 이미지를 찾을 수 없습니다." >&2; exit 1; fi', ' docker run --rm --user 0:0 -v "$ROOT:/workspace" --entrypoint /bin/sh "$IMAGE" -lc "mkdir -p /workspace/maps && chown -R $(id -u):$(id -g) /workspace/maps"', 'fi', @@ -1460,7 +1509,7 @@ async function startRemoteVisualizationBridge( `-e ROBOT_STACK=${quoteShellSingle(stack === 'autoware' ? 'autoware' : 'nav2')}`, '-e ODOM_TOPIC=/odom -e SCAN_TOPIC=/scan -e MAP_TOPIC=/map -e CMD_VEL_TOPIC=/cmd_vel -e NAVIGATE_TO_POSE_ACTION=/navigate_to_pose', ...(stack === 'slam3d' - ? ['-e POINT_CLOUD_TOPIC=/cloud_map -e FALLBACK_POINT_CLOUD_TOPIC=/lidar_filtered -e ICP_ODOM_TOPIC=/icp_odom -e POINT_CLOUD_LIMIT=50000 -e READ_ONLY=true -e RTABMAP_MAP_SAVE_ENABLED=true'] + ? ['-e POINT_CLOUD_TOPIC=/cloud_map -e FALLBACK_POINT_CLOUD_TOPIC=/lidar_filtered -e ICP_ODOM_TOPIC=/icp_odom -e POINT_CLOUD_LIMIT=50000 -e RTABMAP_MAP_SAVE_ENABLED=true'] : []), bridgeEnvArgs, '-e WORLD_LAYOUT_PATH=/workspace/world-layout.json -e SAVED_MAPS_PATH=/workspace/maps -e RTABMAP_DATABASE_PATH=/workspace/maps/rtabmap-active.db', @@ -2450,7 +2499,7 @@ function bridgeContainerCommand(stack: VisualizationStack): string { return [ 'source /opt/ros/humble/setup.bash', setupWorkspace, - `python3 -c "import fastapi, uvicorn, websockets" || python3 -m pip install --no-cache-dir -r ${requirementPath}`, + `(python3 -c "import fastapi, uvicorn, websockets" || python3 -m pip install --no-cache-dir -r ${requirementPath})`, `python3 ${bridgePath}` ].filter(Boolean).join(' && ') } @@ -2739,6 +2788,7 @@ async function startVisualizationBridge( rosDomainId?: string | null, environmentProfile?: EnvironmentProfileConfig | null, rosNetwork?: RosNetworkConfig | null, + savedMapDatabasePath?: string | null, ): Promise { const stack = normalizeVisualizationStack(environmentProfile?.type?.toLowerCase() ?? stackInput) const resolvedProfile = withRuntimeProfileDefaults(environmentProfile, stack) @@ -2840,6 +2890,10 @@ async function startVisualizationBridge( '-e', 'RTABMAP_DATABASE_PATH=/workspace/maps/rtabmap-active.db', ) if (stack === 'slam3d') args.push('-e', 'RTABMAP_MAP_SAVE_ENABLED=true') + const savedMapFileName = savedMapDatabasePath ? safeSavedMapFileName(basename(savedMapDatabasePath)) : null + if (savedMapFileName?.endsWith('.db') && existsSync(resolve(layoutWorkspaceRoot, 'maps', savedMapFileName))) { + args.push('-e', `SAVED_MAP_DATABASE_PATH=/workspace/maps/${savedMapFileName}`) + } } const fastDdsConfigPath = await prepareLocalRosNetworkConfig(rosNetwork, requestedRosDomainId) if (fastDdsConfigPath && existsSync(fastDdsConfigPath)) { @@ -3101,8 +3155,15 @@ async function installRemoteWorkspaceModule( ' expected = (item.get("checksumSha256") or "").strip().lower()', ' if expected:', ' import hashlib', - ' with open(path, "rb") as fp: actual = hashlib.sha256(fp.read()).hexdigest().lower()', - ' if actual != expected: raise SystemExit(f"artifact checksum mismatch: {name}")', + ' def checksum(file_path):', + ' with open(file_path, "rb") as fp: return hashlib.sha256(fp.read()).hexdigest().lower()', + ' actual = checksum(path)', + ' if actual != expected:', + ' # Stable artifact URLs may have a newer object than the local cache.', + ' os.unlink(path)', + ' subprocess.run(["curl", "-fL", url, "-o", path], check=True)', + ' actual = checksum(path)', + ' if actual != expected: raise SystemExit(f"artifact checksum mismatch: {name}")', ' entries = subprocess.check_output(["tar", *tar_args(path, "list"), path], text=True)', ' for entry in entries.splitlines():', ' entry = entry.strip()', @@ -3512,13 +3573,13 @@ function buildNav2DockerRunScript(): string { 'esac', 'fi', '', - 'if [[ "$MODULE_FOLDER" == *lms-nav2-gazebo-slam3d* ]]; then', + 'if [[ "$MODULE_FOLDER" == *lms-nav2-gazebo-slam3d* || "$MODULE_FOLDER" == *lms-nav2-gazebo-localization* ]]; then', ' mkdir -p "$MODULE_ROOT/maps"', ' has_database_path=false', ' for arg in "$@"; do', ' if [[ "$arg" == database_path:=* ]]; then has_database_path=true; break; fi', ' done', - ' if [ "$has_database_path" = false ]; then', + ' if [[ "$MODULE_FOLDER" == *lms-nav2-gazebo-slam3d* && "$has_database_path" = false ]] || [[ "$MODULE_FOLDER" == *lms-nav2-gazebo-localization* && "$has_database_path" = false ]]; then', ' set -- "database_path:=/modules/maps/rtabmap-active.db" "$@"', ' fi', 'fi', @@ -3621,11 +3682,19 @@ function buildNav2DockerBuildScript(): string { ' set -e', ' source /opt/ros/humble/setup.bash', ' mapfile -t paths < <(colcon list --base-paths src --paths-only)', - " rosdep install --from-paths \"${paths[@]}\" --ignore-src \\", + ' ROSDEP_SKIP_ARGS=()', + ' if ! apt-cache show ros-humble-gazebo-ros >/dev/null 2>&1; then', + ' ROSDEP_SKIP_ARGS=(--skip-keys gazebo_ros)', + ' fi', + " rosdep install --from-paths \"${paths[@]}\" --ignore-src \"${ROSDEP_SKIP_ARGS[@]}\" \\", " --dependency-types buildtool --dependency-types build \\", ' --dependency-types build_export --dependency-types exec -r -y', + ' BUILD_TARGETS=("$2")', + ' if [[ "$1" == *lms-nav2-gazebo-slam* && "$1" != *lms-nav2-gazebo-slam3d* ]]; then', + ' BUILD_TARGETS=(slam_pkg slam2d_pkg slam3d_pkg)', + ' fi', " colcon --log-base log build --base-paths src --build-base build --install-base install \\", - ' --executor sequential --cmake-args -DBUILD_TESTING=OFF --packages-up-to "$2"', + ' --executor sequential --cmake-args -DBUILD_TESTING=OFF --packages-up-to "${BUILD_TARGETS[@]}"', "' bash \"$MODULE_FOLDER\" \"$TARGET\"", '' ].join('\n') @@ -3785,6 +3854,22 @@ async function findRunningNav2SimulatorContainer(): Promise { } async function findNav2SimulatorRobotModel(containerName: string): Promise { + // Query Gazebo directly first. The ROS GetModelList service can be slow or + // unavailable while DDS participants are still discovering each other, even + // though the simulator itself is already able to accept a pose update. + const knownModels = ['mkmini_gazebo', 'turtlebot3_burger_3d_lidar', 'turtlebot3_burger'] + for (const modelName of knownModels) { + const pose = await runCommandOutput( + 'docker', + ['exec', containerName, 'gz', 'model', '-m', modelName, '-p'], + undefined, + undefined, + 5_000, + ).catch(() => '') + const values = pose.trim().split(/\s+/).map(Number) + if (values.length === 6 && values.every(Number.isFinite)) return modelName + } + const output = await runCommandOutput( 'docker', [ @@ -3798,7 +3883,7 @@ async function findNav2SimulatorRobotModel(containerName: string): Promise match[1]) const robotModel = modelNames.find((modelName) => ( - /turtlebot|burger|robot/i.test(modelName) && !/world/i.test(modelName) + /turtlebot|burger|mkmini|mk-mini|robot/i.test(modelName) && !/world/i.test(modelName) )) if (!robotModel) throw new Error('Gazebo 로봇 모델을 찾을 수 없습니다.') return robotModel @@ -3815,6 +3900,10 @@ async function setNav2SimulatorPose( const containerName = await findRunningNav2SimulatorContainer() if (!containerName) throw new Error('실행 중인 NAV2 Simulator를 찾을 수 없습니다.') const robotModel = await findNav2SimulatorRobotModel(containerName) + // MK-mini's model origin is higher than TurtleBot's. Teleporting it at the + // TurtleBot ground height puts its chassis and wheels into the floor, which + // makes Gazebo eject or tip the vehicle while resolving the collision. + const spawnHeight = /^mkmini/i.test(robotModel) ? '0.16' : '0.01' await runCommand( 'docker', @@ -3823,7 +3912,7 @@ async function setNav2SimulatorPose( 'gz', 'model', '-m', robotModel, '-x', String(x), '-y', String(y), - '-z', '0.01', + '-z', spawnHeight, '-R', '0', '-P', '0', '-Y', String(yaw), @@ -5141,6 +5230,9 @@ function registerLocalWorkspaceHandlers(): void { ipcMain.handle('remote-workspace:maps:read-artifacts', (_event, target: RemoteWorkspaceTarget, artifacts: SavedMapArtifactPath[]) => ( readRemoteSavedMapArtifacts(target, artifacts) )) + ipcMain.handle('remote-workspace:maps:copy-active-rtabmap-database', (_event, target: RemoteWorkspaceTarget, localWorkspaceRoot: string | null | undefined, destinationPath: string) => ( + copyRemoteActiveRtabmapDatabaseToLocal(target, localWorkspaceRoot, destinationPath) + )) ipcMain.handle('remote-workspace:maps:sync', (_event, target: RemoteWorkspaceTarget, artifacts: SavedMapArtifactDownload[]) => ( syncRemoteSavedMapArtifacts(target, artifacts) )) @@ -5204,7 +5296,7 @@ function registerLocalWorkspaceHandlers(): void { return { ok: true } }) ipcMain.handle('local-workspace:visualization:status', () => readVisualizationBridgeStatus()) - ipcMain.handle('local-workspace:visualization:start', (_event, stack?: string | null, workspaceRoot?: string | null, rosDomainId?: string | null, environmentProfile?: EnvironmentProfileConfig | null, rosNetwork?: RosNetworkConfig | null) => startVisualizationBridge(stack, workspaceRoot, rosDomainId, environmentProfile, rosNetwork)) + ipcMain.handle('local-workspace:visualization:start', (_event, stack?: string | null, workspaceRoot?: string | null, rosDomainId?: string | null, environmentProfile?: EnvironmentProfileConfig | null, rosNetwork?: RosNetworkConfig | null, savedMapDatabasePath?: string | null) => startVisualizationBridge(stack, workspaceRoot, rosDomainId, environmentProfile, rosNetwork, savedMapDatabasePath)) ipcMain.handle('local-workspace:visualization:stop', (_event, environmentProfile?: EnvironmentProfileConfig | null) => stopVisualizationBridge(environmentProfile)) ipcMain.handle('local-workspace:git:status', (_event, moduleName: string, workspaceRoot?: string | null, folderName?: string | null, selectedFilePath?: string | null) => readWorkspaceGitStatus(moduleName, workspaceRoot, folderName, selectedFilePath)) ipcMain.handle('local-workspace:git:submit', (_event, moduleName: string, workspaceRoot?: string | null, folderName?: string | null, message?: string | null, authorName?: string | null, authorEmail?: string | null, submissionId?: string | null, repositoryUrl?: string | null) => submitWorkspaceGitChanges(moduleName, workspaceRoot, folderName, message, authorName, authorEmail, submissionId, repositoryUrl)) diff --git a/src/preload/index.ts b/src/preload/index.ts index a5f4017..772cea2 100644 --- a/src/preload/index.ts +++ b/src/preload/index.ts @@ -220,7 +220,7 @@ contextBridge.exposeInMainWorld('localWorkspace', { }, visualization: { status: () => ipcRenderer.invoke('local-workspace:visualization:status'), - start: (stack?: 'autoware' | 'nav2' | 'slam', workspaceRoot?: string, rosDomainId?: string, environmentProfile?: unknown, rosNetwork?: RosNetworkConfig | null) => ipcRenderer.invoke('local-workspace:visualization:start', stack, workspaceRoot, rosDomainId, environmentProfile, rosNetwork), + start: (stack?: 'autoware' | 'nav2' | 'slam', workspaceRoot?: string, rosDomainId?: string, environmentProfile?: unknown, rosNetwork?: RosNetworkConfig | null, savedMapDatabasePath?: string | null) => ipcRenderer.invoke('local-workspace:visualization:start', stack, workspaceRoot, rosDomainId, environmentProfile, rosNetwork, savedMapDatabasePath), stop: (environmentProfile?: unknown) => ipcRenderer.invoke('local-workspace:visualization:stop', environmentProfile) }, git: { @@ -266,6 +266,7 @@ contextBridge.exposeInMainWorld('remoteWorkspace', { }, maps: { readArtifacts: (target: RemoteWorkspaceTarget, artifacts: SavedMapArtifactPath[]) => ipcRenderer.invoke('remote-workspace:maps:read-artifacts', target, artifacts), + copyActiveRtabmapDatabaseToLocal: (target: RemoteWorkspaceTarget, localWorkspaceRoot: string | undefined, destinationPath: string) => ipcRenderer.invoke('remote-workspace:maps:copy-active-rtabmap-database', target, localWorkspaceRoot, destinationPath), sync: (target: RemoteWorkspaceTarget, artifacts: SavedMapArtifactDownload[]) => ipcRenderer.invoke('remote-workspace:maps:sync', target, artifacts) }, modules: { diff --git a/src/renderer/public/slam-viewer/assets/mk-mini.glb b/src/renderer/public/slam-viewer/assets/mk-mini.glb index c14e23d..24787ac 100644 Binary files a/src/renderer/public/slam-viewer/assets/mk-mini.glb and b/src/renderer/public/slam-viewer/assets/mk-mini.glb differ diff --git a/src/renderer/public/slam-viewer/index.html b/src/renderer/public/slam-viewer/index.html index fd81429..43ddb73 100644 --- a/src/renderer/public/slam-viewer/index.html +++ b/src/renderer/public/slam-viewer/index.html @@ -43,9 +43,51 @@ border-color:rgba(255,255,255,.34); color:#fff; } - #miniMap.hidden + #miniMapToggle { - bottom:16px; + #miniMap.hidden + #miniMapToggle { + bottom:16px; } + #cameraPreviewWrap { + position:absolute; + right:16px; + bottom:256px; + z-index:10; + display:none; + width:260px; + border:1px solid rgba(0,0,0,.45); + background:#111520; + box-shadow:0 10px 26px rgba(0,0,0,.16); + } + #cameraPreviewWrap.is-visible { display:block; } + #cameraPreviewWrap.is-collapsed { background:transparent; border-color:transparent; box-shadow:none; } + #cameraPreview { + display:block; + width:260px; + aspect-ratio:16 / 9; + object-fit:cover; + background:#111520; + } + #cameraPreviewWrap.is-collapsed #cameraPreview { display:none; } + #cameraPreviewToggle { + position:absolute; + right:0; + top:-28px; + display:grid; + width:30px; + height:28px; + place-items:center; + padding:0; + border:1px solid rgba(255,255,255,.22); + background:rgba(17,24,39,.42); + color:rgba(255,255,255,.9); + font:11px sans-serif; + font-weight:800; + cursor:pointer; + backdrop-filter:blur(8px); + box-shadow:0 10px 28px rgba(0,0,0,.16); + } + #cameraPreviewToggle:hover { color:#fff; background:rgba(255,255,255,.06); } + #cameraPreviewChevron { font-size:13px; line-height:1; } + #cameraPreviewWrap.is-collapsed #cameraPreviewChevron { transform:rotate(180deg); } #topRightControls { position:absolute; right:16px; @@ -150,6 +192,23 @@ opacity:0; pointer-events:none; } + #mapSaveOverlay { + position:fixed; + inset:0; + z-index:40; + display:flex; + align-items:center; + justify-content:center; + background:rgba(5,8,16,.46); + backdrop-filter:blur(2px); + opacity:0; + pointer-events:none; + transition:opacity .16s ease; + } + #mapSaveOverlay.is-visible { + opacity:1; + pointer-events:auto; + } .response-wait-dialog { display:flex; align-items:center; @@ -184,7 +243,6 @@ #poseToggle, #goalToggle, #obstacleToggle, - #initToggle, #cameraPresetSelect { min-height:32px; border:1px solid rgba(255,255,255,.22); @@ -203,8 +261,7 @@ #splatToggle, #poseToggle, #goalToggle, - #obstacleToggle, - #initToggle { + #obstacleToggle { padding:0 11px; } #downloadMap { @@ -216,7 +273,21 @@ display:inline-flex; align-items:center; justify-content:center; + position:absolute; + top:16px; + left:16px; + z-index:10; + width:14px; + min-width:14px; + min-height:14px; + height:14px; + padding:0; + border-color:rgba(203,213,225,.3); + border-radius:2px; + background:rgba(148,163,184,.3); + box-shadow:none; } + #splatToggle:hover { background:rgba(148,163,184,.5); border-color:rgba(226,232,240,.45); } #cameraPresetSelect { min-width:86px; padding:0 24px 0 9px; @@ -243,10 +314,14 @@ opacity:.45; cursor:not-allowed; } - #goalToggle:disabled { - opacity:.42; - cursor:not-allowed; - } + #goalToggle:disabled { + opacity:.42; + cursor:not-allowed; + } + #poseToggle:disabled { + opacity:.32; + cursor:not-allowed; + } #pathToggle.active { background:rgba(37,99,235,.62); border-color:rgba(147,197,253,.55); @@ -258,9 +333,8 @@ color:#fff; } #splatToggle.active { - background:rgba(109,40,217,.62); - border-color:rgba(196,181,253,.58); - color:#fff; + background:rgba(148,163,184,.62); + border-color:rgba(226,232,240,.58); } #goalToggle.active { background:rgba(22,163,74,.62); @@ -306,15 +380,16 @@ background:rgba(255,255,255,.1); color:#fff; } - .editor-mode #topRightControls, + .editor-mode #topRightControls, .editor-mode #miniMap, .editor-mode #miniMapToggle, + .editor-mode #cameraPreviewWrap, .editor-mode #keyGuide, .editor-mode #rosTopicStatusWrap { display:none; } #splatToggle { display:none; } - .nav2-slam #goalToggle, .nav2-slam #poseToggle, .nav2-slam #sensorToggle, .nav2-slam #initToggle { display:none; } + .nav2-slam #goalToggle, .nav2-slam #poseToggle, .nav2-slam #sensorToggle { display:none; } .nav2-navigation #downloadWrap { display:none; } .nav2-navigation #topLeftControls { display:block; } .nav2-navigation #keyGuide { display:none; } @@ -328,16 +403,15 @@
+
- -