#!/usr/bin/env python3 """ ROS2 bridge for Autoware web backend. - /get_behavior_path : 긴 예상 경로 - /get_trajectory : 짧은 제어 궤적 - /get_kinematic_state : 실제 차량 상태 """ import asyncio import logging import math import os import threading import time import traceback from contextlib import asynccontextmanager from typing import Optional logging.basicConfig(level=logging.INFO) import rclpy from rclpy.node import Node from rclpy.executors import SingleThreadedExecutor from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy from rclpy.action import ActionClient from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from nav_msgs.msg import OccupancyGrid, Odometry, Path as NavPath from sensor_msgs.msg import PointCloud2 from visualization_msgs.msg import MarkerArray import sensor_msgs_py.point_cloud2 as point_cloud2 try: from nav2_msgs.action import NavigateToPose HAS_NAV2 = True except ImportError: NavigateToPose = None HAS_NAV2 = False try: from autoware_adapi_v1_msgs.srv import SetRoutePoints, ClearRoute HAS_ROUTING = True except ImportError: SetRoutePoints = None ClearRoute = None HAS_ROUTING = False try: from autoware_auto_planning_msgs.msg import Trajectory, Path HAS_PLANNING = True except ImportError: try: from autoware_planning_msgs.msg import Trajectory, Path HAS_PLANNING = True except ImportError: Trajectory = object Path = object HAS_PLANNING = False try: from autoware_adapi_v1_msgs.srv import ChangeOperationMode from autoware_adapi_v1_msgs.msg import OperationModeState USE_OPERATION_MODE = True HAS_OPERATION = True except ImportError: try: from tier4_external_api_msgs.srv import Engage as EngageSrv USE_OPERATION_MODE = False HAS_OPERATION = True except ImportError: ChangeOperationMode = None OperationModeState = None EngageSrv = None USE_OPERATION_MODE = False HAS_OPERATION = False try: from autoware_auto_vehicle_msgs.msg import SteeringReport HAS_STEERING = True except ImportError: try: from autoware_vehicle_msgs.msg import SteeringReport HAS_STEERING = True except ImportError: HAS_STEERING = False try: from autoware_auto_vehicle_msgs.msg import TurnIndicatorsReport HAS_TURN = True except ImportError: try: from autoware_vehicle_msgs.msg import TurnIndicatorsReport HAS_TURN = True except ImportError: HAS_TURN = False from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect from fastapi.middleware.cors import CORSMiddleware from pydantic import BaseModel import uvicorn # Coordinate conversion constants (must match backend .env CENTER_X/CENTER_Y) _CENTER_X = float(os.environ.get("CENTER_X", "3763.558837890625")) _CENTER_Y = float(os.environ.get("CENTER_Y", "73675.99609375")) _MAP_POINT_STRIDE = max(1, int(os.environ.get("MAP_POINT_STRIDE", "8"))) _MAP_POINT_LIMIT = max(1000, int(os.environ.get("MAP_POINT_LIMIT", "80000"))) _ROBOT_STACK = os.environ.get("ROBOT_STACK", "autoware").strip().lower() _AUTOWARE_ROUTE_PREFIX = os.environ.get( "AUTOWARE_ROUTE_PREFIX", "/api/routing", ).rstrip("/") _AUTOWARE_ROUTE_INPUT = os.environ.get("AUTOWARE_ROUTE_INPUT", "rviz_topic").strip().lower() if _ROBOT_STACK not in ("autoware", "nav2"): logging.warning("Unsupported ROBOT_STACK=%s; falling back to autoware", _ROBOT_STACK) _ROBOT_STACK = "autoware" if _AUTOWARE_ROUTE_INPUT not in ("rviz_topic", "service"): logging.warning("Unsupported AUTOWARE_ROUTE_INPUT=%s; falling back to rviz_topic", _AUTOWARE_ROUTE_INPUT) _AUTOWARE_ROUTE_INPUT = "rviz_topic" def _map_to_three(mx: float, my: float, mz: float) -> dict: return {"x": mx - _CENTER_X, "y": mz, "z": -(my - _CENTER_Y)} def _center_from_bounds(x_min: float, x_max: float, y_min: float, y_max: float) -> dict: if not all(math.isfinite(v) for v in (x_min, x_max, y_min, y_max)): return {"x": _CENTER_X, "y": _CENTER_Y} return {"x": (x_min + x_max) / 2.0, "y": (y_min + y_max) / 2.0} def _quat_to_three_heading(qx: float, qy: float, qz: float, qw: float) -> float: """ROS2 quaternion → Three.js heading in radians (0=north, CW+).""" yaw = math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz)) return math.pi / 2.0 - yaw _svc_lock = threading.Lock() class SubscriberNode(Node): def __init__(self): super().__init__(f"{_ROBOT_STACK}_web_bridge") best_effort_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE, ) reliable_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE, ) map_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, ) self._pointcloud_map_payload = None self._pointcloud_map_revision = 0 self._pointcloud_map_lock = threading.Lock() self._vector_map_payload = None self._vector_map_revision = 0 self._vector_map_lock = threading.Lock() self._occupancy_map_payload = None self._occupancy_map_revision = 0 self._occupancy_map_lock = threading.Lock() if _ROBOT_STACK == "autoware": self.create_subscription( PointCloud2, "/map/pointcloud_map", self._on_pointcloud_map, map_qos, ) self.create_subscription( MarkerArray, "/map/vector_map_marker", self._on_vector_map_marker, map_qos, ) else: self.create_subscription( OccupancyGrid, "/map", self._on_occupancy_map, map_qos, ) self.initial_pose_pub = self.create_publisher( PoseWithCovarianceStamped, "/initialpose", 10, ) self.rviz_rough_goal_pub = self.create_publisher( PoseStamped, "/rviz/routing/rough_goal", 10, ) # 짧은 제어 궤적 / 긴 예상 경로 self._trajectory = None self._trajectory_event = threading.Event() self._behavior_path = None self._behavior_path_event = threading.Event() self._nav2_plan = None self._nav2_plan_revision = 0 self._nav2_plan_lock = threading.Lock() if _ROBOT_STACK == "autoware" and HAS_PLANNING: self.create_subscription( Trajectory, "/planning/scenario_planning/trajectory", self._on_trajectory, reliable_qos, ) self.create_subscription( Path, "/planning/scenario_planning/lane_driving/behavior_planning/path", self._on_behavior_path, reliable_qos, ) elif _ROBOT_STACK == "nav2": self.create_subscription( NavPath, "/plan", self._on_nav2_plan, reliable_qos, ) # 차량 현재 상태 self._kinematic_state: Optional[Odometry] = None self._kinematic_lock = threading.Lock() if _ROBOT_STACK == "autoware": self.create_subscription( Odometry, "/localization/kinematic_state", self._on_kinematic_state, reliable_qos, ) else: self.create_subscription( PoseWithCovarianceStamped, "/amcl_pose", self._on_amcl_pose, reliable_qos, ) self.create_subscription( Odometry, "/odom", self._on_odom, reliable_qos, ) self._nav2_goal_active = False self._pending_nav2_goal = None # 조향각 self._steering_angle: float = 0.0 self._steering_lock = threading.Lock() if _ROBOT_STACK == "autoware" and HAS_STEERING: self.create_subscription( SteeringReport, "/vehicle/status/steering_status", self._on_steering, best_effort_qos, ) # operation mode (1=STOP, 2=AUTONOMOUS, 3=LOCAL, 4=REMOTE) self._operation_mode: int = 0 self._operation_mode_lock = threading.Lock() if _ROBOT_STACK == "autoware" and USE_OPERATION_MODE and HAS_OPERATION: self.create_subscription( OperationModeState, "/api/operation_mode/state", self._on_operation_mode, reliable_qos, ) # 방향지시등 (1=off, 2=left, 3=right) self._turn_indicator: int = 1 self._turn_lock = threading.Lock() if _ROBOT_STACK == "autoware" and HAS_TURN: self.create_subscription( TurnIndicatorsReport, "/vehicle/status/turn_indicators_status", self._on_turn, best_effort_qos, ) # 서비스 클라이언트 self._svc_lock = threading.Lock() self._cli_clear_route = None self._cli_set_route = None self._cli_enable_ctrl = None self._cli_autonomous = None self._cli_stop = None self._cli_engage = None self._nav2_action_client = None self._nav2_goal_handle = None if _ROBOT_STACK == "autoware" and HAS_ROUTING: self._cli_clear_route = self.create_client( ClearRoute, f"{_AUTOWARE_ROUTE_PREFIX}/clear_route", ) self._cli_set_route = self.create_client( SetRoutePoints, f"{_AUTOWARE_ROUTE_PREFIX}/set_route_points", ) if _ROBOT_STACK == "autoware" and USE_OPERATION_MODE and HAS_OPERATION: self._cli_enable_ctrl = self.create_client( ChangeOperationMode, "/api/operation_mode/enable_autoware_control", ) self._cli_autonomous = self.create_client( ChangeOperationMode, "/api/operation_mode/change_to_autonomous", ) self._cli_stop = self.create_client( ChangeOperationMode, "/api/operation_mode/change_to_stop", ) elif _ROBOT_STACK == "autoware" and HAS_OPERATION: self._cli_engage = self.create_client( EngageSrv, "/api/operation/engage", ) elif _ROBOT_STACK == "nav2" and HAS_NAV2: self._nav2_action_client = ActionClient( self, NavigateToPose, "/navigate_to_pose", ) self.get_logger().info(f"SubscriberNode started stack={_ROBOT_STACK}") def _on_trajectory(self, msg): if len(msg.points) > 0: self._trajectory = msg self._trajectory_event.set() def _on_behavior_path(self, msg): if len(msg.points) > 0: self._behavior_path = msg self._behavior_path_event.set() def _on_nav2_plan(self, msg: NavPath): if len(msg.poses) > 0: with self._nav2_plan_lock: self._nav2_plan = msg self._nav2_plan_revision += 1 self._behavior_path = msg self._trajectory = msg self._behavior_path_event.set() self._trajectory_event.set() def get_nav2_plan_snapshot(self): with self._nav2_plan_lock: if self._nav2_plan is None: return self._nav2_plan_revision, [] return self._nav2_plan_revision, self._nav_path_to_points(self._nav2_plan) def _on_kinematic_state(self, msg: Odometry): with self._kinematic_lock: self._kinematic_state = msg def _on_amcl_pose(self, msg: PoseWithCovarianceStamped): odom = Odometry() odom.header = msg.header odom.pose = msg.pose with self._kinematic_lock: current = self._kinematic_state if current is not None: odom.twist = current.twist self._kinematic_state = odom def _on_odom(self, msg: Odometry): with self._kinematic_lock: if self._kinematic_state is None: self._kinematic_state = msg else: self._kinematic_state.twist = msg.twist def _on_steering(self, msg): with self._steering_lock: self._steering_angle = msg.steering_tire_angle def get_steering_angle(self) -> float: with self._steering_lock: return self._steering_angle def _on_turn(self, msg): with self._turn_lock: self._turn_indicator = msg.report def get_turn_indicator(self) -> int: with self._turn_lock: return self._turn_indicator def _on_operation_mode(self, msg): with self._operation_mode_lock: self._operation_mode = msg.mode def get_operation_mode(self) -> int: if _ROBOT_STACK == "nav2": return 2 if getattr(self, "_nav2_goal_active", False) else 1 with self._operation_mode_lock: return self._operation_mode def is_autonomous(self) -> bool: if _ROBOT_STACK == "nav2": return True return self.get_operation_mode() == 2 def set_initial_pose(self, x, y, z, qx, qy, qz, qw): msg = PoseWithCovarianceStamped() msg.header.frame_id = "map" msg.header.stamp = self.get_clock().now().to_msg() msg.pose.pose.position.x = x msg.pose.pose.position.y = y msg.pose.pose.position.z = z msg.pose.pose.orientation.x = qx msg.pose.pose.orientation.y = qy msg.pose.pose.orientation.z = qz msg.pose.pose.orientation.w = qw msg.pose.covariance[0] = 0.25 msg.pose.covariance[7] = 0.25 msg.pose.covariance[35] = 0.06853892326654787 self.initial_pose_pub.publish(msg) def publish_rviz_rough_goal(self, x, y, z, qx, qy, qz, qw): msg = PoseStamped() msg.header.frame_id = "map" msg.header.stamp = self.get_clock().now().to_msg() msg.pose.position.x = x msg.pose.position.y = y msg.pose.position.z = z msg.pose.orientation.x = qx msg.pose.orientation.y = qy msg.pose.orientation.z = qz msg.pose.orientation.w = qw self.rviz_rough_goal_pub.publish(msg) def get_trajectory(self, timeout=30.0): if _ROBOT_STACK == "nav2": self._trajectory_event.wait(timeout=min(timeout, 1.0)) path = self._trajectory if path is None and self._pending_nav2_goal is not None: return self._pending_nav2_goal_to_points() if path is None: raise RuntimeError("Nav2 plan not received and no pending goal exists") return self._nav_path_to_points(path) if not HAS_PLANNING: raise RuntimeError("Planning message packages are not available in this bridge image") self._trajectory = None self._trajectory_event.clear() if not self._trajectory_event.wait(timeout=timeout): raise RuntimeError("Trajectory not received within timeout") traj = self._trajectory return self._planning_points_to_dicts(traj.points) def _planning_points_to_dicts(self, points): return [ { "x": p.pose.position.x, "y": p.pose.position.y, "z": p.pose.position.z, "t": round(i * 0.1, 3), } for i, p in enumerate(points) ] def _points_reach_goal(self, points, goal_x, goal_y, max_distance=12.0): if not points: return False max_distance_sq = max_distance * max_distance tail = points[-80:] if len(points) > 80 else points return any( ((p["x"] - goal_x) * (p["x"] - goal_x) + (p["y"] - goal_y) * (p["y"] - goal_y)) <= max_distance_sq for p in tail ) def wait_behavior_path_for_goal(self, goal_x, goal_y, timeout=30.0): if _ROBOT_STACK == "nav2": return self.get_behavior_path(timeout=timeout) if not HAS_PLANNING: raise RuntimeError("Planning message packages are not available in this bridge image") deadline = time.time() + timeout last_distance = None while time.time() < deadline: remaining = max(0.1, deadline - time.time()) self._behavior_path_event.clear() if not self._behavior_path_event.wait(timeout=min(1.0, remaining)): continue path = self._behavior_path if path is None: continue points = self._planning_points_to_dicts(path.points) if points: last_distance = min(math.hypot(point["x"] - goal_x, point["y"] - goal_y) for point in points[-80:]) if self._points_reach_goal(points, goal_x, goal_y): return points detail = f"; nearest tail distance={last_distance:.2f}m" if last_distance is not None else "" raise RuntimeError(f"Behavior path for requested goal not received within timeout{detail}") def wait_trajectory_for_goal(self, goal_x, goal_y, timeout=30.0): if _ROBOT_STACK == "nav2": return self.get_trajectory(timeout=timeout) if not HAS_PLANNING: raise RuntimeError("Planning message packages are not available in this bridge image") deadline = time.time() + timeout last_points = None while time.time() < deadline: remaining = max(0.1, deadline - time.time()) self._trajectory_event.clear() if not self._trajectory_event.wait(timeout=min(1.0, remaining)): continue traj = self._trajectory if traj is None: continue points = self._planning_points_to_dicts(traj.points) if points: last_points = points if self._points_reach_goal(points, goal_x, goal_y, max_distance=25.0): return points if last_points: logging.warning("Trajectory for requested goal was not received; returning latest trajectory") return last_points raise RuntimeError("Trajectory not received within timeout") def get_behavior_path(self, timeout=30.0): if _ROBOT_STACK == "nav2": self._behavior_path_event.wait(timeout=min(timeout, 1.0)) path = self._behavior_path if path is None and self._pending_nav2_goal is not None: return self._pending_nav2_goal_to_points() if path is None: raise RuntimeError("Nav2 plan not received and no pending goal exists") return self._nav_path_to_points(path) if not HAS_PLANNING: raise RuntimeError("Planning message packages are not available in this bridge image") self._behavior_path = None self._behavior_path_event.clear() if not self._behavior_path_event.wait(timeout=timeout): raise RuntimeError("Behavior path not received within timeout") path = self._behavior_path return self._planning_points_to_dicts(path.points) def get_kinematic_state(self): with self._kinematic_lock: return self._kinematic_state def _nav_path_to_points(self, path: NavPath): return [ { "x": p.pose.position.x, "y": p.pose.position.y, "z": p.pose.position.z, "t": round(i * 0.1, 3), "v": 0.0, } for i, p in enumerate(path.poses) ] def _pending_nav2_goal_to_points(self): goal = self._pending_nav2_goal if goal is None: return [] state = self.get_kinematic_state() start_pos = state.pose.pose.position if state is not None else None if start_pos is None: return [{ "x": goal["x"], "y": goal["y"], "z": goal["z"], "t": 0.0, "v": 0.0, }] return [ { "x": start_pos.x, "y": start_pos.y, "z": start_pos.z, "t": 0.0, "v": 0.0, }, { "x": goal["x"], "y": goal["y"], "z": goal["z"], "t": 0.1, "v": 0.0, }, ] def _on_pointcloud_map(self, msg: PointCloud2): try: payload = self._pointcloud_to_payload(msg) with self._pointcloud_map_lock: self._pointcloud_map_payload = payload self._pointcloud_map_revision += 1 self.get_logger().info( f"pointcloud map cached: {payload['count']} / {payload['source_count']} points" ) except Exception as e: self.get_logger().error(f"pointcloud map conversion failed: {e}") def _on_vector_map_marker(self, msg: MarkerArray): try: payload = self._marker_array_to_payload(msg) with self._vector_map_lock: self._vector_map_payload = payload self._vector_map_revision += 1 # A vector map update means the active map may have changed. Drop any # old latched pointcloud until a fresh /map/pointcloud_map arrives. with self._pointcloud_map_lock: self._pointcloud_map_payload = None self._pointcloud_map_revision += 1 self.get_logger().info( f"vector map cached: {payload['count']} markers; pointcloud cache cleared" ) except Exception as e: self.get_logger().error(f"vector map conversion failed: {e}") def _on_occupancy_map(self, msg: OccupancyGrid): try: payload = self._occupancy_grid_to_payload(msg) with self._occupancy_map_lock: self._occupancy_map_payload = payload self._occupancy_map_revision += 1 self.get_logger().info( f"occupancy map cached: {payload['width']}x{payload['height']} res={payload['resolution']}" ) except Exception as e: self.get_logger().error(f"occupancy map conversion failed: {e}") def _pointcloud_to_payload(self, msg: PointCloud2): points = [] source_count = int(msg.width * msg.height) stride = max(1, _MAP_POINT_STRIDE) limit = max(1000, _MAP_POINT_LIMIT) x_min = math.inf x_max = -math.inf y_min = math.inf y_max = -math.inf z_min = math.inf z_max = -math.inf for i, p in enumerate(point_cloud2.read_points( msg, field_names=("x", "y", "z"), skip_nans=True, )): if i % stride != 0: continue try: mx, my, mz = float(p[0]), float(p[1]), float(p[2]) except Exception: mx, my, mz = float(p["x"]), float(p["y"]), float(p["z"]) points.append([round(mx, 3), round(my, 3), round(mz, 3)]) x_min = min(x_min, mx) x_max = max(x_max, mx) y_min = min(y_min, my) y_max = max(y_max, my) z_min = min(z_min, mz) z_max = max(z_max, mz) if len(points) >= limit: break if not math.isfinite(z_min): x_min = x_max = _CENTER_X y_min = y_max = _CENTER_Y z_min = z_max = 0.0 center = _center_from_bounds(x_min, x_max, y_min, y_max) return { "type": "pointcloud", "topic": "/map/pointcloud_map", "coordinate_frame": "map", "frame_id": msg.header.frame_id, "source_count": source_count, "count": len(points), "stride": stride, "center": {"x": round(center["x"], 3), "y": round(center["y"], 3)}, "bounds": { "x_min": round(x_min, 3), "x_max": round(x_max, 3), "y_min": round(y_min, 3), "y_max": round(y_max, 3), "z_min": round(z_min, 3), "z_max": round(z_max, 3), }, "z_min": round(z_min, 3), "z_max": round(z_max, 3), "points": points, } def _occupancy_grid_to_payload(self, msg: OccupancyGrid): width = int(msg.info.width) height = int(msg.info.height) resolution = float(msg.info.resolution) origin = msg.info.origin.position x_min = float(origin.x) y_min = float(origin.y) x_max = x_min + width * resolution y_max = y_min + height * resolution z = float(origin.z) center = _center_from_bounds(x_min, x_max, y_min, y_max) data = [int(v) for v in msg.data] return { "type": "occupancy_grid", "topic": "/map", "coordinate_frame": "map", "frame_id": msg.header.frame_id, "width": width, "height": height, "resolution": resolution, "origin": { "x": round(x_min, 3), "y": round(y_min, 3), "z": round(z, 3), }, "center": {"x": round(center["x"], 3), "y": round(center["y"], 3)}, "bounds": { "x_min": round(x_min, 3), "x_max": round(x_max, 3), "y_min": round(y_min, 3), "y_max": round(y_max, 3), "z_min": round(z, 3), "z_max": round(z, 3), }, "data": data, } def _marker_array_to_payload(self, msg: MarkerArray): markers = [] x_min = math.inf x_max = -math.inf y_min = math.inf y_max = -math.inf z_min = math.inf z_max = -math.inf for marker in msg.markers: if marker.action == 2: # DELETE markers.append({ "ns": marker.ns, "id": marker.id, "action": marker.action, "points": [], }) continue points = [] for p in marker.points: mx, my, mz = float(p.x), float(p.y), float(p.z) points.append([round(mx, 3), round(my, 3), round(mz, 3)]) x_min = min(x_min, mx) x_max = max(x_max, mx) y_min = min(y_min, my) y_max = max(y_max, my) z_min = min(z_min, mz) z_max = max(z_max, mz) pose = marker.pose color = marker.color markers.append({ "ns": marker.ns, "id": marker.id, "type": marker.type, "action": marker.action, "frame_id": marker.header.frame_id, "pose": { "position": { "x": pose.position.x, "y": pose.position.y, "z": pose.position.z, }, "orientation": { "x": pose.orientation.x, "y": pose.orientation.y, "z": pose.orientation.z, "w": pose.orientation.w, }, }, "scale": { "x": marker.scale.x, "y": marker.scale.y, "z": marker.scale.z, }, "color": [color.r, color.g, color.b, color.a], "points": points, }) if not math.isfinite(x_min): x_min = x_max = _CENTER_X y_min = y_max = _CENTER_Y z_min = z_max = 0.0 center = _center_from_bounds(x_min, x_max, y_min, y_max) return { "type": "vector_map", "topic": "/map/vector_map_marker", "coordinate_frame": "map", "center": {"x": round(center["x"], 3), "y": round(center["y"], 3)}, "bounds": { "x_min": round(x_min, 3), "x_max": round(x_max, 3), "y_min": round(y_min, 3), "y_max": round(y_max, 3), "z_min": round(z_min, 3), "z_max": round(z_max, 3), }, "count": len(markers), "markers": markers, } def get_map_snapshot(self): with self._pointcloud_map_lock: pointcloud = self._pointcloud_map_payload pointcloud_revision = self._pointcloud_map_revision with self._vector_map_lock: vector = self._vector_map_payload vector_revision = self._vector_map_revision with self._occupancy_map_lock: occupancy = self._occupancy_map_payload occupancy_revision = self._occupancy_map_revision return { "pointcloud": pointcloud, "pointcloud_revision": pointcloud_revision, "vector": vector, "vector_revision": vector_revision, "occupancy": occupancy, "occupancy_revision": occupancy_revision, } def call_client(self, client, request, timeout=15.0): if client is None: raise RuntimeError("ROS service client is not available for this stack") with self._svc_lock: if not client.wait_for_service(timeout_sec=5.0): raise RuntimeError(f"{client.srv_name} not available") future = client.call_async(request) start = time.time() while not future.done(): if time.time() - start > timeout: raise RuntimeError( f"{client.srv_name} call timed out ({timeout}s)" ) time.sleep(0.05) return future.result() def send_nav2_goal(self, x, y, z, qx, qy, qz, qw, timeout=10.0): if not HAS_NAV2 or self._nav2_action_client is None: raise RuntimeError("nav2_msgs NavigateToPose action is not available in this bridge image") if not self._nav2_action_client.wait_for_server(timeout_sec=timeout): raise RuntimeError("/navigate_to_pose action server not available") goal = NavigateToPose.Goal() goal.pose = PoseStamped() goal.pose.header.frame_id = "map" goal.pose.header.stamp = self.get_clock().now().to_msg() goal.pose.pose.position.x = x goal.pose.pose.position.y = y goal.pose.pose.position.z = z goal.pose.pose.orientation.x = qx goal.pose.pose.orientation.y = qy goal.pose.pose.orientation.z = qz goal.pose.pose.orientation.w = qw self._behavior_path = None self._trajectory = None self._behavior_path_event.clear() self._trajectory_event.clear() future = self._nav2_action_client.send_goal_async(goal) start = time.time() while not future.done(): if time.time() - start > timeout: raise RuntimeError("NavigateToPose goal send timed out") time.sleep(0.05) goal_handle = future.result() if not goal_handle or not goal_handle.accepted: return {"success": False, "code": 1, "message": "NavigateToPose goal rejected"} self._nav2_goal_handle = goal_handle self._nav2_goal_active = True return {"success": True, "code": 0, "message": "NavigateToPose goal accepted"} def set_pending_nav2_goal(self, x, y, z, qx, qy, qz, qw): self._pending_nav2_goal = { "x": x, "y": y, "z": z, "qx": qx, "qy": qy, "qz": qz, "qw": qw, } self._behavior_path = None self._trajectory = None self._behavior_path_event.clear() self._trajectory_event.clear() return {"success": True, "code": 0, "message": "Nav2 goal staged"} def clear_planning_cache(self): self._behavior_path = None self._trajectory = None self._behavior_path_event.clear() self._trajectory_event.clear() def start_pending_nav2_goal(self): goal = self._pending_nav2_goal if goal is None: raise RuntimeError("No pending Nav2 goal") return self.send_nav2_goal( goal["x"], goal["y"], goal["z"], goal["qx"], goal["qy"], goal["qz"], goal["qw"], ) def cancel_nav2_goal(self, timeout=5.0): self._nav2_goal_active = False goal_handle = getattr(self, "_nav2_goal_handle", None) if goal_handle is None: return {"ok": True} future = goal_handle.cancel_goal_async() start = time.time() while not future.done(): if time.time() - start > timeout: raise RuntimeError("NavigateToPose cancel timed out") time.sleep(0.05) self._nav2_goal_handle = None return {"ok": True} sub_node: Optional[SubscriberNode] = None _spin_thread: Optional[threading.Thread] = None @asynccontextmanager async def lifespan(app: FastAPI): global sub_node, _spin_thread rclpy.init() sub_node = SubscriberNode() def spin(): executor = SingleThreadedExecutor() executor.add_node(sub_node) try: executor.spin() except Exception: pass _spin_thread = threading.Thread(target=spin, daemon=True) _spin_thread.start() yield if rclpy.ok(): rclpy.shutdown() app = FastAPI(title="Autoware ROS2 Bridge", lifespan=lifespan) app.add_middleware( CORSMiddleware, allow_origins=["*"], allow_methods=["*"], allow_headers=["*"], ) class PosePayload(BaseModel): x: float y: float z: float qx: float qy: float qz: float qw: float class EngagePayload(BaseModel): engage: bool = True @app.get("/health") def health(): return { "ok": True, "stack": _ROBOT_STACK, "route_prefix": _AUTOWARE_ROUTE_PREFIX, "route_input": _AUTOWARE_ROUTE_INPUT, } @app.get("/stack") def get_stack(): return { "stack": _ROBOT_STACK, "route_prefix": _AUTOWARE_ROUTE_PREFIX, "route_input": _AUTOWARE_ROUTE_INPUT, "supported": ["autoware", "nav2"], } @app.get("/operation_mode") def get_operation_mode(): mode = sub_node.get_operation_mode() if sub_node else 0 return {"mode": mode, "autonomous": mode == 2} @app.websocket("/ws/operation_mode") async def ws_operation_mode(websocket: WebSocket): await websocket.accept() last_mode = -1 try: while True: mode = sub_node.get_operation_mode() if sub_node else 0 if mode != last_mode: last_mode = mode await websocket.send_json({"mode": mode, "autonomous": mode == 2}) await asyncio.sleep(0.5) except (WebSocketDisconnect, Exception): pass @app.get("/map/status") def map_status(): snap = sub_node.get_map_snapshot() if sub_node else {} return { "pointcloud": bool(snap.get("pointcloud")), "pointcloud_revision": snap.get("pointcloud_revision", 0), "vector": bool(snap.get("vector")), "vector_revision": snap.get("vector_revision", 0), "occupancy": bool(snap.get("occupancy")), "occupancy_revision": snap.get("occupancy_revision", 0), } @app.websocket("/ws/map") async def ws_map(websocket: WebSocket, stack: Optional[str] = None): await websocket.accept() if stack and stack.lower() != _ROBOT_STACK: await websocket.send_json({ "type": "stack_mismatch", "requested": stack.lower(), "active": _ROBOT_STACK, "message": "Restart bridge with ROBOT_STACK to change ROS adapter", }) last_pointcloud_revision = -1 last_vector_revision = -1 last_occupancy_revision = -1 try: while True: if sub_node: snap = sub_node.get_map_snapshot() pointcloud_revision = snap["pointcloud_revision"] vector_revision = snap["vector_revision"] occupancy_revision = snap["occupancy_revision"] if snap["vector"] and vector_revision != last_vector_revision: last_vector_revision = vector_revision await websocket.send_json({ "revision": vector_revision, **snap["vector"], }) if snap["pointcloud"] and pointcloud_revision != last_pointcloud_revision: last_pointcloud_revision = pointcloud_revision await websocket.send_json({ "revision": pointcloud_revision, **snap["pointcloud"], }) if snap["occupancy"] and occupancy_revision != last_occupancy_revision: last_occupancy_revision = occupancy_revision await websocket.send_json({ "revision": occupancy_revision, **snap["occupancy"], }) await asyncio.sleep(0.25) except (WebSocketDisconnect, Exception): pass @app.websocket("/ws/nav2") async def ws_nav2(websocket: WebSocket): await websocket.accept() if _ROBOT_STACK != "nav2": await websocket.send_json({ "type": "stack_mismatch", "requested": "nav2", "active": _ROBOT_STACK, "message": "Restart bridge with ROBOT_STACK=nav2 to stream Nav2 paths", }) await websocket.close() return last_revision = -1 try: while True: if sub_node: revision, poses = sub_node.get_nav2_plan_snapshot() if poses and revision != last_revision: last_revision = revision await websocket.send_json({ "type": "nav2_path", "revision": revision, "poses": poses, }) await asyncio.sleep(0.25) except (WebSocketDisconnect, Exception): pass @app.post("/set_initial_pose") def set_initial_pose(payload: PosePayload): try: sub_node.set_initial_pose( payload.x, payload.y, payload.z, payload.qx, payload.qy, payload.qz, payload.qw, ) return {"ok": True} except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=500, detail=str(e)) @app.post("/set_route") def set_route(payload: PosePayload): try: if _ROBOT_STACK == "nav2": result = sub_node.set_pending_nav2_goal( payload.x, payload.y, payload.z, payload.qx, payload.qy, payload.qz, payload.qw, ) return { "ok": True, "success": result["success"], "code": result["code"], "message": result["message"], } if not HAS_ROUTING or SetRoutePoints is None: raise HTTPException( status_code=503, detail="Autoware routing message package is not available in this bridge image", ) if _AUTOWARE_ROUTE_INPUT == "rviz_topic": sub_node.clear_planning_cache() sub_node.publish_rviz_rough_goal( payload.x, payload.y, payload.z, payload.qx, payload.qy, payload.qz, payload.qw, ) logging.info( f"set_route via /rviz/routing/rough_goal: x={payload.x:.2f} y={payload.y:.2f}" ) return { "ok": True, "success": True, "code": 0, "message": "published /rviz/routing/rough_goal", } try: sub_node.call_client( sub_node._cli_clear_route, ClearRoute.Request(), timeout=5.0, ) logging.info("Route cleared") except Exception as e: logging.warning(f"clear_route skipped: {e}") from geometry_msgs.msg import Pose req = SetRoutePoints.Request() req.header.frame_id = "map" req.goal = Pose() req.goal.position.x = payload.x req.goal.position.y = payload.y req.goal.position.z = payload.z req.goal.orientation.x = payload.qx req.goal.orientation.y = payload.qy req.goal.orientation.z = payload.qz req.goal.orientation.w = payload.qw logging.info(f"set_route: x={payload.x:.2f} y={payload.y:.2f}") result = None for attempt in range(20): try: result = sub_node.call_client( sub_node._cli_set_route, req, timeout=15.0, ) logging.info( f"set_route attempt={attempt + 1} " f"success={result.status.success} " f"code={result.status.code} " f"msg={result.status.message}" ) if result.status.success or result.status.code != 50001: break logging.warning("Routing not ready (50001), retrying in 3s...") except Exception as e: logging.warning( f"set_route attempt={attempt + 1} failed: {e}, retrying in 3s..." ) time.sleep(3) if result is None: raise HTTPException( status_code=503, detail="Routing service not available after retries", ) return { "ok": True, "success": result.status.success, "code": result.status.code, "message": result.status.message, } except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=500, detail=str(e)) @app.post("/plan") def plan(payload: PosePayload): try: route = set_route(payload) if not route.get("ok"): return route if _ROBOT_STACK == "nav2": path_points = sub_node.get_behavior_path(timeout=5.0) return {"ok": True, "planned": path_points, "plannedTrajectory": path_points} path_points = sub_node.wait_behavior_path_for_goal(payload.x, payload.y, timeout=30.0) trajectory_points = sub_node.wait_trajectory_for_goal(payload.x, payload.y, timeout=10.0) return { "ok": True, "planned": path_points, "plannedTrajectory": trajectory_points, } except HTTPException: raise except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=504, detail=str(e)) @app.post("/disengage") def disengage(): try: if _ROBOT_STACK == "nav2": return sub_node.cancel_nav2_goal() if not HAS_OPERATION: logging.warning("disengage skipped: operation message package is not available") return {"ok": True, "skipped": "operation message package is not available"} try: if USE_OPERATION_MODE: sub_node.call_client( sub_node._cli_stop, sub_node._cli_stop.srv_type.Request(), timeout=5.0, ) else: req = sub_node._cli_engage.srv_type.Request() req.engage = False sub_node.call_client( sub_node._cli_engage, req, timeout=5.0, ) except Exception as e: logging.warning(f"change_to_stop skipped: {e}") try: sub_node.call_client( sub_node._cli_clear_route, ClearRoute.Request(), timeout=5.0, ) except Exception as e: logging.warning(f"clear_route skipped: {e}") logging.info("Disengaged and route cleared") return {"ok": True} except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=500, detail=str(e)) @app.post("/reset") def reset(): """ 오토웨어 소프트 리셋: 1. disengage (stop mode) 2. clear route 3. 현재 kinematic state로 /initialpose 재발행 → 플래닝/컨트롤 모듈 재초기화 """ try: if _ROBOT_STACK == "nav2": try: sub_node.cancel_nav2_goal() except Exception as e: logging.warning(f"nav2 cancel skipped: {e}") state = sub_node.get_kinematic_state() if state is not None: pos = state.pose.pose.position ori = state.pose.pose.orientation sub_node.set_initial_pose( pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w, ) return {"ok": True} has_stop_client = ( (USE_OPERATION_MODE and sub_node._cli_stop is not None) or ((not USE_OPERATION_MODE) and sub_node._cli_engage is not None) ) # 1. stop if has_stop_client: try: if USE_OPERATION_MODE: sub_node.call_client( sub_node._cli_stop, sub_node._cli_stop.srv_type.Request(), timeout=5.0, ) else: req = sub_node._cli_engage.srv_type.Request() req.engage = False sub_node.call_client(sub_node._cli_engage, req, timeout=5.0) except Exception as e: logging.warning(f"stop skipped: {e}") else: logging.warning("stop skipped: operation client is not available") # 2. clear route if HAS_ROUTING and ClearRoute is not None and sub_node._cli_clear_route is not None: try: sub_node.call_client(sub_node._cli_clear_route, ClearRoute.Request(), timeout=5.0) except Exception as e: logging.warning(f"clear_route skipped: {e}") else: logging.warning("clear_route skipped: routing client is not available") # 3. /initialpose 재발행 (현재 위치 기준) state = sub_node.get_kinematic_state() if state is not None: pos = state.pose.pose.position ori = state.pose.pose.orientation sub_node.set_initial_pose( pos.x, pos.y, pos.z, ori.x, ori.y, ori.z, ori.w, ) logging.info(f"reset: republished initialpose at x={pos.x:.2f} y={pos.y:.2f}") # localization 재수렴 대기 (최대 5초) deadline = time.time() + 5.0 while time.time() < deadline: s = sub_node.get_kinematic_state() if s is not None: dx = s.pose.pose.position.x - pos.x dy = s.pose.pose.position.y - pos.y if math.sqrt(dx*dx + dy*dy) < 2.0: break time.sleep(0.2) else: logging.warning("reset: no kinematic state available, skipping initialpose") return {"ok": True} except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=500, detail=str(e)) @app.post("/clear_route") def clear_route(): try: if _ROBOT_STACK == "nav2": return sub_node.cancel_nav2_goal() if not HAS_ROUTING or ClearRoute is None or sub_node._cli_clear_route is None: logging.warning("clear_route skipped: routing message package is not available") return {"ok": True, "skipped": "routing message package is not available"} sub_node.call_client( sub_node._cli_clear_route, ClearRoute.Request(), timeout=5.0, ) logging.info("Route cleared explicitly") return {"ok": True} except Exception as e: logging.warning(f"clear_route skipped: {e}") return {"ok": True, "skipped": str(e)} @app.get("/get_trajectory") def get_trajectory(): try: points = sub_node.get_trajectory(timeout=30.0) return {"points": points} except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=504, detail=str(e)) @app.get("/get_behavior_path") def get_behavior_path(): try: points = sub_node.get_behavior_path(timeout=30.0) return {"points": points} except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=504, detail=str(e)) @app.post("/engage") def engage(payload: EngagePayload): try: if _ROBOT_STACK == "nav2": result = sub_node.start_pending_nav2_goal() return { "ok": True, "success": result["success"], "code": result["code"], "message": result["message"], } if not HAS_OPERATION: raise HTTPException( status_code=503, detail="Autoware operation message package is not available in this bridge image", ) if USE_OPERATION_MODE: try: r1 = sub_node.call_client( sub_node._cli_enable_ctrl, sub_node._cli_enable_ctrl.srv_type.Request(), timeout=5.0, ) logging.info( f"enable_autoware_control: " f"success={r1.status.success} " f"msg={r1.status.message!r}" ) except Exception as e: logging.warning(f"enable_autoware_control skipped: {e}") result = None try: result = sub_node.call_client( sub_node._cli_autonomous, sub_node._cli_autonomous.srv_type.Request(), timeout=3.0, ) logging.info( f"change_to_autonomous: " f"success={result.status.success} " f"msg={result.status.message!r} " f"code={result.status.code}" ) except Exception as e: logging.warning(f"change_to_autonomous response skipped: {e}") deadline = time.time() + 8.0 while time.time() < deadline: if sub_node.get_operation_mode() == 2: return {"ok": True} time.sleep(0.1) if result is not None and not result.status.success: raise HTTPException( status_code=503, detail=( f"change_to_autonomous failed: " f"[{result.status.code}] {result.status.message}" ), ) raise HTTPException( status_code=503, detail="change_to_autonomous requested but operation mode did not become autonomous", ) else: req = sub_node._cli_engage.srv_type.Request() req.engage = True sub_node.call_client( sub_node._cli_engage, req, timeout=10.0, ) return {"ok": True} except HTTPException: raise except Exception as e: logging.error(traceback.format_exc()) raise HTTPException(status_code=500, detail=str(e)) @app.get("/get_kinematic_state") def get_kinematic_state(): state = sub_node.get_kinematic_state() if state is None: raise HTTPException(status_code=503, detail="No kinematic state yet") pos = state.pose.pose.position vel = state.twist.twist.linear return { "x": pos.x, "y": pos.y, "z": pos.z, "vx": vel.x, "vy": vel.y, "vz": vel.z, "t": state.header.stamp.sec + state.header.stamp.nanosec * 1e-9, } @app.websocket("/ws/kinematic") async def ws_kinematic(websocket: WebSocket, stack: Optional[str] = None): """Stream kinematic state at ~10 Hz, only while autonomous.""" await websocket.accept() if stack and stack.lower() != _ROBOT_STACK: await websocket.send_json({ "type": "stack_mismatch", "requested": stack.lower(), "active": _ROBOT_STACK, "message": "Restart bridge with ROBOT_STACK to change ROS adapter", }) try: while True: if sub_node and sub_node.is_autonomous(): state = sub_node.get_kinematic_state() if state is not None: pos = state.pose.pose.position ori = state.pose.pose.orientation vel = state.twist.twist.linear three = _map_to_three(pos.x, pos.y, pos.z) heading = _quat_to_three_heading(ori.x, ori.y, ori.z, ori.w) steer_rad = sub_node.get_steering_angle() turn = sub_node.get_turn_indicator() await websocket.send_json({ "x": round(three["x"], 3), "y": round(three["y"], 3), "z": round(three["z"], 3), "map_x": round(pos.x, 3), "map_y": round(pos.y, 3), "map_z": round(pos.z, 3), "heading": round(heading, 5), "speed": round(vel.x, 3), "steer": round(math.degrees(steer_rad), 2), "turn": turn, "t": state.header.stamp.sec + state.header.stamp.nanosec * 1e-9, }) await asyncio.sleep(0.1) except (WebSocketDisconnect, Exception): pass if __name__ == "__main__": uvicorn.run( app, host="0.0.0.0", port=8765, log_level="warning", )