998 lines
38 KiB
Python
998 lines
38 KiB
Python
#!/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 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, 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"],
|
|
}
|
|
|
|
|
|
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.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._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(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:
|
|
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:
|
|
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,
|
|
}
|
|
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 = {
|
|
**compose_2d(map_to_odom, pose),
|
|
"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:
|
|
# 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 = {
|
|
"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": 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
|
|
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", {}),
|
|
}
|
|
|
|
|
|
@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"),
|
|
)
|