|
|
|
|
@ -0,0 +1,198 @@
|
|
|
|
|
"""
|
|
|
|
|
FastAPI + ROS2 网关
|
|
|
|
|
接口：
|
|
|
|
|
GET /api/video_feed MJPEG 视频流（带 YOLO 框）
|
|
|
|
|
WS /ws/telemetry JSON 数据推送（10Hz）
|
|
|
|
|
POST /api/control/emergency_stop 急停
|
|
|
|
|
POST /api/move 速度控制
|
|
|
|
|
"""
|
|
|
|
|
import json
|
|
|
|
|
import asyncio
|
|
|
|
|
import threading
|
|
|
|
|
import queue
|
|
|
|
|
import time
|
|
|
|
|
from contextlib import asynccontextmanager
|
|
|
|
|
|
|
|
|
|
import cv2
|
|
|
|
|
import numpy as np
|
|
|
|
|
import rclpy
|
|
|
|
|
from rclpy.node import Node
|
|
|
|
|
from std_msgs.msg import String
|
|
|
|
|
from sensor_msgs.msg import Image
|
|
|
|
|
from geometry_msgs.msg import Twist
|
|
|
|
|
from cv_bridge import CvBridge
|
|
|
|
|
|
|
|
|
|
from fastapi import FastAPI, WebSocket, WebSocketDisconnect
|
|
|
|
|
from fastapi.middleware.cors import CORSMiddleware
|
|
|
|
|
from fastapi.responses import StreamingResponse
|
|
|
|
|
import uvicorn
|
|
|
|
|
|
|
|
|
|
_profile_queue: queue.Queue = queue.Queue(maxsize=50)
|
|
|
|
|
_status_queue: queue.Queue = queue.Queue(maxsize=20)
|
|
|
|
|
_audio_queue: queue.Queue = queue.Queue(maxsize=20)
|
|
|
|
|
_latest_frame = None # 最新标注帧（numpy BGR）
|
|
|
|
|
_frame_lock = threading.Lock()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class GatewayRosNode(Node):
|
|
|
|
|
def __init__(self):
|
|
|
|
|
super().__init__('dark_gateway')
|
|
|
|
|
self.bridge = CvBridge()
|
|
|
|
|
self.create_subscription(String, '/brain/enemy_profiles', self._on_profiles, 10)
|
|
|
|
|
self.create_subscription(String, '/brain/system_status', self._on_status, 10)
|
|
|
|
|
self.create_subscription(String, '/perception/audio_doa', self._on_audio, 10)
|
|
|
|
|
self.create_subscription(Image, '/perception/image_annotated', self._on_frame, 10)
|
|
|
|
|
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
|
|
|
|
|
self.heartbeat_pub = self.create_publisher(String, '/gateway/heartbeat', 10)
|
|
|
|
|
self.estop_pub = self.create_publisher(String, '/gateway/estop', 10)
|
|
|
|
|
|
|
|
|
|
def _on_profiles(self, msg):
|
|
|
|
|
if not _profile_queue.full():
|
|
|
|
|
_profile_queue.put_nowait(msg.data)
|
|
|
|
|
|
|
|
|
|
def _on_status(self, msg):
|
|
|
|
|
if not _status_queue.full():
|
|
|
|
|
_status_queue.put_nowait(msg.data)
|
|
|
|
|
|
|
|
|
|
def _on_audio(self, msg):
|
|
|
|
|
if not _audio_queue.full():
|
|
|
|
|
_audio_queue.put_nowait(msg.data)
|
|
|
|
|
|
|
|
|
|
def _on_frame(self, msg):
|
|
|
|
|
global _latest_frame
|
|
|
|
|
frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
|
|
|
|
|
with _frame_lock:
|
|
|
|
|
_latest_frame = frame
|
|
|
|
|
|
|
|
|
|
def publish_cmd_vel(self, vx, vy, vyaw):
|
|
|
|
|
t = Twist()
|
|
|
|
|
t.linear.x, t.linear.y, t.angular.z = vx, vy, vyaw
|
|
|
|
|
self.cmd_pub.publish(t)
|
|
|
|
|
|
|
|
|
|
def publish_heartbeat(self):
|
|
|
|
|
msg = String(); msg.data = 'ping'
|
|
|
|
|
self.heartbeat_pub.publish(msg)
|
|
|
|
|
|
|
|
|
|
def publish_estop(self):
|
|
|
|
|
msg = String(); msg.data = 'stop'
|
|
|
|
|
self.estop_pub.publish(msg)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
_ros_node: GatewayRosNode = None
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _ros_spin():
|
|
|
|
|
rclpy.init()
|
|
|
|
|
global _ros_node
|
|
|
|
|
_ros_node = GatewayRosNode()
|
|
|
|
|
rclpy.spin(_ros_node)
|
|
|
|
|
rclpy.shutdown()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@asynccontextmanager
|
|
|
|
|
async def lifespan(app: FastAPI):
|
|
|
|
|
t = threading.Thread(target=_ros_spin, daemon=True)
|
|
|
|
|
t.start()
|
|
|
|
|
yield
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
app = FastAPI(lifespan=lifespan)
|
|
|
|
|
app.add_middleware(CORSMiddleware, allow_origins=['*'], allow_methods=['*'], allow_headers=['*'])
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ── 视频流 ──────────────────────────────────────────────
|
|
|
|
|
def _mjpeg_generator():
|
|
|
|
|
while True:
|
|
|
|
|
with _frame_lock:
|
|
|
|
|
frame = _latest_frame.copy() if _latest_frame is not None else None
|
|
|
|
|
if frame is None:
|
|
|
|
|
time.sleep(0.05)
|
|
|
|
|
continue
|
|
|
|
|
ret, buf = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 80])
|
|
|
|
|
if not ret:
|
|
|
|
|
continue
|
|
|
|
|
yield (b'--frame\r\nContent-Type: image/jpeg\r\n\r\n' + buf.tobytes() + b'\r\n')
|
|
|
|
|
time.sleep(0.033) # ~30fps
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@app.get('/api/video_feed')
|
|
|
|
|
def video_feed():
|
|
|
|
|
return StreamingResponse(_mjpeg_generator(),
|
|
|
|
|
media_type='multipart/x-mixed-replace; boundary=frame')
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ── 控制接口 ────────────────────────────────────────────
|
|
|
|
|
@app.post('/api/move')
|
|
|
|
|
async def move(vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
|
|
|
|
|
if _ros_node:
|
|
|
|
|
_ros_node.publish_cmd_vel(vx, vy, vyaw)
|
|
|
|
|
return {'ok': True}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@app.post('/api/control/emergency_stop')
|
|
|
|
|
async def emergency_stop():
|
|
|
|
|
if _ros_node:
|
|
|
|
|
_ros_node.publish_estop()
|
|
|
|
|
return {'ok': True}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ── WebSocket 数据推送 ──────────────────────────────────
|
|
|
|
|
@app.websocket('/ws/telemetry')
|
|
|
|
|
async def ws_telemetry(ws: WebSocket):
|
|
|
|
|
await ws.accept()
|
|
|
|
|
try:
|
|
|
|
|
while True:
|
|
|
|
|
# 处理前端心跳
|
|
|
|
|
try:
|
|
|
|
|
data = await asyncio.wait_for(ws.receive_text(), timeout=0.05)
|
|
|
|
|
msg = json.loads(data)
|
|
|
|
|
if msg.get('cmd') == 'heartbeat' and _ros_node:
|
|
|
|
|
_ros_node.publish_heartbeat()
|
|
|
|
|
except asyncio.TimeoutError:
|
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
# 组装前端说明里的 JSON 格式
|
|
|
|
|
payload = {'timestamp': int(time.time()), 'visual_depth_fusion': None,
|
|
|
|
|
'audio_radar': None, 'system_status': None}
|
|
|
|
|
|
|
|
|
|
if not _profile_queue.empty():
|
|
|
|
|
profiles = json.loads(_profile_queue.get_nowait())
|
|
|
|
|
targets = []
|
|
|
|
|
for i, p in enumerate(profiles):
|
|
|
|
|
targets.append({
|
|
|
|
|
'id': i + 1,
|
|
|
|
|
'type': 'PERSON',
|
|
|
|
|
'confidence': p.get('confidence', 0),
|
|
|
|
|
'posture': p.get('posture', 'unknown').upper(),
|
|
|
|
|
'weapon_detected': p.get('armed', False),
|
|
|
|
|
'vl_analysis': p.get('vl_analysis', ''),
|
|
|
|
|
'distance_z_meters': p.get('z', 0),
|
|
|
|
|
'estimated_height_cm': round(p.get('height', 0) * 100),
|
|
|
|
|
'relative_coords': {'x': p.get('x', 0), 'y': p.get('y', 0), 'z': p.get('z', 0)},
|
|
|
|
|
})
|
|
|
|
|
payload['visual_depth_fusion'] = {'target_detected': len(targets) > 0, 'targets': targets}
|
|
|
|
|
|
|
|
|
|
if not _audio_queue.empty():
|
|
|
|
|
audio = json.loads(_audio_queue.get_nowait())
|
|
|
|
|
payload['audio_radar'] = {
|
|
|
|
|
'sound_detected': audio.get('confidence', 0) > 0.3,
|
|
|
|
|
'direction_angle': audio.get('angle'),
|
|
|
|
|
'intensity_db': audio.get('sound_level', 0),
|
|
|
|
|
'threat_level': 'HIGH' if audio.get('confidence', 0) > 0.7 else 'LOW',
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if not _status_queue.empty():
|
|
|
|
|
status = json.loads(_status_queue.get_nowait())
|
|
|
|
|
payload['system_status'] = {
|
|
|
|
|
'mode': 'OFFLINE' if status.get('offline_mode') else 'AUTONOMOUS',
|
|
|
|
|
'network': 'LOST' if status.get('offline_mode') else 'GOOD',
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
await ws.send_text(json.dumps(payload))
|
|
|
|
|
await asyncio.sleep(0.1) # 10Hz
|
|
|
|
|
except WebSocketDisconnect:
|
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
|
uvicorn.run(app, host='0.0.0.0', port=8000)
|
