platform/lms-fe
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(bridge): preserve 3d slam maps for localization

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