#!/usr/bin/env python3 import asyncio import array import base64 import json import logging import math import os import re import shutil import sqlite3 import struct import subprocess import tempfile 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, Response, 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.clock import Clock, ClockType 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 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"))) _SENSOR_POINT_CLOUD_LIMIT = max(1_000, int(os.environ.get("SENSOR_POINT_CLOUD_LIMIT", "4000"))) _CAMERA_FRAME_MAX_AGE_SECONDS = 3.0 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 deadman: bool = False class SteeringPayload(BaseModel): angle: float 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.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") self.plan_topic = os.environ.get("PLAN_TOPIC", "/plan") 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, 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._color_point_cloud: Optional[PointCloud2] = None self._camera_image: Optional[CompressedImage] = None self._camera_frames = {} self._camera_subscriptions = {} self._icp_odom: Optional[Odometry] = None self._map: Optional[OccupancyGrid] = None self._plan: Optional[NavPath] = None self._joint_positions = {} self._steering_feedback = None self._simulator_steering_angle = 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._color_point_cloud_count = 0 self._camera_image_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_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 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._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(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) if not os.environ.get("CAMERA_IMAGE_TOPIC"): self.create_subscription(CompressedImage, "/image_raw/compressed", self._on_camera_image, qos_profile_sensor_data) self._camera_discovery_timer = self.create_timer( 1.0, self._discover_cameras, clock=Clock(clock_type=ClockType.STEADY_TIME)) self.create_subscription(Odometry, self.icp_odom_topic, self._on_icp_odom, best_effort_qos) self.create_subscription(Odometry, self.icp_odom_topic, self._on_icp_odom, reliable_qos) self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, map_qos) 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(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._teleop_lock = threading.Lock() self._teleop_deadline = None self._teleop_timer = self.create_timer( 0.1, self._check_teleop_timeout, clock=Clock(clock_type=ClockType.STEADY_TIME)) 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}, 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") -> dict: """Provide a map-frame seed without overwriting live ICP odometry.""" with self._lock: 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() message.header.stamp = Time().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 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 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(): # 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 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() # 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 { "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 _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 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 _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 _discover_cameras(self): try: topics = sorted(name for name, types in self.get_topic_names_and_types() if re.fullmatch(r'/camera_test/cameras/video[0-9]+/image_jpeg', name) and 'sensor_msgs/msg/CompressedImage' in types)[:8] except Exception: return for topic in topics: camera_id = topic.split('/')[-2] if camera_id not in self._camera_subscriptions: self._camera_subscriptions[camera_id] = self.create_subscription( CompressedImage, topic, lambda msg, key=camera_id, name=topic: self._on_camera_image(msg, key, name), qos_profile_sensor_data) active = {topic.split('/')[-2] for topic in topics} for camera_id in list(self._camera_subscriptions): if camera_id not in active: self.destroy_subscription(self._camera_subscriptions.pop(camera_id)) with self._lock: self._camera_frames.pop(camera_id, None) def _on_camera_image(self, msg: CompressedImage, camera_id='legacy', topic=None): with self._lock: if camera_id != 'legacy' and camera_id not in self._camera_subscriptions: return self._camera_image = msg self._camera_image_count += 1 self._last_camera_image_wall_time = time.time() self._camera_frames[camera_id] = (msg, self._last_camera_image_wall_time, topic or self.camera_image_topic) def camera_sources(self): now = time.time() with self._lock: frames = list(self._camera_frames.items()) return [{"id": key, "name": f'/dev/{key}' if key != 'legacy' else 'Camera', "topic": topic, "available": now - received <= _CAMERA_FRAME_MAX_AGE_SECONDS and bool(image.data) and 'jpeg' in image.format.lower()} for key, (image, received, topic) in sorted(frames)] def _active_point_cloud(self) -> Optional[PointCloud2]: with self._lock: 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 _clear_plan_locked(self): """Discard a path that belongs to a completed or replaced navigation goal.""" self._plan = None self._last_plan_wall_time = None def _on_joint_states(self, msg: JointState): with self._lock: self._joint_positions = { name: position for name, position in zip(msg.name, msg.position) } def _on_steering_feedback(self, msg: Float64): with self._lock: self._steering_feedback = float(msg.data) 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 color_point_cloud_last = self._last_color_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, # 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": any(camera['available'] for camera in self.camera_sources()), }, "plan": {"name": self.plan_topic, "available": plan_available}, "cmd_vel": {"name": self.cmd_vel_topic, "available": cmd_vel_available}, "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, deadman: bool = False): if self.read_only: raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.") if not math.isfinite(linear) or not math.isfinite(angular): raise RuntimeError("Invalid velocity") if deadman: linear = max(-0.1, min(0.1, linear)) angular = max(-0.5, min(0.5, angular)) msg = Twist() msg.linear.x = float(linear) msg.angular.z = float(angular) with self._teleop_lock: self._teleop_deadline = time.monotonic() + 0.5 if deadman and (linear or angular) else None self._cmd_vel_pub.publish(msg) def _check_teleop_timeout(self): # Only stop expired manual commands, never idle autonomous navigation. with self._teleop_lock: if self._teleop_deadline is not None and time.monotonic() >= self._teleop_deadline: self._teleop_deadline = None self._cmd_vel_pub.publish(Twist()) def set_simulator_steering(self, angle: float) -> None: if self.read_only: raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.") if not math.isfinite(angle): raise RuntimeError("조향각이 올바르지 않습니다.") steering_angle = max(-0.5, min(0.5, float(angle))) command = Float64() command.data = steering_angle with self._lock: self._simulator_steering_angle = steering_angle self._steering_pub.publish(command) drive_command = Twist() drive_command.angular.z = steering_angle self._cmd_vel_pub.publish(drive_command) 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._clear_plan_locked() 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 self._clear_plan_locked() def cancel_navigation(self, wait: bool = False) -> bool: with self._lock: goal_handle = self._active_goal_handle self._navigation = {**self._navigation, "status": "canceled"} self._clear_plan_locked() 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 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) point_cloud = self._active_point_cloud() pose = None pose_map = None scan_pose = None scan_pose_map = None vehicle = {} 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 -> base is the live localization transform and must win over # /amcl_pose. AMCL may publish a pose only when it changes, while # TF continues updating with odometry; preferring the message made # the viewer appear to stop between AMCL updates. 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"], } elif amcl_pose is not None and amcl_pose.header.frame_id.lstrip("/") == "map": amcl_position = amcl_pose.pose.pose.position pose_map = { "x": amcl_position.x, "y": amcl_position.y, "z": amcl_position.z, "yaw": yaw_from_quaternion(amcl_pose.pose.pose.orientation), "frame_id": "map", "child_frame_id": odom.child_frame_id, "source_frame_id": odom.header.frame_id, "t": amcl_pose.header.stamp.sec + amcl_pose.header.stamp.nanosec * 1e-9, } elif ( requested_initial_pose and requested_initial_pose["frame_id"].lstrip("/") == "map" and time.monotonic() <= requested_initial_pose.get("expires_at", 0.0) ): pose_map = { **requested_initial_pose, "child_frame_id": odom.child_frame_id, "source_frame_id": odom.header.frame_id, } # The Gazebo MK-mini uses independent front steering joints. Keep # both values so the viewer renders the Ackermann inner/outer turn. steering_left_angle = next( ( position for name, position in joint_positions.items() if name == "front_left_visual_steering_joint" or name.endswith("/front_left_visual_steering_joint") ), None, ) steering_right_angle = next( ( position for name, position in joint_positions.items() if name == "front_right_visual_steering_joint" or name.endswith("/front_right_visual_steering_joint") ), None, ) steering_angle = None if steering_left_angle is not None and steering_right_angle is not None: vehicle["steering_left_angle"] = steering_left_angle vehicle["steering_right_angle"] = steering_right_angle steering_angle = (steering_left_angle + steering_right_angle) * 0.5 else: steering_angle = next( ( position for name, position in joint_positions.items() if name == "front_steering_joint" or name.endswith("/front_steering_joint") ), None, ) if steering_angle is None and steering_feedback is not None: steering_angle = steering_feedback if steering_angle is None: front_fork = resolve_2d_transform( tf_snapshot, odom.child_frame_id or "base_link", "front_fork", ) steering_angle = front_fork["yaw"] if front_fork is not None else None if steering_angle is None: steering_angle = self._simulator_steering_angle if steering_angle is not None: vehicle["steering_angle"] = steering_angle if scan is not None and pose_map is not None: # Prefer direct map -> sensor TF for the current scan. It carries # the current odometry timestamp and keeps the laser synchronized # with the moving robot. 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, } else: base_to_scan = resolve_2d_transform( tf_snapshot, odom.child_frame_id if odom is not None else "base_link", scan.header.frame_id, ) if base_to_scan is not None: scan_pose_map = { **compose_2d(pose_map, base_to_scan), "roll": 0.0, "pitch": 0.0, "frame_id": "map", "source_frame_id": scan.header.frame_id, "t": pose_map["t"], } 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, } map_to_icp_odom = resolve_2d_transform(tf_snapshot, "map", icp_odom.header.frame_id) if map_to_icp_odom is not None: # 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": "map", "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, } 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, "teleop_deadman": not self.read_only, "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, "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, "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, "cameras": self.camera_sources(), "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, "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]: 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 = _SENSOR_POINT_CLOUD_LIMIT) -> 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, camera_id=None) -> Optional[bytes]: with self._lock: if camera_id is not None: entry = self._camera_frames.get(camera_id) if entry is None: return None image, received_at, _topic = entry else: image = self._camera_image received_at = self._last_camera_image_wall_time if image is None or received_at is None or time.time() - received_at > _CAMERA_FRAME_MAX_AGE_SECONDS: return None if not image.data or "jpeg" not in image.format.lower(): 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 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 = [] 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): 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]) 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]) 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: 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.color_point_cloud_topic, self.camera_image_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, "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, "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), "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), "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, "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, "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_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, }, } 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("/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(camera_id: Optional[str] = None): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") frame = slam_node.camera_frame_jpeg(camera_id) if frame is None: raise HTTPException(status_code=404, detail="카메라 프레임을 아직 수신하지 못했습니다.") return Response(content=frame, media_type="image/jpeg", headers={"Cache-Control": "no-store"}) @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, RuntimeError) 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, payload.deadman) return {"ok": True} @app.post("/steering") def steering(payload: SteeringPayload): if not slam_node: raise HTTPException(status_code=503, detail="ROS node is not ready") try: slam_node.set_simulator_steering(payload.angle) except RuntimeError as error: raise HTTPException(status_code=409, detail=str(error)) from error 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 화면에서는 초기 위치를 설정할 수 없습니다.") 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") 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), deadman=payload.get("deadman", False), ) except (TypeError, ValueError): continue try: slam_node.publish_cmd_vel(velocity.linear, velocity.angular, velocity.deadman) 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"), )