커밋.
This commit is contained in:
chyoo
@@ -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"
|
||||
Binary file not shown.
@@ -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"),
|
||||
)
|
||||
@@ -0,0 +1,3 @@
|
||||
fastapi>=0.110.0
|
||||
uvicorn[standard]>=0.29.0
|
||||
pydantic>=2.0.0
|
||||
Reference in New Issue
Block a user