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。
259 lines
11 KiB
Python
259 lines
11 KiB
Python
#!/usr/bin/env python3
|
||
"""task_runner_real.py —— 真机任务编排入口（真实吊舱串口 + 地面站 UDP 注入 + PX4）。
|
||
|
||
与 task_runner_sitl.py（仿真）的差异：
|
||
* 吊舱 = PodSerial（真实串口表2-2 遥测），非 VirtualPod；
|
||
* 地面坐标 = GroundLink.run()（UDP 接收地面站 JSON），非 inject() 直接注入；
|
||
* 吊舱目标识别/锁定 = PodController（首次进入引导阶段发 0x07 识别 + 0x0F 自动锁定，
|
||
RTL 前发 0x0E 停止 + 0x08 识别关；**只发一次**，重入不会重发打断跟踪）；
|
||
* PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540；
|
||
* 收尾 = RTL 返航（默认）或 `--land` 就地降落；
|
||
* SIGINT/SIGTERM 优雅停机：先请状态机转 RTL，再退出（不把飞机留在 offboard 上）。
|
||
|
||
任务链（与 SITL 一致）：WAIT→ARM_TAKEOFF→GOTO(飞向参考点)→SEARCH(原地扫视)→TRACK(缓慢靠近)→RTL
|
||
|
||
前置（真机联调增量：纯软单测 → 串口回环 → 仅吊舱 → 仅飞控 → 地面联调）：
|
||
1. 填 utils/config/pod_bridge.json 的 pod_serial.port / px4.connection_string；
|
||
2. 吊舱上电、串口连通（run_bridge.py --dry-run 已验证遥测）；
|
||
3. 地面站往 ground_inject.port（默认 14555）**持续**发 JSON 坐标
|
||
{"lat":..,"lon":..,"alt":..}（每 0.5~1s 一次）；停发会导致坐标超龄 → 返航。
|
||
|
||
用法：
|
||
python task_runner_real.py
|
||
python task_runner_real.py --config utils/config/pod_bridge.json --target-type 1
|
||
python task_runner_real.py --port /dev/ttyAMA0 --land
|
||
"""
|
||
|
||
import argparse
|
||
import asyncio
|
||
import logging
|
||
import signal
|
||
import sys
|
||
import time
|
||
from datetime import datetime
|
||
from pathlib import Path
|
||
|
||
_ROOT = Path(__file__).resolve().parent
|
||
_UTILS = _ROOT / "utils"
|
||
for _p in (_UTILS, _ROOT):
|
||
if str(_p) not in sys.path:
|
||
sys.path.insert(0, str(_p))
|
||
|
||
from mavsdk import System
|
||
|
||
from pod_bridge.bridge import PodBridge
|
||
from pod_bridge.config import load_from_json
|
||
from pod_bridge.ground_link import GroundLink
|
||
from pod_bridge.pod_serial import PodSerial
|
||
from pod_bridge.px4_gateway import Px4Gateway
|
||
from task_runner.log import TaskLogConfig, TaskLogger
|
||
from task_runner.pod_control import PodController
|
||
from task_runner.return_home import ReturnConfig
|
||
from task_runner.runner import TaskRunner
|
||
from task_runner.state_machine import TaskStateConfig, TaskStateMachine
|
||
from sitl_support import now_ms, print_flight_mode, set_telemetry_rates, wait_until_ready
|
||
|
||
logger = logging.getLogger("task_real")
|
||
|
||
CONFIG_PATH = _UTILS / "config" / "pod_bridge.json"
|
||
|
||
TASK_TIMEOUT_S = 300.0 # 全流程总超时（真机放宽）
|
||
|
||
|
||
def build_sm_config() -> TaskStateConfig:
|
||
"""真机状态机参数。⚠ 全部为占位值，须台架标定后回填，标定前不得实飞。"""
|
||
return TaskStateConfig(
|
||
takeoff_altitude_m=5.0,
|
||
arrival_radius_m=30.0, # 到参考点半径 → 转搜索（待标定）
|
||
goto_timeout_s=180.0,
|
||
search_timeout_s=20.0, # 搜索 20s 未发现目标 → 返航
|
||
track_timeout_s=20.0, # 追踪/靠近段上限 → 返航（无距离，判不出"已到目标"）
|
||
center_threshold_deg=3.0, # 仅日志/诊断
|
||
sweep_rate_dps=15.0, # 搜索扫视角速度（度/秒，待标定）
|
||
sweep_reverse_s=4.0, # 每 4s 换向
|
||
loop_frequency_hz=20.0,
|
||
)
|
||
|
||
|
||
def build_return_config() -> ReturnConfig:
|
||
"""自定义返航参数：回起飞点 → 恢复初始航向 → 垂直降落（不额外爬高）。
|
||
|
||
⚠ 速度为占位值，须按机体/场地标定。
|
||
"""
|
||
return ReturnConfig(
|
||
enabled=True,
|
||
speed_mps=2.0, # 转场水平速度
|
||
arrival_radius_m=3.0, # 距起飞点该半径内视为到位
|
||
timeout_s=120.0, # 转场超时 → PX4 RTL 兜底
|
||
yaw_kp=1.0,
|
||
yaw_rate_limit_dps=20.0,
|
||
align_kp=1.0,
|
||
align_rate_limit_dps=15.0,
|
||
yaw_align_tolerance_deg=5.0,
|
||
yaw_align_timeout_s=15.0,
|
||
alt_kp=0.5,
|
||
alt_limit_mps=0.5,
|
||
)
|
||
|
||
|
||
def _sync_staleness_with_injection(cfg) -> None:
|
||
"""把「坐标时效」口径统一到 `ground_inject.timeout_ms`。
|
||
|
||
⚠ 这是 SITL 实测暴露的一个真机必踩的坑：`guidance.target_stale_ms` 默认 1000ms，
|
||
而地面站按 0.5~1s 上报 → 报文间隔稍有抖动（无线链路/页面节流）就会让引导律判
|
||
「目标超龄 → 零速刹车」，表现为飞机在路上走走停停；而任务层要等
|
||
`ground_inject.timeout_ms`（默认 5000ms）才判坐标失效。
|
||
SITL 入口是硬编码 5000ms 绕过的，真机这里改为**派生**，两个门限只有一个来源。
|
||
"""
|
||
inject_ms = int(cfg.ground_inject.timeout_ms)
|
||
if cfg.guidance.target_stale_ms < inject_ms:
|
||
logger.info("[Task] guidance.target_stale_ms %d → %d（与 ground_inject 对齐）",
|
||
cfg.guidance.target_stale_ms, inject_ms)
|
||
cfg.guidance.target_stale_ms = inject_ms
|
||
|
||
|
||
def _install_signal_handlers(stop: asyncio.Event, runner: TaskRunner) -> None:
|
||
"""SIGINT/SIGTERM → 请求返航并退出主循环（真机在树莓派上被 kill 也要安全收尾）。
|
||
|
||
只置位不抛异常：让 `runner.run()` 走正常的 RTL 分支（停机+返航+落地+上锁），
|
||
而不是让进程带着 offboard 速度指令消失。
|
||
"""
|
||
loop = asyncio.get_running_loop()
|
||
|
||
def _handler(sig_name: str) -> None:
|
||
logger.warning("[Task] 收到 %s：请求返航并退出", sig_name)
|
||
runner.request_rtl()
|
||
stop.set()
|
||
|
||
for name in ("SIGINT", "SIGTERM"):
|
||
sig = getattr(signal, name, None)
|
||
if sig is None:
|
||
continue
|
||
try:
|
||
loop.add_signal_handler(sig, _handler, name)
|
||
except (NotImplementedError, RuntimeError, ValueError):
|
||
# Windows / 非主线程不支持：退化为 signal.signal（可能不生效）
|
||
try:
|
||
signal.signal(sig, lambda *_a, _n=name: _handler(_n))
|
||
except (ValueError, OSError):
|
||
pass
|
||
|
||
|
||
async def run(args) -> int:
|
||
cfg = load_from_json(args.config)
|
||
if args.port:
|
||
cfg.pod_serial.port = args.port # 先应用覆盖，再校验（否则 --port 救不了空配置）
|
||
if not cfg.pod_serial.port:
|
||
print("! 未配置吊舱串口：在 pod_bridge.json 填 pod_serial.port，或用 --port 覆盖")
|
||
return 2
|
||
cfg.ground_inject.enabled = True # 地面注入必须开（真机无激光，表4 永不出）
|
||
_sync_staleness_with_injection(cfg) # 坐标时效口径统一（SITL 实测踩过的坑）
|
||
sm_cfg = build_sm_config()
|
||
return_cfg = build_return_config()
|
||
|
||
drone = System()
|
||
await drone.connect(system_address=args.conn or cfg.px4.connection_string)
|
||
try:
|
||
await wait_until_ready(drone)
|
||
except RuntimeError as e:
|
||
print(f"! {e}")
|
||
return 1
|
||
await set_telemetry_rates(drone)
|
||
|
||
# 装配：真吊舱串口 + 地面站 UDP 注入 + 吊舱识别/锁定控制
|
||
px4 = Px4Gateway(drone)
|
||
ground = GroundLink(cfg.ground_inject, now_ms_fn=now_ms)
|
||
pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate)
|
||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms,
|
||
ground_target_fn=lambda: ground.latest())
|
||
sm = TaskStateMachine(sm_cfg)
|
||
pod_control = PodController(bridge, target_type=args.target_type)
|
||
log_dir = args.log_dir or f"logs/real_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
|
||
task_log = TaskLogger(TaskLogConfig(dir=log_dir), now_ms_fn=now_ms)
|
||
runner = TaskRunner(cfg, sm_cfg, px4, bridge, sm, ground, drone,
|
||
now_ms_fn=now_ms, logger=task_log, pod_control=pod_control,
|
||
land_instead_of_rtl=args.land, return_cfg=return_cfg)
|
||
|
||
stop = asyncio.Event()
|
||
_install_signal_handlers(stop, runner)
|
||
ground_task = asyncio.ensure_future(ground.run(stop)) # UDP 接收地面站坐标
|
||
runner_task = asyncio.ensure_future(runner.run(stop))
|
||
|
||
print(f"-- 启动真机任务全流程（吊舱 {cfg.pod_serial.port}，"
|
||
f"地面站 UDP {cfg.ground_inject.port}，目标类型 {args.target_type}，"
|
||
f"收尾 {'就地降落' if args.land else '返航'}）", flush=True)
|
||
print(f" 状态机: takeoff={sm_cfg.takeoff_altitude_m}m "
|
||
f"arrival_radius={sm_cfg.arrival_radius_m}m "
|
||
f"search={sm_cfg.search_timeout_s}s@{sm_cfg.sweep_rate_dps}°/s "
|
||
f"track_max={sm_cfg.track_timeout_s}s | "
|
||
f"坐标时效 ground_inject={cfg.ground_inject.timeout_ms}ms "
|
||
f"guidance={cfg.guidance.target_stale_ms}ms", flush=True)
|
||
print(f" 等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}", flush=True)
|
||
|
||
started = time.monotonic()
|
||
last_state = None
|
||
try:
|
||
while not runner.rtl_done:
|
||
await asyncio.sleep(1.0)
|
||
if runner.state is not last_state:
|
||
last_state = runner.state
|
||
print(f"[任务] 进入状态: {runner.state.value}", flush=True)
|
||
if runner_task.done():
|
||
exc = runner_task.exception()
|
||
if exc is not None:
|
||
print(f"! runner 异常退出: {type(exc).__name__}: {exc}",
|
||
flush=True)
|
||
break
|
||
if stop.is_set():
|
||
break # 信号处理已请求退出，交给 finally 收尾
|
||
if time.monotonic() - started > TASK_TIMEOUT_S:
|
||
print("! 全流程超时，强制返航", flush=True)
|
||
runner.request_rtl()
|
||
if runner.rtl_done:
|
||
print("-- 任务完成（落地）", flush=True)
|
||
except asyncio.CancelledError:
|
||
raise
|
||
finally:
|
||
stop.set()
|
||
ground_task.cancel()
|
||
runner_task.cancel()
|
||
try:
|
||
await runner_task
|
||
except asyncio.CancelledError:
|
||
pass
|
||
try:
|
||
await ground_task
|
||
except asyncio.CancelledError:
|
||
pass
|
||
|
||
await print_flight_mode(drone)
|
||
print("\n===== 真机任务全流程完成 =====", flush=True)
|
||
return 0
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(description="真机任务状态机全流程")
|
||
ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径")
|
||
ap.add_argument("--conn", default=None, help="MAVSDK 连接串（覆盖 px4.connection_string）")
|
||
ap.add_argument("--port", help="覆盖吊舱串口设备号（如 /dev/ttyAMA0）")
|
||
ap.add_argument("--target-type", type=int, default=1,
|
||
help="吊舱目标类型：0=人/车，1=无人机")
|
||
ap.add_argument("--log-dir", default=None, help="日志输出目录（默认按时间戳）")
|
||
ap.add_argument("--land", action="store_true",
|
||
help="收尾改为就地降落（默认返航 RTL）")
|
||
args = ap.parse_args()
|
||
|
||
logging.basicConfig(level=logging.INFO,
|
||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||
datefmt="%H:%M:%S")
|
||
# 真机常在树莓派上经 SSH 运行：stdout 非 tty 时块缓冲会让进度看不到，强制行缓冲
|
||
try:
|
||
sys.stdout.reconfigure(line_buffering=True)
|
||
except (AttributeError, ValueError):
|
||
pass
|
||
return asyncio.run(run(args))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|