#!/usr/bin/env python3 import asyncio import array import base64 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 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.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 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( 2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z), ) def quaternion_from_yaw(yaw: float) -> dict: half = yaw * 0.5 return { "x": 0.0, "y": 0.0, "z": math.sin(half), "w": math.cos(half), } def euler_from_quaternion(q) -> dict: sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z) cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y) roll = math.atan2(sinr_cosp, cosr_cosp) sinp = 2.0 * (q.w * q.y - q.z * q.x) if abs(sinp) >= 1.0: pitch = math.copysign(math.pi / 2.0, sinp) else: pitch = math.asin(sinp) return { "roll": roll, "pitch": pitch, "yaw": yaw_from_quaternion(q), } def compose_2d(a: dict, b: dict) -> dict: c = math.cos(a["yaw"]) s = math.sin(a["yaw"]) return { "x": a["x"] + c * b["x"] - s * b["y"], "y": a["y"] + s * b["x"] + c * b["y"], "z": a.get("z", 0.0) + b.get("z", 0.0), "yaw": a["yaw"] + b["yaw"], } def invert_2d(transform: dict) -> dict: """Return the inverse of a planar TF transform.""" yaw = -transform["yaw"] c = math.cos(yaw) s = math.sin(yaw) return { "x": c * -transform["x"] - s * -transform["y"], "y": s * -transform["x"] + c * -transform["y"], "z": -transform.get("z", 0.0), "yaw": yaw, } def resolve_2d_transform(transforms: dict, target_frame: str, source_frame: str) -> Optional[dict]: """Resolve target <- source through the currently published TF graph.""" if not target_frame or not source_frame: return None if target_frame == source_frame: return {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0} pending = [(target_frame, {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0})] visited = set() while pending: frame, accumulated = pending.pop(0) if frame in visited: continue visited.add(frame) if frame == source_frame: return accumulated for (parent, child), transform in transforms.items(): if parent == frame and child not in visited: pending.append((child, compose_2d(accumulated, transform))) elif child == frame and parent not in visited: pending.append((parent, compose_2d(accumulated, invert_2d(transform)))) return None class VelocityPayload(BaseModel): linear: float = 0.0 angular: float = 0.0 class NavigationGoalPayload(BaseModel): x: float y: float yaw: float = 0.0 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.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.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, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE, ) reliable_qos = QoSProfile( depth=10, 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._amcl_pose: Optional[PoseWithCovarianceStamped] = 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._amcl_pose_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_amcl_pose_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._plan_count = 0 self._last_plan_wall_time = None self._topic_status_cache = {} self._topic_status_last_refresh = 0.0 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(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) 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, 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}, 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): 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. with self._lock: odom_stamp = self._odom.header.stamp if self._odom is not None else None 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.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) # Keep a compact copy with the map record so the LMS can render an # actual saved-map preview without accessing the student's workspace. preview_limit = 192 preview_scale = max(1, math.ceil(width / preview_limit), math.ceil(height / preview_limit)) preview_width = math.ceil(width / preview_scale) preview_height = math.ceil(height / preview_scale) preview_pixels = bytearray() for preview_row in range(preview_height): source_top = preview_row * preview_scale source_bottom = min(height, source_top + preview_scale) for preview_column in range(preview_width): source_left = preview_column * preview_scale source_right = min(width, source_left + preview_scale) # Preserve occupied cells while reducing the grid: black is # lower than unknown/free in the generated PGM palette. preview_pixels.append(min( pixels[row * width + column] for row in range(source_top, source_bottom) for column in range(source_left, source_right) )) preview_pgm = f"P5\n{preview_width} {preview_height}\n255\n".encode("ascii") + preview_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, "previewPgm": base64.b64encode(preview_pgm).decode("ascii"), } 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_amcl_pose(self, msg: PoseWithCovarianceStamped): with self._lock: self._amcl_pose = msg self._amcl_pose_count += 1 self._last_amcl_pose_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 self._plan_count += 1 self._last_plan_wall_time = time.time() def topic_status(self) -> dict: now = time.time() with self._lock: if now - self._topic_status_last_refresh < 1.0: return dict(self._topic_status_cache) scan_last = self._last_scan_wall_time odom_last = self._last_odom_wall_time 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 map_count = self._map_count plan_count = self._plan_count def received_recently(last_received: Optional[float]) -> bool: return last_received is not None and now - last_received <= 3.0 try: plan_available = plan_count > 0 or self.count_publishers(self.plan_topic) > 0 cmd_vel_available = self.count_subscribers(self.cmd_vel_topic) > 0 navigation_available = self._navigate_to_pose_client.server_is_ready() except Exception: plan_available = plan_count > 0 cmd_vel_available = False navigation_available = False status = { "scan": {"name": self.scan_topic, "available": received_recently(scan_last)}, "odom": {"name": self.odom_topic, "available": received_recently(odom_last)}, "map": {"name": self.map_topic, "available": map_count > 0}, "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), }, "plan": {"name": self.plan_topic, "available": plan_available}, "cmd_vel": {"name": self.cmd_vel_topic, "available": cmd_vel_available}, "navigate_to_pose": {"name": self.navigate_to_pose_action, "available": navigation_available}, } with self._lock: self._topic_status_cache = status self._topic_status_last_refresh = now return dict(status) @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: t = transform.transform.translation r = transform.transform.rotation self._tf[(transform.header.frame_id, transform.child_frame_id)] = { "parent": transform.header.frame_id, "child": transform.child_frame_id, "x": t.x, "y": t.y, "z": t.z, "yaw": yaw_from_quaternion(r), "t": transform.header.stamp.sec + transform.header.stamp.nanosec * 1e-9, } 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}") goal_msg = NavigateToPose.Goal() goal_msg.pose = PoseStamped() goal_msg.pose.header.frame_id = frame_id or "map" goal_msg.pose.header.stamp = self.get_clock().now().to_msg() goal_msg.pose.pose.position.x = float(x) goal_msg.pose.pose.position.y = float(y) goal_msg.pose.pose.position.z = 0.0 q = quaternion_from_yaw(float(yaw)) goal_msg.pose.pose.orientation.x = q["x"] goal_msg.pose.pose.orientation.y = q["y"] goal_msg.pose.pose.orientation.z = q["z"] goal_msg.pose.pose.orientation.w = q["w"] future = self._navigate_to_pose_client.send_goal_async(goal_msg) deadline = time.time() + 3.0 while not future.done() and time.time() < deadline: time.sleep(0.02) if not future.done(): raise RuntimeError("NavigateToPose goal request timed out") goal_handle = future.result() 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": 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 vel = odom.twist.twist euler = euler_from_quaternion(ori) pose = { "frame_id": odom.header.frame_id, "child_frame_id": odom.child_frame_id, "x": pos.x, "y": pos.y, "z": pos.z, "roll": euler["roll"], "pitch": euler["pitch"], "yaw": euler["yaw"], "linear": vel.linear.x, "angular": vel.angular.z, "t": odom.header.stamp.sec + odom.header.stamp.nanosec * 1e-9, } odom_to_scan = ( resolve_2d_transform(tf_snapshot, odom.header.frame_id, scan.header.frame_id) if scan is not None else None ) if odom_to_scan is not None: scan_pose = { **odom_to_scan, "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_base = resolve_2d_transform(tf_snapshot, "map", odom.child_frame_id) if map_to_base is not None: pose_map = { **map_to_base, "frame_id": "map", "child_frame_id": odom.child_frame_id, "source_frame_id": odom.header.frame_id, "t": pose["t"], } if scan is not None and pose_map is not None: # Resolve map -> sensor directly through the published TF graph so # the scan uses the same localization correction as the map. map_to_scan = resolve_2d_transform(tf_snapshot, "map", scan.header.frame_id) if map_to_scan is not None: scan_pose_map = { **map_to_scan, "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 = { "frame_id": scan.header.frame_id, "angle_min": scan.angle_min, "angle_max": scan.angle_max, "angle_increment": scan.angle_increment, "range_min": scan.range_min, "range_max": scan.range_max, "count": len(scan.ranges), "encoding": "binary-float32-scan1", "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9, } map_payload = None if include_map and effective_map is not None: map_payload = { "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": 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 { "ok": True, "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": resolve_2d_transform(tf_snapshot, "map", "odom"), }, "navigation": navigation, "topic_status": self.topic_status(), "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 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", effective_map["width"], effective_map["height"]) data = effective_map["data"].tobytes() return header + data def scan_binary(self) -> Optional[bytes]: with self._lock: scan = self._scan if scan is None: return None values = array.array( "f", [ math.nan if math.isinf(v) or math.isnan(v) else float(v) for v in scan.ranges ], ) if values.itemsize != 4: values = array.array("f", values.tolist()) header = struct.pack("<4sI", b"SCN1", len(values)) return header + values.tobytes() 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): 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, }, } slam_node: Optional[SlamNode] = None @asynccontextmanager async def lifespan(app: FastAPI): global slam_node rclpy.init() slam_node = SlamNode() def spin(): executor = MultiThreadedExecutor(num_threads=2) executor.add_node(slam_node) try: executor.spin() except Exception: logging.error(traceback.format_exc()) thread = threading.Thread(target=spin, daemon=True) thread.start() yield if rclpy.ok(): rclpy.shutdown() app = FastAPI(title="SLAM ROS Bridge", lifespan=lifespan) app.add_middleware( CORSMiddleware, allow_origins=["*"], allow_methods=["*"], allow_headers=["*"], ) @app.get("/health") def health(): snap = slam_node.snapshot() if slam_node else {} return { "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", {}), "topic_status": snap.get("topic_status", {}), } @app.get("/state") def state(): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") 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: raise HTTPException(status_code=503, detail="ROS node is not ready") return slam_node.topic_debug() @app.post("/cmd_vel") def cmd_vel(payload: VelocityPayload): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") slam_node.publish_cmd_vel(payload.linear, payload.angular) return {"ok": True} @app.post("/stop") def stop(): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") 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} @app.post("/navigate_to_pose") def navigate_to_pose(payload: NavigationGoalPayload): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") try: result = slam_node.send_navigation_goal(payload.x, payload.y, payload.yaw, payload.frame_id) except RuntimeError as error: raise HTTPException(status_code=503, detail=str(error)) from error 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() last_map_sent = 0.0 try: while True: if slam_node: now = time.time() include_map = now - last_map_sent >= 1.0 await websocket.send_json(slam_node.snapshot(include_map=include_map)) scan_bytes = slam_node.scan_binary() if scan_bytes is not None: await websocket.send_bytes(scan_bytes) if include_map: map_bytes = slam_node.map_binary() if map_bytes is not None: await websocket.send_bytes(map_bytes) last_map_sent = now await asyncio.sleep(1.0 / 15.0) except (WebSocketDisconnect, Exception): 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, host="0.0.0.0", port=int(os.environ.get("PORT", "8765")), log_level=os.environ.get("LOG_LEVEL", "info"), )