1642 lines
54 KiB
Python
1642 lines
54 KiB
Python
#!/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",
|
|
)
|