精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 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。
This commit is contained in:
keita
+359
-26
@@ -1,30 +1,193 @@
|
||||
"""ground_station/app.py —— 地面站页面（Flask，临时测试用）。
|
||||
|
||||
预定流程 C 阶段：地面坐标输入 + 状态可视化配套页面。
|
||||
预定流程 C 阶段：地面坐标输入 + 任务/吊舱控制 + 状态可视化配套页面。
|
||||
|
||||
功能：
|
||||
* GET / 地面站页面（坐标输入 + 状态面板 + 模拟视野）
|
||||
* POST /api/inject 接收 {lat, lon, alt}，UDP 发到吊舱桥接的 ground_inject 端口
|
||||
（默认 14555，GroundLink.run() 接收）
|
||||
* GET /api/state 返回最新任务状态（读任务日志：事件最后状态 + 遥测最后一行）
|
||||
* 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。
|
||||
与无人机 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, jsonify, render_template, request
|
||||
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，避免读全文）。"""
|
||||
@@ -80,41 +243,197 @@ def _latest_state(log_dir: str) -> dict:
|
||||
pass
|
||||
|
||||
return {"state": state, "telemetry": telemetry,
|
||||
"events": events[-20:], "log_dir": str(log_path)}
|
||||
"events": events[-40:], "log_dir": str(log_path)}
|
||||
|
||||
|
||||
def create_app(udp_port: int, log_dir: str) -> Flask:
|
||||
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 {}
|
||||
lat = data.get("lat")
|
||||
lon = data.get("lon")
|
||||
if lat is None or lon is None:
|
||||
return jsonify({"ok": False, "error": "需要 lat 和 lon"}), 400
|
||||
try:
|
||||
lat = float(lat)
|
||||
lon = float(lon)
|
||||
alt = float(data.get("alt", 0.0) or 0.0)
|
||||
except (TypeError, ValueError):
|
||||
return jsonify({"ok": False, "error": "lat/lon 必须是数值"}), 400
|
||||
alt = float(data.get("alt", 0.0) or 0.0)
|
||||
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
|
||||
|
||||
payload = json.dumps({"lat": lat, "lon": lon, "alt": alt})
|
||||
try:
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
|
||||
s.sendto(payload.encode("utf-8"), ("127.0.0.1", udp_port))
|
||||
_send_udp(target, {"lat": lat, "lon": lon, "alt": alt})
|
||||
except OSError as exc:
|
||||
return jsonify({"ok": False, "error": f"UDP 发送失败: {exc}"}), 500
|
||||
return jsonify({"ok": True, "sent": {"lat": lat, "lon": lon, "alt": alt}})
|
||||
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():
|
||||
return jsonify(_latest_state(log_dir))
|
||||
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
|
||||
|
||||
@@ -124,13 +443,27 @@ def main() -> int:
|
||||
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 监听）")
|
||||
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)
|
||||
print(f"[地面站] http://{args.host}:{args.port} | UDP 注入 → 127.0.0.1:{args.udp_port}"
|
||||
f" | 日志 → {args.log_dir}")
|
||||
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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user