diff --git a/resources/slam-ros-bridge/Dockerfile b/resources/slam-ros-bridge/Dockerfile index 077079b..cc50371 100644 --- a/resources/slam-ros-bridge/Dockerfile +++ b/resources/slam-ros-bridge/Dockerfile @@ -7,7 +7,7 @@ COPY bridge.py /app/bridge.py RUN apt-get update && \ apt-get install -y --no-install-recommends python3-pip ros-humble-nav2-msgs && \ - pip3 install --no-cache-dir -r /app/requirements.txt && \ + pip3 install --no-cache-dir --retries 5 --timeout 180 -r /app/requirements.txt && \ rm -rf /var/lib/apt/lists/* CMD bash -c "source /opt/ros/humble/setup.bash && python3 /app/bridge.py" diff --git a/resources/slam-ros-bridge/bridge.py b/resources/slam-ros-bridge/bridge.py index 9c530eb..00f80dc 100644 --- a/resources/slam-ros-bridge/bridge.py +++ b/resources/slam-ros-bridge/bridge.py @@ -1,34 +1,41 @@ #!/usr/bin/env python3 import asyncio import array +import json import logging import math import os +import shutil +import sqlite3 import struct import threading import time import traceback from contextlib import asynccontextmanager +from pathlib import Path from typing import Optional import rclpy from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect from fastapi.middleware.cors import CORSMiddleware -from geometry_msgs.msg import Twist -from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import OccupancyGrid, Odometry +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist +from action_msgs.msg import GoalStatus +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.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 sensor_msgs.msg import LaserScan +from sensor_msgs.msg import LaserScan, PointCloud2 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"))) + def yaw_from_quaternion(q) -> float: return math.atan2( @@ -88,15 +95,29 @@ class NavigationGoalPayload(BaseModel): frame_id: str = "map" +class InitialPosePayload(NavigationGoalPayload): + pass + + +class SaveMapPayload(BaseModel): + name: Optional[str] = None + + class SlamNode(Node): def __init__(self): super().__init__("slam_web_bridge") + self.set_parameters([Parameter("use_sim_time", value=True)]) self.odom_topic = os.environ.get("ODOM_TOPIC", "/odom") self.scan_topic = os.environ.get("SCAN_TOPIC", "/scan") + 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.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") + self.plan_topic = os.environ.get("PLAN_TOPIC", "/plan") 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") best_effort_qos = QoSProfile( depth=1, @@ -108,33 +129,62 @@ class SlamNode(Node): reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE, ) + map_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + tf_static_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) self._odom: Optional[Odometry] = None self._scan: Optional[LaserScan] = None + self._point_cloud: Optional[PointCloud2] = None + self._fallback_point_cloud: Optional[PointCloud2] = None + self._icp_odom: Optional[Odometry] = None self._map: Optional[OccupancyGrid] = None + self._plan: Optional[NavPath] = None self._tf = {} self._odom_count = 0 self._scan_count = 0 + self._point_cloud_count = 0 + self._fallback_point_cloud_count = 0 + self._icp_odom_count = 0 self._map_count = 0 self._tf_count = 0 self._last_odom_wall_time = None self._last_scan_wall_time = None + self._last_point_cloud_wall_time = None + self._last_fallback_point_cloud_wall_time = None + self._last_icp_odom_wall_time = None self._last_map_wall_time = None + self._navigation = {"status": "idle", "goal_id": None} + self._active_goal_handle = None self._lock = threading.Lock() 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(OccupancyGrid, self.map_topic, self._on_map, reliable_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) + 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) self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, best_effort_qos) + self.create_subscription(NavPath, self.plan_topic, self._on_plan, reliable_qos) self.create_subscription(TFMessage, "/tf", self._on_tf, qos_profile_sensor_data) - self.create_subscription(TFMessage, "/tf_static", self._on_tf, reliable_qos) + 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._initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10) self._navigate_to_pose_client = ActionClient(self, NavigateToPose, self.navigate_to_pose_action) self.get_logger().info( - f"SLAM bridge started: odom={self.odom_topic}, scan={self.scan_topic}, map={self.map_topic}, cmd_vel={self.cmd_vel_topic}, navigate={self.navigate_to_pose_action}" + 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}" ) def _on_odom(self, msg: Odometry): @@ -143,18 +193,216 @@ class SlamNode(Node): 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: + message = PoseWithCovarianceStamped() + message.header.stamp = self.get_clock().now().to_msg() + message.header.frame_id = frame_id + message.pose.pose.position.x = x + message.pose.pose.position.y = y + orientation = quaternion_from_yaw(yaw) + message.pose.pose.orientation.x = orientation["x"] + message.pose.pose.orientation.y = orientation["y"] + message.pose.pose.orientation.z = orientation["z"] + message.pose.pose.orientation.w = orientation["w"] + message.pose.covariance[0] = 0.25 + message.pose.covariance[7] = 0.25 + message.pose.covariance[35] = 0.0685 + self._initial_pose_pub.publish(message) + + def save_map(self, name: Optional[str] = None) -> dict: + with self._lock: + grid = self._map + if grid is None or not grid.data: + raise ValueError("저장할 SLAM 맵이 없습니다.") + width = grid.info.width + height = grid.info.height + resolution = grid.info.resolution + origin = grid.info.origin + data = list(grid.data) + + if width <= 0 or height <= 0 or len(data) < width * height: + raise ValueError("저장할 SLAM 맵 데이터가 올바르지 않습니다.") + + safe_name = "".join(char if char.isalnum() or char in "-_" else "-" for char in (name or "").strip()) + if not safe_name: + safe_name = time.strftime("slam-map-%Y%m%d-%H%M%S") + maps_dir = Path(os.environ.get("SAVED_MAPS_PATH", "/workspace/maps")) + maps_dir.mkdir(parents=True, exist_ok=True) + pgm_path = maps_dir / f"{safe_name}.pgm" + yaml_path = maps_dir / f"{safe_name}.yaml" + + # OccupancyGrid starts at the lower-left origin; PGM rows start at the top. + pixels = bytearray() + for row in range(height - 1, -1, -1): + for column in range(width): + value = data[row * width + column] + pixels.append(0 if value >= 50 else 254 if value == 0 else 205) + pgm_path.write_bytes(f"P5\n{width} {height}\n255\n".encode("ascii") + pixels) + yaml_path.write_text( + "\n".join([ + f"image: {pgm_path.name}", + "mode: trinary", + f"resolution: {resolution}", + f"origin: [{origin.position.x}, {origin.position.y}, {yaw_from_quaternion(origin.orientation)}]", + "negate: 0", + "occupied_thresh: 0.65", + "free_thresh: 0.25", + "", + ]), + encoding="utf-8", + ) + + # Keep the RTAB-Map database beside the Nav2-compatible PGM/YAML map. + # The live database is reset when a new SLAM session starts, so each + # explicit save receives its own SQLite-consistent snapshot. + 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(): + database_snapshot = maps_dir / f"{safe_name}.db" + source_connection = None + destination_connection = None + try: + source_connection = sqlite3.connect(f"file:{database_source}?mode=ro", uri=True) + destination_connection = sqlite3.connect(str(database_snapshot)) + source_connection.backup(destination_connection) + except sqlite3.Error: + if destination_connection is not None: + destination_connection.close() + destination_connection = None + shutil.copy2(database_source, database_snapshot) + finally: + if destination_connection is not None: + destination_connection.close() + if source_connection is not None: + source_connection.close() + database_path = str(database_snapshot) + + return { + "name": safe_name, + "yamlPath": str(yaml_path), + "pgmPath": str(pgm_path), + "databasePath": database_path, + "resolution": resolution, + "width": width, + "height": height, + } + def _on_scan(self, msg: LaserScan): with self._lock: self._scan = msg self._scan_count += 1 self._last_scan_wall_time = time.time() + def _on_point_cloud(self, msg: PointCloud2): + with self._lock: + self._point_cloud = msg + self._point_cloud_count += 1 + self._last_point_cloud_wall_time = time.time() + + def _on_fallback_point_cloud(self, msg: PointCloud2): + with self._lock: + self._fallback_point_cloud = msg + self._fallback_point_cloud_count += 1 + self._last_fallback_point_cloud_wall_time = time.time() + + def _active_point_cloud(self) -> Optional[PointCloud2]: + with self._lock: + primary = self._point_cloud + fallback = self._fallback_point_cloud + + # A global RTAB-Map cloud is the visualization source. Raw LiDAR is + # only a startup fallback before the first global map is received. + if primary is not None: + return primary + return fallback + + def _on_icp_odom(self, msg: Odometry): + with self._lock: + self._icp_odom = msg + self._icp_odom_count += 1 + self._last_icp_odom_wall_time = time.time() + def _on_map(self, msg: OccupancyGrid): with self._lock: self._map = msg self._map_count += 1 self._last_map_wall_time = time.time() + def _on_plan(self, msg: NavPath): + with self._lock: + self._plan = msg + + @staticmethod + def _has_populated_map(grid: Optional[OccupancyGrid]) -> bool: + return bool(grid and grid.info.width > 0 and grid.info.height > 0 and grid.data) + + @staticmethod + def _initial_scan_map(scan: Optional[LaserScan], odom: Optional[Odometry]) -> Optional[dict]: + if scan is None or not scan.ranges: + return None + + # Show the first LiDAR sweep before slam_toolbox has produced an occupancy grid. + resolution = 0.05 + width = 160 + height = 160 + origin_x = (odom.pose.pose.position.x if odom else 0.0) - (width * resolution / 2.0) + origin_y = (odom.pose.pose.position.y if odom else 0.0) - (height * resolution / 2.0) + robot_x = odom.pose.pose.position.x if odom else 0.0 + robot_y = odom.pose.pose.position.y if odom else 0.0 + robot_yaw = yaw_from_quaternion(odom.pose.pose.orientation) if odom else 0.0 + data = array.array("b", [-1]) * (width * height) + + def to_cell(x: float, y: float) -> tuple[int, int]: + return int((x - origin_x) / resolution), int((y - origin_y) / resolution) + + for index, distance in enumerate(scan.ranges): + if not math.isfinite(distance) or distance < scan.range_min or distance >= scan.range_max: + continue + angle = robot_yaw + scan.angle_min + index * scan.angle_increment + steps = max(1, int(distance / resolution)) + for step in range(0, steps, 2): + x, y = to_cell( + robot_x + math.cos(angle) * step * resolution, + robot_y + math.sin(angle) * step * resolution, + ) + if 0 <= x < width and 0 <= y < height: + data[y * width + x] = 0 + x, y = to_cell( + robot_x + math.cos(angle) * distance, + robot_y + math.sin(angle) * distance, + ) + if 0 <= x < width and 0 <= y < height: + data[y * width + x] = 100 + + return { + "frame_id": odom.header.frame_id if odom else "odom", + "width": width, + "height": height, + "resolution": resolution, + "origin": {"x": origin_x, "y": origin_y, "z": 0.0, "yaw": 0.0}, + "data": data, + "t": time.time(), + } + + def _effective_map(self, grid: Optional[OccupancyGrid], scan: Optional[LaserScan], odom: Optional[Odometry]) -> Optional[dict]: + if self._has_populated_map(grid): + origin = grid.info.origin + return { + "frame_id": grid.header.frame_id, + "width": grid.info.width, + "height": grid.info.height, + "resolution": grid.info.resolution, + "origin": { + "x": origin.position.x, + "y": origin.position.y, + "z": origin.position.z, + "yaw": yaw_from_quaternion(origin.orientation), + }, + "data": array.array("b", grid.data), + "t": grid.header.stamp.sec + grid.header.stamp.nanosec * 1e-9, + } + return self._initial_scan_map(scan, odom) + def _on_tf(self, msg: TFMessage): with self._lock: for transform in msg.transforms: @@ -172,12 +420,16 @@ class SlamNode(Node): self._tf_count += 1 def publish_cmd_vel(self, linear: float, angular: float): + if self.read_only: + raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.") msg = Twist() msg.linear.x = float(linear) msg.angular.z = float(angular) self._cmd_vel_pub.publish(msg) def send_navigation_goal(self, x: float, y: float, yaw: float = 0.0, frame_id: str = "map") -> dict: + if self.read_only: + raise RuntimeError("읽기 전용 3D SLAM 화면에서는 목표점을 설정할 수 없습니다.") if not self._navigate_to_pose_client.wait_for_server(timeout_sec=2.0): raise RuntimeError(f"NavigateToPose action server is not available: {self.navigate_to_pose_action}") @@ -206,21 +458,83 @@ class SlamNode(Node): if not goal_handle.accepted: return {"ok": False, "accepted": False, "message": "Navigation goal was rejected"} + goal_id = bytes(goal_handle.goal_id.uuid).hex() + with self._lock: + self._navigation = {"status": "running", "goal_id": goal_id} + self._active_goal_handle = goal_handle + goal_handle.get_result_async().add_done_callback( + lambda result_future, expected_goal_id=goal_id: self._on_navigation_result( + result_future, expected_goal_id + ) + ) + return { "ok": True, "accepted": True, - "goal_id": bytes(goal_handle.goal_id.uuid).hex(), + "goal_id": goal_id, } + def _on_navigation_result(self, future, goal_id: str): + try: + status = future.result().status + except Exception: + self.get_logger().warning("NavigateToPose result could not be read") + return + + if status == GoalStatus.STATUS_SUCCEEDED: + result_status = "succeeded" + elif status == GoalStatus.STATUS_CANCELED: + result_status = "canceled" + else: + result_status = "failed" + + with self._lock: + # A previous goal can finish after a newer goal has been accepted. + # Do not let that completion overwrite the current navigation state. + if self._navigation.get("goal_id") != goal_id: + return + self._navigation = {**self._navigation, "status": result_status} + self._active_goal_handle = None + + def cancel_navigation(self, wait: bool = False) -> bool: + with self._lock: + goal_handle = self._active_goal_handle + self._navigation = {**self._navigation, "status": "canceled"} + if goal_handle is None: + return True + + cancel_future = goal_handle.cancel_goal_async() + if not wait: + return True + + deadline = time.time() + 2.0 + while not cancel_future.done() and time.time() < deadline: + time.sleep(0.02) + if not cancel_future.done(): + return False + try: + cancel_future.result() + return True + except Exception: + self.get_logger().warning("NavigateToPose goal cancellation could not be confirmed") + return False + def snapshot(self, include_map: bool = True) -> dict: with self._lock: odom = self._odom + icp_odom = self._icp_odom scan = self._scan grid = self._map + plan = self._plan tf_snapshot = dict(self._tf) + navigation = dict(self._navigation) + effective_map = self._effective_map(grid, scan, odom) + point_cloud = self._active_point_cloud() pose = None pose_map = None + scan_pose = None + scan_pose_map = None if odom is not None: pos = odom.pose.pose.position ori = odom.pose.pose.orientation @@ -239,6 +553,21 @@ class SlamNode(Node): "angular": vel.angular.z, "t": odom.header.stamp.sec + odom.header.stamp.nanosec * 1e-9, } + base_to_scan = ( + tf_snapshot.get((odom.child_frame_id, scan.header.frame_id)) + if scan is not None + else None + ) + if base_to_scan is not None: + scan_pose = { + **compose_2d(pose, base_to_scan), + "z": pose.get("z", 0.0) + base_to_scan.get("z", 0.0), + "roll": 0.0, + "pitch": 0.0, + "frame_id": pose["frame_id"], + "source_frame_id": scan.header.frame_id, + "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9, + } map_to_odom = tf_snapshot.get(("map", odom.header.frame_id)) if map_to_odom is not None: pose_map = { @@ -249,6 +578,50 @@ class SlamNode(Node): "t": pose["t"], } + if scan is not None and pose_map is not None: + # A scan is measured from its sensor frame, not base_link. Use the + # published base_link -> scan-frame TF so its endpoints overlay the + # occupancy grid in the same map coordinate system. + base_to_scan = tf_snapshot.get((pose_map["child_frame_id"], scan.header.frame_id)) + if base_to_scan is not None: + scan_pose_map = { + **compose_2d(pose_map, base_to_scan), + "z": pose_map.get("z", 0.0) + base_to_scan.get("z", 0.0), + "roll": 0.0, + "pitch": 0.0, + "frame_id": "map", + "source_frame_id": scan.header.frame_id, + "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9, + } + + pose_3d = None + if icp_odom is not None: + pos = icp_odom.pose.pose.position + ori = icp_odom.pose.pose.orientation + euler = euler_from_quaternion(ori) + pose_3d = { + "frame_id": icp_odom.header.frame_id, + "child_frame_id": icp_odom.child_frame_id, + "x": pos.x, + "y": pos.y, + "z": pos.z, + "roll": euler["roll"], + "pitch": euler["pitch"], + "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 + 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. + pose_3d = { + **pose_3d, + **compose_2d(map_to_icp_odom, pose_3d), + "frame_id": cloud_frame, + "source_frame_id": icp_odom.header.frame_id, + } + scan_payload = None if scan is not None: scan_payload = { @@ -264,21 +637,23 @@ class SlamNode(Node): } map_payload = None - if include_map and grid is not None: - origin = grid.info.origin + if include_map and effective_map is not None: map_payload = { - "frame_id": grid.header.frame_id, - "width": grid.info.width, - "height": grid.info.height, - "resolution": grid.info.resolution, - "origin": { - "x": origin.position.x, - "y": origin.position.y, - "z": origin.position.z, - "yaw": yaw_from_quaternion(origin.orientation), - }, + "frame_id": effective_map["frame_id"], + "width": effective_map["width"], + "height": effective_map["height"], + "resolution": effective_map["resolution"], + "origin": effective_map["origin"], "encoding": "binary-int8-map1", - "t": grid.header.stamp.sec + grid.header.stamp.nanosec * 1e-9, + "t": effective_map["t"], + } + + point_cloud_payload = None + if point_cloud is not None: + point_cloud_payload = { + "frame_id": point_cloud.header.frame_id, + "count": point_cloud.width * point_cloud.height, + "t": point_cloud.header.stamp.sec + point_cloud.header.stamp.nanosec * 1e-9, } return { @@ -286,27 +661,83 @@ class SlamNode(Node): "topics": { "odom": self.odom_topic, "scan": self.scan_topic, + "point_cloud": self.point_cloud_topic, + "icp_odom": self.icp_odom_topic, "map": self.map_topic, "cmd_vel": self.cmd_vel_topic, "navigate_to_pose": self.navigate_to_pose_action, }, "pose": pose, "pose_map": pose_map, + "scan_pose": scan_pose, + "scan_pose_map": scan_pose_map, + "pose_3d": pose_3d, "scan": scan_payload, + "point_cloud": point_cloud_payload, "map": map_payload, "tf": { "map_to_odom": tf_snapshot.get(("map", "odom")), }, + "navigation": navigation, + "plan": { + "frame_id": plan.header.frame_id, + "poses": [ + { + "x": pose.pose.position.x, + "y": pose.pose.position.y, + "yaw": yaw_from_quaternion(pose.pose.orientation), + } + for pose in plan.poses + ], + } if plan is not None else None, "server_time": time.time(), } + def point_cloud_payload(self, limit: int = _POINT_CLOUD_LIMIT) -> Optional[dict]: + cloud = self._active_point_cloud() + if cloud is None or cloud.point_step <= 0 or not cloud.data: + return None + + offsets = {field.name: field.offset for field in cloud.fields} + if not all(axis in offsets for axis in ("x", "y", "z")): + return None + + total = cloud.width * cloud.height + if total <= 0: + return None + stride = max(1, math.ceil(total / max(1, limit))) + prefix = ">" if cloud.is_bigendian else "<" + data = cloud.data + points = [] + for index in range(0, total, stride): + offset = index * cloud.point_step + if offset + cloud.point_step > len(data): + break + try: + x = struct.unpack_from(f"{prefix}f", data, offset + offsets["x"])[0] + y = struct.unpack_from(f"{prefix}f", data, offset + offsets["y"])[0] + z = struct.unpack_from(f"{prefix}f", data, offset + offsets["z"])[0] + except struct.error: + break + if math.isfinite(x) and math.isfinite(y) and math.isfinite(z): + points.append([x, y, z]) + + return { + "frame_id": cloud.header.frame_id, + "stamp": cloud.header.stamp.sec + cloud.header.stamp.nanosec * 1e-9, + "points": points, + } + def map_binary(self) -> Optional[bytes]: with self._lock: grid = self._map - if grid is None: + scan = self._scan + odom = self._odom + effective_map = self._effective_map(grid, scan, odom) + if effective_map is None: return None - header = struct.pack("<4sII", b"MAP1", grid.info.width, grid.info.height) - data = array.array("b", grid.data).tobytes() + header = struct.pack("<4sII", b"MAP1", effective_map["width"], effective_map["height"]) + data = effective_map["data"].tobytes() return header + data def scan_binary(self) -> Optional[bytes]: @@ -329,28 +760,40 @@ 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.scan_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.map_topic, self.cmd_vel_topic): topics[name] = types return { "configured": { "odom": self.odom_topic, "scan": self.scan_topic, + "point_cloud": self.point_cloud_topic, + "fallback_point_cloud": self.fallback_point_cloud_topic, + "icp_odom": self.icp_odom_topic, "map": self.map_topic, "cmd_vel": self.cmd_vel_topic, "navigate_to_pose": self.navigate_to_pose_action, }, "visible": topics, "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), + "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), "cmd_vel_subscribers": self.count_subscribers(self.cmd_vel_topic), "received": { "scan_count": self._scan_count, + "point_cloud_count": self._point_cloud_count, + "fallback_point_cloud_count": self._fallback_point_cloud_count, + "icp_odom_count": self._icp_odom_count, "odom_count": self._odom_count, "map_count": self._map_count, "tf_count": self._tf_count, "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_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, }, @@ -399,6 +842,7 @@ def health(): "ok": True, "has_pose": bool(snap.get("pose")), "has_scan": bool(snap.get("scan")), + "has_point_cloud": bool(snap.get("point_cloud")), "has_map": bool(snap.get("map")), "topics": snap.get("topics", {}), } @@ -411,6 +855,37 @@ def state(): return slam_node.snapshot() +@app.get("/point-cloud") +def point_cloud(): + if not slam_node: + raise HTTPException(status_code=503, detail="ROS node is not ready") + payload = slam_node.point_cloud_payload() + if payload is None: + raise HTTPException(status_code=404, detail="포인트클라우드를 아직 수신하지 못했습니다.") + return payload + + +@app.get("/world-layout") +def world_layout(): + path = Path(os.environ.get("WORLD_LAYOUT_PATH", "/workspace/world-layout.json")) + try: + parsed = json.loads(path.read_text(encoding="utf-8")) + objects = parsed.get("objects", []) if isinstance(parsed, dict) else [] + return {"version": 1, "objects": objects if isinstance(objects, list) else []} + except (FileNotFoundError, OSError, json.JSONDecodeError): + return {"version": 1, "objects": []} + + +@app.post("/save-map") +def save_map(payload: SaveMapPayload): + if not slam_node: + 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: + raise HTTPException(status_code=409, detail=str(error)) from error + + @app.get("/debug/topics") def debug_topics(): if not slam_node: @@ -430,7 +905,10 @@ def cmd_vel(payload: VelocityPayload): def stop(): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") - slam_node.publish_cmd_vel(0.0, 0.0) + try: + slam_node.publish_cmd_vel(0.0, 0.0) + except RuntimeError as error: + raise HTTPException(status_code=403, detail=str(error)) from error return {"ok": True} @@ -445,6 +923,25 @@ def navigate_to_pose(payload: NavigationGoalPayload): return result +@app.post("/cancel_navigation") +def cancel_navigation(): + if not slam_node: + raise HTTPException(status_code=503, detail="ROS node is not ready") + if not slam_node.cancel_navigation(wait=True): + raise HTTPException(status_code=504, detail="기존 자율주행 목표 취소 시간이 초과되었습니다.") + return {"ok": True} + + +@app.post("/initialpose") +def initial_pose(payload: InitialPosePayload): + if not slam_node: + 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} + + @app.websocket("/ws/state") async def ws_state(websocket: WebSocket): await websocket.accept() @@ -468,6 +965,29 @@ async def ws_state(websocket: WebSocket): pass +@app.websocket("/ws/control") +async def ws_control(websocket: WebSocket): + await websocket.accept() + try: + while True: + payload = await websocket.receive_json() + if not slam_node or payload.get("type") != "cmd_vel": + continue + try: + velocity = VelocityPayload( + linear=payload.get("linear", 0.0), + angular=payload.get("angular", 0.0), + ) + except (TypeError, ValueError): + continue + try: + slam_node.publish_cmd_vel(velocity.linear, velocity.angular) + except RuntimeError: + continue + except WebSocketDisconnect: + pass + + if __name__ == "__main__": uvicorn.run( app,