커밋.

resources/autoware-ros-bridge/Dockerfile

@@ -0,0 +1,23 @@
+FROM ghcr.io/autowarefoundation/autoware-universe:humble-20240301-cuda
+
+WORKDIR /ros_bridge
+
+RUN curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+    -o /usr/share/keyrings/ros-archive-keyring.gpg \
+ && printf "deb [arch=%s signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu %s main\n" \
+    "$(dpkg --print-architecture)" "$(. /etc/os-release && echo "$UBUNTU_CODENAME")" \
+    > /etc/apt/sources.list.d/ros2.list \
+ && apt-get update \
+ && apt-get install -y --no-install-recommends \
+    ros-humble-autoware-adapi-v1-msgs \
+    ros-humble-autoware-planning-msgs \
+    ros-humble-autoware-vehicle-msgs \
+    ros-humble-nav2-msgs \
+ && rm -rf /var/lib/apt/lists/*
+
+COPY requirements.txt /ros_bridge/requirements.txt
+RUN pip3 install --no-cache-dir -r /ros_bridge/requirements.txt
+
+COPY bridge.py /ros_bridge/bridge.py
+
+CMD bash -c "source /opt/ros/humble/setup.bash && if [ -f /autoware_ws/install/setup.bash ]; then source /autoware_ws/install/setup.bash; fi && python3 /ros_bridge/bridge.py"

resources/autoware-ros-bridge/requirements.txt

@@ -0,0 +1,3 @@
+fastapi>=0.110.0
+uvicorn[standard]>=0.29.0
+pydantic>=2.0.0

resources/nav2-ros-bridge/Dockerfile

@@ -0,0 +1,16 @@
+FROM ros:humble-ros-base
+
+WORKDIR /ros_bridge
+
+RUN apt-get update \
+ && apt-get install -y --no-install-recommends \
+    python3-pip \
+    ros-humble-nav2-msgs \
+ && rm -rf /var/lib/apt/lists/*
+
+COPY requirements.txt /ros_bridge/requirements.txt
+RUN pip3 install --no-cache-dir -r /ros_bridge/requirements.txt
+
+COPY bridge.py /ros_bridge/bridge.py
+
+CMD bash -c "source /opt/ros/humble/setup.bash && python3 /ros_bridge/bridge.py"

resources/nav2-ros-bridge/requirements.txt

@@ -0,0 +1,3 @@
+fastapi>=0.110.0
+uvicorn[standard]>=0.29.0
+pydantic>=2.0.0

resources/slam-ros-bridge/Dockerfile

@@ -0,0 +1,13 @@
+FROM ros:humble-ros-base
+
+WORKDIR /app
+
+COPY requirements.txt /app/requirements.txt
+COPY bridge.py /app/bridge.py
+
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends python3-pip ros-humble-nav2-msgs && \
+    pip3 install --no-cache-dir -r /app/requirements.txt && \
+    rm -rf /var/lib/apt/lists/*
+
+CMD bash -c "source /opt/ros/humble/setup.bash && python3 /app/bridge.py"

resources/slam-ros-bridge/bridge.py

@@ -0,0 +1,477 @@
+#!/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"),
+    )

resources/slam-ros-bridge/requirements.txt

@@ -0,0 +1,3 @@
+fastapi>=0.110.0
+uvicorn[standard]>=0.29.0
+pydantic>=2.0.0