1664 lines
70 KiB
Python
1664 lines
70 KiB
Python
#!/usr/bin/env python3
|
|
import asyncio
|
|
import array
|
|
import base64
|
|
import json
|
|
import logging
|
|
import math
|
|
import os
|
|
import shutil
|
|
import sqlite3
|
|
import struct
|
|
import 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.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
|
|
|
|
|
|
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._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)
|
|
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._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 _on_camera_image(self, msg: CompressedImage):
|
|
with self._lock:
|
|
self._camera_image = msg
|
|
self._camera_image_count += 1
|
|
self._last_camera_image_wall_time = time.time()
|
|
|
|
def _active_point_cloud(self) -> Optional[PointCloud2]:
|
|
with self._lock:
|
|
primary = self._point_cloud
|
|
fallback = self._fallback_point_cloud
|
|
|
|
# A global RTAB-Map cloud is the visualization source. Raw LiDAR is
|
|
# only a startup fallback before the first global map is received.
|
|
if primary is not None:
|
|
return primary
|
|
return fallback
|
|
|
|
def _on_icp_odom(self, msg: Odometry):
|
|
with self._lock:
|
|
self._icp_odom = msg
|
|
self._icp_odom_count += 1
|
|
self._last_icp_odom_wall_time = time.time()
|
|
|
|
def _on_map(self, msg: OccupancyGrid):
|
|
with self._lock:
|
|
self._map = msg
|
|
self._map_count += 1
|
|
self._last_map_wall_time = time.time()
|
|
|
|
def _on_plan(self, msg: NavPath):
|
|
with self._lock:
|
|
self._plan = msg
|
|
self._plan_count += 1
|
|
self._last_plan_wall_time = time.time()
|
|
|
|
def _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
|
|
camera_image_last = self._last_camera_image_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": received_recently(camera_image_last),
|
|
},
|
|
"plan": {"name": self.plan_topic, "available": plan_available},
|
|
"cmd_vel": {"name": self.cmd_vel_topic, "available": cmd_vel_available},
|
|
"navigate_to_pose": {"name": self.navigate_to_pose_action, "available": navigation_available},
|
|
}
|
|
with self._lock:
|
|
self._topic_status_cache = status
|
|
self._topic_status_last_refresh = now
|
|
return dict(status)
|
|
|
|
@staticmethod
|
|
def _has_populated_map(grid: Optional[OccupancyGrid]) -> bool:
|
|
return bool(grid and grid.info.width > 0 and grid.info.height > 0 and grid.data)
|
|
|
|
@staticmethod
|
|
def _initial_scan_map(scan: Optional[LaserScan], odom: Optional[Odometry]) -> Optional[dict]:
|
|
if scan is None or not scan.ranges:
|
|
return None
|
|
|
|
# Show the first LiDAR sweep before slam_toolbox has produced an occupancy grid.
|
|
resolution = 0.05
|
|
width = 160
|
|
height = 160
|
|
origin_x = (odom.pose.pose.position.x if odom else 0.0) - (width * resolution / 2.0)
|
|
origin_y = (odom.pose.pose.position.y if odom else 0.0) - (height * resolution / 2.0)
|
|
robot_x = odom.pose.pose.position.x if odom else 0.0
|
|
robot_y = odom.pose.pose.position.y if odom else 0.0
|
|
robot_yaw = yaw_from_quaternion(odom.pose.pose.orientation) if odom else 0.0
|
|
data = array.array("b", [-1]) * (width * height)
|
|
|
|
def to_cell(x: float, y: float) -> tuple[int, int]:
|
|
return int((x - origin_x) / resolution), int((y - origin_y) / resolution)
|
|
|
|
for index, distance in enumerate(scan.ranges):
|
|
if not math.isfinite(distance) or distance < scan.range_min or distance >= scan.range_max:
|
|
continue
|
|
angle = robot_yaw + scan.angle_min + index * scan.angle_increment
|
|
steps = max(1, int(distance / resolution))
|
|
for step in range(0, steps, 2):
|
|
x, y = to_cell(
|
|
robot_x + math.cos(angle) * step * resolution,
|
|
robot_y + math.sin(angle) * step * resolution,
|
|
)
|
|
if 0 <= x < width and 0 <= y < height:
|
|
data[y * width + x] = 0
|
|
x, y = to_cell(
|
|
robot_x + math.cos(angle) * distance,
|
|
robot_y + math.sin(angle) * distance,
|
|
)
|
|
if 0 <= x < width and 0 <= y < height:
|
|
data[y * width + x] = 100
|
|
|
|
return {
|
|
"frame_id": odom.header.frame_id if odom else "odom",
|
|
"width": width,
|
|
"height": height,
|
|
"resolution": resolution,
|
|
"origin": {"x": origin_x, "y": origin_y, "z": 0.0, "yaw": 0.0},
|
|
"data": data,
|
|
"t": time.time(),
|
|
}
|
|
|
|
def _effective_map(self, grid: Optional[OccupancyGrid], scan: Optional[LaserScan], odom: Optional[Odometry]) -> Optional[dict]:
|
|
if self._has_populated_map(grid):
|
|
origin = grid.info.origin
|
|
return {
|
|
"frame_id": grid.header.frame_id,
|
|
"width": grid.info.width,
|
|
"height": grid.info.height,
|
|
"resolution": grid.info.resolution,
|
|
"origin": {
|
|
"x": origin.position.x,
|
|
"y": origin.position.y,
|
|
"z": origin.position.z,
|
|
"yaw": yaw_from_quaternion(origin.orientation),
|
|
},
|
|
"data": array.array("b", grid.data),
|
|
"t": grid.header.stamp.sec + grid.header.stamp.nanosec * 1e-9,
|
|
}
|
|
return self._initial_scan_map(scan, odom)
|
|
|
|
def _on_tf(self, msg: TFMessage):
|
|
with self._lock:
|
|
for transform in msg.transforms:
|
|
t = transform.transform.translation
|
|
r = transform.transform.rotation
|
|
self._tf[(transform.header.frame_id, transform.child_frame_id)] = {
|
|
"parent": transform.header.frame_id,
|
|
"child": transform.child_frame_id,
|
|
"x": t.x,
|
|
"y": t.y,
|
|
"z": t.z,
|
|
"yaw": yaw_from_quaternion(r),
|
|
"t": transform.header.stamp.sec + transform.header.stamp.nanosec * 1e-9,
|
|
}
|
|
self._tf_count += 1
|
|
|
|
def publish_cmd_vel(self, linear: float, angular: float):
|
|
if self.read_only:
|
|
raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.")
|
|
msg = Twist()
|
|
msg.linear.x = float(linear)
|
|
msg.angular.z = float(angular)
|
|
self._cmd_vel_pub.publish(msg)
|
|
|
|
def 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,
|
|
"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,
|
|
"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) -> Optional[bytes]:
|
|
with self._lock:
|
|
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():
|
|
if not slam_node:
|
|
raise HTTPException(status_code=503, detail="ROS node is not ready")
|
|
frame = slam_node.camera_frame_jpeg()
|
|
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)
|
|
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),
|
|
)
|
|
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"),
|
|
)
|