#!/usr/bin/env python3 import asyncio import array import logging import math import os import struct import threading import time import traceback from contextlib import asynccontextmanager from typing import Optional import rclpy from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect from fastapi.middleware.cors import CORSMiddleware from geometry_msgs.msg import Twist from geometry_msgs.msg import PoseStamped from nav_msgs.msg import OccupancyGrid, Odometry from 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.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data from sensor_msgs.msg import LaserScan from tf2_msgs.msg import TFMessage import uvicorn logging.basicConfig(level=logging.INFO) 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 SlamNode(Node): def __init__(self): super().__init__("slam_web_bridge") self.odom_topic = os.environ.get("ODOM_TOPIC", "/odom") self.scan_topic = os.environ.get("SCAN_TOPIC", "/scan") self.cmd_vel_topic = os.environ.get("CMD_VEL_TOPIC", "/cmd_vel") self.map_topic = os.environ.get("MAP_TOPIC", "/map") self.navigate_to_pose_action = os.environ.get("NAVIGATE_TO_POSE_ACTION", "/navigate_to_pose") best_effort_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE, ) reliable_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE, ) self._odom: Optional[Odometry] = None self._scan: Optional[LaserScan] = None self._map: Optional[OccupancyGrid] = None self._tf = {} self._odom_count = 0 self._scan_count = 0 self._map_count = 0 self._tf_count = 0 self._last_odom_wall_time = None self._last_scan_wall_time = None self._last_map_wall_time = None self._lock = threading.Lock() self.create_subscription(Odometry, self.odom_topic, self._on_odom, reliable_qos) self.create_subscription(Odometry, self.odom_topic, self._on_odom, best_effort_qos) self.create_subscription(LaserScan, self.scan_topic, self._on_scan, qos_profile_sensor_data) self.create_subscription(LaserScan, self.scan_topic, self._on_scan, reliable_qos) self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, reliable_qos) self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, best_effort_qos) self.create_subscription(TFMessage, "/tf", self._on_tf, qos_profile_sensor_data) self.create_subscription(TFMessage, "/tf_static", self._on_tf, reliable_qos) self._cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10) self._navigate_to_pose_client = ActionClient(self, NavigateToPose, self.navigate_to_pose_action) self.get_logger().info( f"SLAM bridge started: odom={self.odom_topic}, scan={self.scan_topic}, map={self.map_topic}, cmd_vel={self.cmd_vel_topic}, navigate={self.navigate_to_pose_action}" ) def _on_odom(self, msg: Odometry): with self._lock: self._odom = msg self._odom_count += 1 self._last_odom_wall_time = time.time() 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_map(self, msg: OccupancyGrid): with self._lock: self._map = msg self._map_count += 1 self._last_map_wall_time = time.time() 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): 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 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"} return { "ok": True, "accepted": True, "goal_id": bytes(goal_handle.goal_id.uuid).hex(), } def snapshot(self, include_map: bool = True) -> dict: with self._lock: odom = self._odom scan = self._scan grid = self._map tf_snapshot = dict(self._tf) pose = None 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, } 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"], } 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 grid is not None: origin = grid.info.origin map_payload = { "frame_id": grid.header.frame_id, "width": grid.info.width, "height": grid.info.height, "resolution": grid.info.resolution, "origin": { "x": origin.position.x, "y": origin.position.y, "z": origin.position.z, "yaw": yaw_from_quaternion(origin.orientation), }, "encoding": "binary-int8-map1", "t": grid.header.stamp.sec + grid.header.stamp.nanosec * 1e-9, } return { "ok": True, "topics": { "odom": self.odom_topic, "scan": self.scan_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": scan_payload, "map": map_payload, "tf": { "map_to_odom": tf_snapshot.get(("map", "odom")), }, "server_time": time.time(), } def map_binary(self) -> Optional[bytes]: with self._lock: grid = self._map if grid is None: return None header = struct.pack("<4sII", b"MAP1", grid.info.width, grid.info.height) data = array.array("b", grid.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.scan_topic, self.map_topic, self.cmd_vel_topic): topics[name] = types return { "configured": { "odom": self.odom_topic, "scan": self.scan_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), "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, "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_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_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("/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") slam_node.publish_cmd_vel(0.0, 0.0) 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.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 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"), )