keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
473 lines
19 KiB
Python
473 lines
19 KiB
Python
"""ground_station/app.py —— 地面站页面（Flask，临时测试用）。
|
||
|
||
预定流程 C 阶段：地面坐标输入 + 任务/吊舱控制 + 状态可视化配套页面。
|
||
|
||
功能：
|
||
* GET / 地面站页面（坐标注入 + 任务控制 + 吊舱控制 + 状态面板 + 模拟视野）
|
||
* GET /px4 PX4 信息子页面（实时遥测 + 任务日志解读分页）
|
||
* POST /api/inject 接收 {lat, lon, alt} 或局部 {north_m, east_m, alt}
|
||
（局部按注入时刻无人机位置换算为经纬度），UDP 发到 ground_inject
|
||
端口（默认 14555，GroundLink.run() 接收）；同时刷新持续注入内容
|
||
* POST /api/continuous 接收 {on: bool}，启停**服务端**持续注入（后台线程每 0.5s 重发）
|
||
* POST /api/command 接收 {cmd}（白名单：rtl/emergency/pod_*），同端口 UDP 下发，
|
||
由 TaskRunner 主循环 drain_commands() 分派
|
||
* GET /api/link 返回链路目标（坐标/命令发往的地址）+ 本机可选地址列表
|
||
* POST /api/link {"host":.., "port":..} 切换链路目标（默认 127.0.0.1:14555）
|
||
* GET /video_feed MJPEG 实时视频（服务端 RTSP 拉流转推；未配置 --rtsp-url 时 503）
|
||
* GET /api/state 返回最新任务状态（读任务日志：事件最后状态 + 遥测最后一行，
|
||
附 continuous 标志与 video 状态）
|
||
* GET /api/logs/telemetry 整段遥测 CSV → JSON（子页面图表用；只读）
|
||
* GET /api/logs/events 整段事件 JSONL → JSON（子页面解读用；只读）
|
||
|
||
与无人机 runner 解耦：本页面只做「发 UDP（坐标/命令）+ 读日志（只读）」，不侵入 runner。
|
||
持续注入放服务端的原因：浏览器会节流不可见标签页的 JS 定时器（实测 0.5s 被降到约
|
||
1 次/分钟，注入超龄致任务误返航），服务端线程不受页面可见性影响。
|
||
用法：
|
||
python ground_station/app.py --udp-port 14555 --log-dir logs
|
||
python ground_station/app.py --rtsp-url rtsp://192.168.144.119:8554/video
|
||
"""
|
||
|
||
import argparse
|
||
import csv
|
||
import json
|
||
import math
|
||
import os
|
||
import socket
|
||
import sys
|
||
import threading
|
||
from pathlib import Path
|
||
|
||
from flask import (Flask, Response, jsonify, render_template, request,
|
||
stream_with_context)
|
||
|
||
_REPO_ROOT = Path(__file__).resolve().parent.parent # 兼容「脚本直跑 / 包导入」双模式
|
||
if str(_REPO_ROOT) not in sys.path:
|
||
sys.path.insert(0, str(_REPO_ROOT))
|
||
|
||
from ground_station.video_source import RtspVideoSource # noqa: E402
|
||
|
||
_GROUND_UDP_PORT = 14555
|
||
_LOG_DIR = "logs"
|
||
|
||
# 日志解读接口的返回上限（超限返回最近 N 条，防内存/流量失控）
|
||
_LOG_MAX_ROWS = 60000
|
||
_LOG_MAX_EVENTS = 5000
|
||
|
||
# 允许页面下发的控制命令（与 task_runner/runner.py 的 _POD_COMMAND_METHODS 对齐）
|
||
_COMMAND_WHITELIST = frozenset({
|
||
"rtl", "emergency",
|
||
"pod_detect_on", "pod_auto_lock", "pod_stop_track",
|
||
"pod_detect_off", "pod_center",
|
||
})
|
||
|
||
|
||
class _LinkTarget:
|
||
"""链路目标：坐标/命令 UDP 发往的地址（页面可运行时切换）。"""
|
||
|
||
def __init__(self, host: str, port: int) -> None:
|
||
self.host = host
|
||
self.port = int(port)
|
||
|
||
|
||
def _local_ipv4_list() -> list:
|
||
"""本机 IPv4 地址列表（供页面下拉选择；解析失败返回空列表）。"""
|
||
ips = []
|
||
try:
|
||
for info in socket.getaddrinfo(socket.gethostname(), None,
|
||
socket.AF_INET):
|
||
ip = info[4][0]
|
||
if ip not in ips:
|
||
ips.append(ip)
|
||
except OSError:
|
||
pass
|
||
return ips
|
||
|
||
|
||
def _is_number(value) -> bool:
|
||
"""数值（排除 bool）。"""
|
||
return isinstance(value, (int, float)) and not isinstance(value, bool)
|
||
|
||
|
||
_DEG_LAT_M = 111320.0 # 局部平面近似的纬度米/度（与 task_runner_sitl 同口径）
|
||
|
||
|
||
def _offset_to_latlon(lat0: float, lon0: float, north_m: float,
|
||
east_m: float) -> tuple:
|
||
"""北东偏移（米）→ 经纬度（局部平面近似，小范围够用）。"""
|
||
lat = lat0 + north_m / _DEG_LAT_M
|
||
lon = lon0 + east_m / (_DEG_LAT_M * math.cos(math.radians(lat0)))
|
||
return lat, lon
|
||
|
||
|
||
def _session_start_position(log_dir: str):
|
||
"""本次任务运行段（遥测最后一段）的首个有效定位 ≈ 起飞点/home。
|
||
|
||
运行段按 t_s 回退切分；取最后一段里第一条 pos_valid=True 的经纬度。
|
||
无日志 / 无有效定位 → (None, None)。
|
||
"""
|
||
path = Path(log_dir) / "task_telemetry.csv"
|
||
try:
|
||
with open(path, encoding="utf-8", newline="") as f:
|
||
rows = list(csv.DictReader(f))
|
||
except OSError:
|
||
return None, None
|
||
start = 0
|
||
for i in range(1, len(rows)):
|
||
try:
|
||
if float(rows[i].get("t_s") or 0) < float(rows[i - 1].get("t_s") or 0):
|
||
start = i
|
||
except ValueError:
|
||
continue
|
||
for row in rows[start:]:
|
||
if (row.get("pos_valid") == "True" and row.get("latitude_deg")
|
||
and row.get("longitude_deg")):
|
||
try:
|
||
return float(row["latitude_deg"]), float(row["longitude_deg"])
|
||
except ValueError:
|
||
continue
|
||
return None, None
|
||
|
||
|
||
def _send_udp(target: _LinkTarget, payload: dict) -> None:
|
||
"""把 JSON 发到链路目标（坐标注入 / 持续注入 / 控制命令共用）。"""
|
||
data = json.dumps(payload, ensure_ascii=False)
|
||
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
|
||
s.sendto(data.encode("utf-8"), (target.host, target.port))
|
||
|
||
|
||
class _ContinuousInjector:
|
||
"""服务端持续注入：后台线程按固定周期重发最近一次坐标。
|
||
|
||
页面 JS 定时器在标签页不可见时被浏览器节流（实测 0.5s → 约 1 次/分钟，
|
||
注入超龄致任务误返航），故持续注入放服务端；手动「注入坐标」同步刷新内容。
|
||
"""
|
||
|
||
def __init__(self, target: _LinkTarget, interval_s: float = 0.5) -> None:
|
||
self._target = target
|
||
self._interval_s = max(0.05, float(interval_s))
|
||
self._lock = threading.Lock()
|
||
self._payload = None
|
||
self._stop = threading.Event()
|
||
self._thread = None
|
||
|
||
@property
|
||
def running(self) -> bool:
|
||
return self._thread is not None and self._thread.is_alive()
|
||
|
||
def update(self, payload: dict) -> None:
|
||
with self._lock:
|
||
self._payload = payload
|
||
|
||
def start(self) -> bool:
|
||
"""开始持续注入；尚无坐标（未手动注入过）→ False。"""
|
||
with self._lock:
|
||
if self._payload is None:
|
||
return False
|
||
if self.running:
|
||
return True
|
||
self._stop.clear()
|
||
self._thread = threading.Thread(target=self._loop,
|
||
name="continuous-inject", daemon=True)
|
||
self._thread.start()
|
||
return True
|
||
|
||
def stop(self) -> None:
|
||
self._stop.set()
|
||
thread, self._thread = self._thread, None
|
||
if thread is not None:
|
||
thread.join(timeout=2.0)
|
||
|
||
def _loop(self) -> None:
|
||
while not self._stop.is_set():
|
||
with self._lock:
|
||
payload = self._payload
|
||
if payload is not None:
|
||
try:
|
||
_send_udp(self._target, payload)
|
||
except OSError:
|
||
pass # 端口未监听等：静默重试
|
||
self._stop.wait(self._interval_s)
|
||
|
||
|
||
def _read_last_line(path: Path):
|
||
"""读文件最后一行（tail，避免读全文）。"""
|
||
try:
|
||
with open(path, "rb") as f:
|
||
f.seek(0, 2)
|
||
size = f.tell()
|
||
if size == 0:
|
||
return None
|
||
f.seek(max(0, size - 8192))
|
||
lines = f.read().decode("utf-8", errors="replace").splitlines()
|
||
return lines[-1] if lines else None
|
||
except OSError:
|
||
return None
|
||
|
||
|
||
def _latest_state(log_dir: str) -> dict:
|
||
"""从任务日志读出当前状态 + 最新遥测（只读，供 /api/state）。"""
|
||
log_path = Path(log_dir)
|
||
state = "unknown"
|
||
events = []
|
||
|
||
# 事件日志：找最后一个 state 事件（当前任务状态）
|
||
events_path = log_path / "task_events.jsonl"
|
||
if events_path.exists():
|
||
try:
|
||
with open(events_path, encoding="utf-8") as f:
|
||
for line in f:
|
||
line = line.strip()
|
||
if not line:
|
||
continue
|
||
try:
|
||
ev = json.loads(line)
|
||
except json.JSONDecodeError:
|
||
continue
|
||
events.append(ev)
|
||
if ev.get("type") == "state":
|
||
state = ev.get("to_state", state)
|
||
except OSError:
|
||
pass
|
||
|
||
# 遥测最后一行
|
||
telemetry = None
|
||
tel_path = log_path / "task_telemetry.csv"
|
||
last = _read_last_line(tel_path)
|
||
if last:
|
||
try:
|
||
with open(tel_path, encoding="utf-8") as f:
|
||
header = f.readline().strip().split(",")
|
||
values = last.split(",")
|
||
telemetry = dict(zip(header, values))
|
||
except OSError:
|
||
pass
|
||
|
||
return {"state": state, "telemetry": telemetry,
|
||
"events": events[-40:], "log_dir": str(log_path)}
|
||
|
||
|
||
def create_app(udp_port: int, log_dir: str,
|
||
udp_host: str = "127.0.0.1",
|
||
inject_interval_s: float = 0.5,
|
||
rtsp_url: str = "",
|
||
video_width: int = 960,
|
||
video_fps: float = 15.0) -> Flask:
|
||
app = Flask(__name__)
|
||
target = _LinkTarget(udp_host, udp_port)
|
||
injector = _ContinuousInjector(target, inject_interval_s)
|
||
video = None
|
||
if rtsp_url:
|
||
video = RtspVideoSource(rtsp_url, width=video_width, fps=video_fps)
|
||
video.start()
|
||
|
||
@app.route("/")
|
||
def index():
|
||
return render_template("index.html", udp_port=udp_port)
|
||
|
||
@app.route("/px4")
|
||
def px4_page():
|
||
return render_template("px4.html")
|
||
|
||
@app.route("/api/logs/telemetry")
|
||
def logs_telemetry():
|
||
"""整段遥测 CSV → JSON（fields + 行数组；前端负责分段/绘图）。只读。"""
|
||
path = Path(log_dir) / "task_telemetry.csv"
|
||
fields, rows = [], []
|
||
try:
|
||
with open(path, encoding="utf-8", newline="") as f:
|
||
for i, row in enumerate(csv.reader(f)):
|
||
if i == 0:
|
||
fields = row
|
||
elif row:
|
||
rows.append(row)
|
||
except OSError:
|
||
pass
|
||
total = len(rows)
|
||
if total > _LOG_MAX_ROWS:
|
||
rows = rows[-_LOG_MAX_ROWS:]
|
||
return jsonify({"fields": fields, "rows": rows,
|
||
"total": total, "returned": len(rows)})
|
||
|
||
@app.route("/api/logs/events")
|
||
def logs_events():
|
||
"""整段事件 JSONL → JSON 列表（前端负责分段/解读）。只读。"""
|
||
path = Path(log_dir) / "task_events.jsonl"
|
||
events = []
|
||
try:
|
||
with open(path, encoding="utf-8") as f:
|
||
for line in f:
|
||
line = line.strip()
|
||
if not line:
|
||
continue
|
||
try:
|
||
events.append(json.loads(line))
|
||
except json.JSONDecodeError:
|
||
continue
|
||
except OSError:
|
||
pass
|
||
total = len(events)
|
||
if total > _LOG_MAX_EVENTS:
|
||
events = events[-_LOG_MAX_EVENTS:]
|
||
return jsonify({"events": events, "total": total})
|
||
|
||
@app.route("/api/inject", methods=["POST"])
|
||
def inject():
|
||
data = request.get_json(silent=True) or {}
|
||
try:
|
||
alt = float(data.get("alt", 0.0) or 0.0)
|
||
except (TypeError, ValueError):
|
||
return jsonify({"ok": False, "error": "alt 必须是数值"}), 400
|
||
|
||
local = None
|
||
if "north_m" in data or "east_m" in data:
|
||
# 局部坐标：按选定原点换算为经纬度
|
||
# current（默认）= 注入时刻无人机位置；home = 本次运行段起始位置（≈起飞点）
|
||
north, east = data.get("north_m"), data.get("east_m")
|
||
if not (_is_number(north) and _is_number(east)):
|
||
return jsonify({"ok": False,
|
||
"error": "north_m/east_m 必须是数值"}), 400
|
||
origin_mode = data.get("origin", "current")
|
||
if origin_mode not in ("current", "home"):
|
||
return jsonify({"ok": False,
|
||
"error": "origin 必须是 current 或 home"}), 400
|
||
if origin_mode == "home":
|
||
lat0, lon0 = _session_start_position(log_dir)
|
||
if lat0 is None:
|
||
return jsonify({"ok": False,
|
||
"error": "无本次运行的有效起始位置（遥测缺失/"
|
||
"定位无效），无法按起飞点换算"}), 400
|
||
else:
|
||
tel = (_latest_state(log_dir).get("telemetry") or {})
|
||
lat0, lon0 = tel.get("latitude_deg"), tel.get("longitude_deg")
|
||
if tel.get("pos_valid") != "True" or not lat0 or not lon0:
|
||
return jsonify({"ok": False,
|
||
"error": "无人机位置无效（无遥测/无 GPS），"
|
||
"无法换算局部坐标"}), 400
|
||
lat0, lon0 = float(lat0), float(lon0)
|
||
lat, lon = _offset_to_latlon(lat0, lon0, float(north), float(east))
|
||
local = {"north_m": float(north), "east_m": float(east),
|
||
"origin": origin_mode,
|
||
"origin_lat": lat0, "origin_lon": lon0}
|
||
else:
|
||
lat, lon = data.get("lat"), data.get("lon")
|
||
if lat is None or lon is None:
|
||
return jsonify({"ok": False,
|
||
"error": "需要 lat/lon 或 north_m/east_m"}), 400
|
||
try:
|
||
lat = float(lat)
|
||
lon = float(lon)
|
||
except (TypeError, ValueError):
|
||
return jsonify({"ok": False,
|
||
"error": "lat/lon 必须是数值"}), 400
|
||
|
||
try:
|
||
_send_udp(target, {"lat": lat, "lon": lon, "alt": alt})
|
||
except OSError as exc:
|
||
return jsonify({"ok": False, "error": f"UDP 发送失败: {exc}"}), 500
|
||
injector.update({"lat": lat, "lon": lon, "alt": alt}) # 刷新持续注入内容
|
||
resp = {"ok": True, "sent": {"lat": lat, "lon": lon, "alt": alt}}
|
||
if local is not None:
|
||
resp["local"] = local
|
||
return jsonify(resp)
|
||
|
||
@app.route("/api/continuous", methods=["POST"])
|
||
def continuous():
|
||
data = request.get_json(silent=True) or {}
|
||
on = data.get("on")
|
||
if not isinstance(on, bool):
|
||
return jsonify({"ok": False, "error": "on 必须是布尔值"}), 400
|
||
if on and not injector.start():
|
||
return jsonify({"ok": False,
|
||
"error": "尚未注入过坐标，请先点「注入坐标」"}), 400
|
||
if not on:
|
||
injector.stop()
|
||
return jsonify({"ok": True, "continuous": injector.running})
|
||
|
||
@app.route("/api/command", methods=["POST"])
|
||
def command():
|
||
data = request.get_json(silent=True) or {}
|
||
cmd = data.get("cmd")
|
||
if not isinstance(cmd, str) or cmd not in _COMMAND_WHITELIST:
|
||
return jsonify({"ok": False, "error": f"未知命令: {cmd!r}"}), 400
|
||
try:
|
||
_send_udp(target, {"cmd": cmd})
|
||
except OSError as exc:
|
||
return jsonify({"ok": False, "error": f"UDP 发送失败: {exc}"}), 500
|
||
return jsonify({"ok": True, "sent": {"cmd": cmd}})
|
||
|
||
@app.route("/api/link")
|
||
def link_get():
|
||
options = [{"label": "本地回环（仿真）", "host": "127.0.0.1"}]
|
||
for ip in _local_ipv4_list():
|
||
if not ip.startswith("127."):
|
||
options.append({"label": f"本机网卡 {ip}", "host": ip})
|
||
return jsonify({"host": target.host, "port": target.port,
|
||
"options": options})
|
||
|
||
@app.route("/api/link", methods=["POST"])
|
||
def link_set():
|
||
data = request.get_json(silent=True) or {}
|
||
host = str(data.get("host") or "").strip()
|
||
port = data.get("port", target.port)
|
||
if not host or len(host) > 64 or any(c.isspace() for c in host):
|
||
return jsonify({"ok": False, "error": "host 不合法"}), 400
|
||
try:
|
||
port = int(port)
|
||
except (TypeError, ValueError):
|
||
return jsonify({"ok": False, "error": "port 必须是整数"}), 400
|
||
if not (1 <= port <= 65535):
|
||
return jsonify({"ok": False, "error": "port 超出范围"}), 400
|
||
target.host, target.port = host, port
|
||
return jsonify({"ok": True, "host": target.host, "port": target.port})
|
||
|
||
@app.route("/api/state")
|
||
def state():
|
||
j = _latest_state(log_dir)
|
||
j["continuous"] = injector.running
|
||
j["video"] = {"enabled": video is not None,
|
||
"online": bool(video and video.online)}
|
||
return jsonify(j)
|
||
|
||
@app.route("/video_feed")
|
||
def video_feed():
|
||
if video is None:
|
||
return "未配置视频源（启动参数 --rtsp-url）", 503
|
||
return Response(stream_with_context(video.mjpeg_stream()),
|
||
mimetype="multipart/x-mixed-replace; boundary=frame")
|
||
|
||
return app
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(description="地面站页面（Flask）")
|
||
ap.add_argument("--host", default="127.0.0.1")
|
||
ap.add_argument("--port", type=int, default=5000, help="Flask 监听端口")
|
||
ap.add_argument("--udp-port", type=int, default=_GROUND_UDP_PORT,
|
||
help="坐标/命令注入 UDP 端口（GroundLink 监听）")
|
||
ap.add_argument("--udp-host", default="127.0.0.1",
|
||
help="坐标/命令注入目标地址（默认本机；真机可填机载/伴侣机 IP，"
|
||
"页面也可运行时切换）")
|
||
ap.add_argument("--log-dir", default=_LOG_DIR, help="任务日志目录")
|
||
ap.add_argument("--rtsp-url", default="",
|
||
help="吊舱 RTSP 视频地址（如 rtsp://192.168.144.119:8554/video）；"
|
||
"缺省则视频面板关闭")
|
||
ap.add_argument("--video-width", type=int, default=960,
|
||
help="MJPEG 输出最大宽度（默认 960）")
|
||
ap.add_argument("--video-fps", type=float, default=15.0,
|
||
help="MJPEG 编码帧率上限（默认 15）")
|
||
args = ap.parse_args()
|
||
|
||
app = create_app(args.udp_port, args.log_dir, udp_host=args.udp_host,
|
||
rtsp_url=args.rtsp_url,
|
||
video_width=args.video_width, video_fps=args.video_fps)
|
||
video_note = f" | 视频 ← {args.rtsp_url}" if args.rtsp_url else " | 视频未配置"
|
||
print(f"[地面站] http://{args.host}:{args.port} | UDP 注入 → "
|
||
f"{args.udp_host}:{args.udp_port}"
|
||
f" | 日志 → {args.log_dir}{video_note}")
|
||
app.run(host=args.host, port=args.port, debug=False)
|
||
return 0
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|