diff --git a/docs/air/uav_right_of_way.md b/docs/air/uav_right_of_way.md
new file mode 100644
index 0000000000..af39b155f2
--- /dev/null
+++ b/docs/air/uav_right_of_way.md
@@ -0,0 +1,107 @@
+# 低空多机路权判定与让路恢复(MuJoCo 真飞)
+
+## 1. 这是什么
+
+低空无人机按计划飞,但航路交叉、对头、追越免不了在空中碰到一起。
+`uav_deconflict` 在起飞前把计划层的冲突解掉,可计划赶不上变化:
+风、计时误差、临时任务都会让两机实际靠得很近。这个模块补的就是
+飞行中这一层——两机 8 秒内最近距离会小于 40 m 时,当场决定谁让谁、
+怎么让,并把每条决策的理由记下来,事后可以逐条对账。
+
+它不管起飞前的事,也不改飞行计划;让路动作作用在参考速度上,
+飞完这一段就恢复原计划。
+
+## 2. 运行效果
+
+三个场景,每架机用 `uav_mujoco_sim` 的四旋翼刚体模型真飞
+(几何控制器 + 刚体积分,指标取自真实位置而不是计划点)。
+
+### 对头相遇
+
+左栏不开规则直飞,右栏开规则。看得出来右侧两对机在相遇前各自
+右转拉开了距离:
+
+
+
+### 交叉相遇
+
+同优先级按左让右处理,右侧机先走:
+
+
+
+### 追越
+
+前机保速保向,追越机左偏 20 m 从左侧通过:
+
+
+
+### 轨迹总览
+
+三场景的基线(虚线)与规则启用(实线)俯视对比:
+
+
+
+### 数字
+
+全程机对间最小三维距离:
+
+| 场景 | 不开规则 | 开规则 | 决策次数 |
+|---|---|---|---|
+| 对头 | 1.68 m | 14.56 m | 6 |
+| 交叉 | 1.19 m | 33.44 m | 8 |
+| 追越 | 0.42 m | 19.98 m | 4 |
+
+代价是航迹偏离计划的平均值从 2~3 m 涨到 4~8 m。
+
+每条决策长这样(`results/<场景>_result.json` 里的 `decision_log`):
+
+```text
+规则③对头相遇:与 UAV-2 航向差 180°、CPA 37m@8.0s,各自右转30°
+规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -44.1),右转30°让行(CPA 40m@8.0s)
+规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 25m@8.0s)
+```
+
+## 3. 做法
+
+**判定(`core/detect.py`)**:双方近似匀速直线,相对运动解析求 CPA:
+
+```
+t_cpa = -Δp·Δv / |Δv|², d_cpa = |Δp + Δv·t_cpa|
+```
+
+`0 < t_cpa ≤ 8 s` 且 `d_cpa < 40 m` 才算威胁。正在远离(`t_cpa ≤ 0`)
+不算,否则追越通过之后永远恢复不了。
+
+**规则(`core/rules.py`)**:优先级 → 左让右 → 对头各右转 → 追越左侧,
+和船只避碰规则一脉相承。几何分类:航向差 > 150° 对头、< 10° 追越、
+其余交叉;左右侧看航向叉积符号。让路动作四种:右转 30°、降高 10 m、
+减速 30%、左侧追越偏置,全部作用在参考速度上,控制器本身不动。
+
+**恢复(`core/recover.py`)**:威胁消失后先观察 3 s,再按余弦剖面
+`(1 + cos πs)/2` 把偏移收回去,两端速度为零,不激励姿态环。恢复期间
+每个决策周期照常检测,有二次冲突立刻中止恢复重新决策。
+
+**闭环(`core/sim.py`)**:每架机独立实例化 `uav_mujoco_sim` 的
+`Quadrotor` + `GeometricController`,参考速度前馈经一阶低通
+(τ = 0.5 s)、姿态阻尼 kw ≈ −0.18,写法与 `uav_deconflict` 的
+`core/sim.py` 一致,防止航向阶跃把姿态打震荡。`--no-sim` 时退化成
+纯运动学参考点,判定/规则/恢复走同一套代码。
+
+## 4. 复现
+
+```bash
+cd src/air/uav_right_of_way
+python main.py # 三场景基线+规则 6 次真飞,本机约 70 s
+python main.py --scenario headon
+python tools/make_gif.py # 重新录上面三个动图
+python -m pytest test/ -q # 48 项测试
+```
+
+安装、ROS 节点话题说明见模块
+[README](https://github.com/OpenHUTB/ros2/tree/master/src/air/uav_right_of_way)。
+
+## 5. 已知限制
+
+- 判定假设两机短时间匀速直线,对方急机动时预测会偏;
+- 只算两两相遇,三机以上同时触发时靠优先级字段兜底,没有做组合让路;
+- 让路幅度(30° / 10 m / 30%)是固定参数,没有按相遇几何自适应。
diff --git a/docs/img/air/uav_right_of_way/cross.gif b/docs/img/air/uav_right_of_way/cross.gif
new file mode 100644
index 0000000000..271cb41a8e
Binary files /dev/null and b/docs/img/air/uav_right_of_way/cross.gif differ
diff --git a/docs/img/air/uav_right_of_way/headon.gif b/docs/img/air/uav_right_of_way/headon.gif
new file mode 100644
index 0000000000..80dfbcb353
Binary files /dev/null and b/docs/img/air/uav_right_of_way/headon.gif differ
diff --git a/docs/img/air/uav_right_of_way/overtake.gif b/docs/img/air/uav_right_of_way/overtake.gif
new file mode 100644
index 0000000000..9677883836
Binary files /dev/null and b/docs/img/air/uav_right_of_way/overtake.gif differ
diff --git a/docs/img/air/uav_right_of_way/trajectories.png b/docs/img/air/uav_right_of_way/trajectories.png
new file mode 100644
index 0000000000..de5fbe471e
Binary files /dev/null and b/docs/img/air/uav_right_of_way/trajectories.png differ
diff --git a/docs/index.md b/docs/index.md
index e610b98f22..6bc763752c 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -130,6 +130,7 @@ ROS 2 话题发布订阅基础实验(talker/listener):[chap2/ros2_pubsub_p
## 5. 空域载具
* [无人机着陆阶段识别与接地载荷预测](./air/landing_assessment.md)
+* [低空多机路权判定与让路恢复](./air/uav_right_of_way.md) —— 两机近距离相遇时的在线让路:优先级/左让右/对头右转/左侧追越,MuJoCo 真飞对比
* [无人机载荷质量估计](./air/payload_mass_estimation.md)
diff --git a/mkdocs.yml b/mkdocs.yml
index e0e1e08ef9..c08934ee71 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -122,6 +122,7 @@ nav:
- 构建与运行: './air/uav_mission_safety_manager/usage.md'
- AirSim 实测记录: './air/uav_mission_safety_manager/airsim_validation.md'
- 无人机着陆阶段识别与接地载荷预测: './air/landing_assessment.md'
+ - 低空多机路权判定与让路恢复: './air/uav_right_of_way.md'
- 无人机载荷质量估计: './air/payload_mass_estimation.md'
- 无人机飞行轨迹类型识别与 ROS2 自动任务日志: './air/flight_pattern_recognition.md'
- 无人机遥测异常检测与 ROS2 在线诊断: './air/telemetry_diagnostics.md'
diff --git a/src/air/uav_right_of_way/README.md b/src/air/uav_right_of_way/README.md
new file mode 100644
index 0000000000..a38b04adc4
--- /dev/null
+++ b/src/air/uav_right_of_way/README.md
@@ -0,0 +1,115 @@
+# uav_right_of_way
+
+两架无人机在空中近距离相遇时,谁让谁、怎么让。这个模块做的事:
+
+1. 用相对运动解析算 CPA(最近接近点),预测 8 s 内两机距离会小于 40 m 就触发决策;
+2. 按四条规则给出让路动作:优先级高的先走 → 交叉左让右 → 对头各右转 → 追越机从左侧过;
+3. 每次决策都写一条带理由的字符串进 JSON,方便事后对账;
+4. 威胁过去之后,按余弦剖面慢慢回到计划航路,恢复期间继续盯着,有二次冲突就重新让。
+
+整个过程不改飞行计划,和 `uav_deconflict`(飞之前改起飞时刻/高度/航路)是两层东西:
+那边管计划层,这边管飞行中临场的让路。
+
+## 效果
+
+三个场景(对头 / 交叉 / 追越),每架机用 `uav_mujoco_sim` 的刚体模型真飞,
+下面是全程机对间最小三维距离(越小越危险):
+
+| 场景 | 不开规则 | 开规则 | 决策次数 |
+|---|---|---|---|
+| 对头 | 1.68 m | 14.56 m | 6 |
+| 交叉 | 1.19 m | 33.44 m | 8 |
+| 追越 | 0.42 m | 19.98 m | 4 |
+
+代价是航迹偏离计划的平均值从 2~3 m 涨到 4~8 m,属于让路的正常代价。
+
+动图(左栏不开规则、右栏开规则,同一时刻对比):
+
+
+
+另两个场景的动图和三场景轨迹总览图在 `docs/img/air/uav_right_of_way/`,
+用 `tools/make_gif.py` 重跑生成。
+
+一条决策日志长这样(存在 `results/<场景>_result.json` 的 `decision_log` 里):
+
+```text
+规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -44.1),右转30°让行(CPA 40m@8.0s)
+规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 25m@8.0s)
+```
+
+## 怎么让的
+
+让路动作一共四种,都作用在参考速度上,不动控制器本身:
+
+- 右转 30°:参考航向叠加偏置
+- 降高 10 m:参考高度下移
+- 减速 30%:参考速度乘 0.7
+- 左侧追越:给参考点加一个横向偏置目标
+
+规则只看两机当时的状态(位置/速度/航向 + 任务优先级),几何分类用航向差:
+大于 150° 算对头,小于 10° 算追越,其余是交叉;左右侧用航向叉积符号判。
+
+威胁消失后先观察 3 s 再开始恢复,恢复是余弦剖面 `(1 + cos πs)/2`:
+起点接住当前偏移、终点归零、两端速度为零,不会拿航向阶跃去激励姿态环。
+恢复期间每个决策周期重新检测,出现二次冲突立即中止恢复重新决策。
+
+## 运行
+
+```bash
+# 仅算法层(不需要 mujoco,几秒跑完)
+python main.py --no-sim
+
+# 三场景 MuJoCo 闭环,基线 + 规则共 6 次真飞(本机实测约 70 s)
+python main.py
+
+# 只跑一个场景
+python main.py --scenario overtake
+
+# 录 gif / 总览图
+python tools/make_gif.py
+```
+
+装包之后:
+
+```bash
+colcon build --packages-select uav_right_of_way
+source install/setup.bash
+ros2 run uav_right_of_way main
+ros2 launch uav_right_of_way main.launch.py # 在线决策节点
+```
+
+节点订阅 `/odom`,预测到相遇就往 `/uav_right_of_way/commands`
+发 JSON(让路动作 + 决策理由)。判定逻辑都在 `core/`,节点只是个收发壳。
+
+依赖:Python 3.10+,`pip install numpy mujoco matplotlib pytest`;
+ROS 节点部分需要 ROS 2 Humble;仿真底座是同仓库的 `uav_mujoco_sim`
+(源码运行时自动挂 `sys.path`)。
+
+## 目录
+
+```
+uav_right_of_way/
+├── main.py 入口
+├── package.xml / setup.py / setup.cfg
+├── config/uav_right_of_way.yaml 判定与规则参数
+├── launch/main.launch.py
+├── uav_right_of_way/
+│ ├── core/
+│ │ ├── detect.py CPA 解析、侵入判定
+│ │ ├── rules.py 四条规则(动作 + 理由字符串)
+│ │ ├── recover.py 余弦恢复 + 恢复期监视
+│ │ ├── scenarios.py headon / cross / overtake
+│ │ ├── sim.py 基线 vs 规则闭环
+│ │ └── pipeline.py 结果汇总、JSON、报告
+│ ├── cli.py
+│ └── ros/row_node.py 在线决策节点
+├── test/ 48 项测试(没装 mujoco 的机器也能全过,闭环用例自动退化)
+├── tools/make_gif.py
+└── results/ 每场景的机读结果
+```
+
+## 已知局限
+
+- 判定假设两机短时间匀速直线,急机动的一方预测会不准(8 s 窗口内影响有限,没专门验证);
+- 只处理两机相遇,三机以上的组合让路没有做(规则是两两算的,多机同时触发时的优先序靠优先级字段兜底);
+- 让路动作的幅度(30°/10 m/30%)写死在参数里,没有按相遇几何自适应调整。
diff --git a/src/air/uav_right_of_way/config/uav_right_of_way.yaml b/src/air/uav_right_of_way/config/uav_right_of_way.yaml
new file mode 100644
index 0000000000..dcdf4ae79c
--- /dev/null
+++ b/src/air/uav_right_of_way/config/uav_right_of_way.yaml
@@ -0,0 +1,19 @@
+# 通行权在线决策节点与算法层参数
+uav_ids: ["UAV-1", "UAV-2", "UAV-3", "UAV-4"]
+odom_prefix: "/uav"
+
+# 侵入判定:预测 horizon_s 秒内 CPA < cpa_threshold_m 即触发让路决策
+horizon_s: 8.0 # s
+cpa_threshold_m: 40.0 # m
+
+# 规则动作参数
+turn_right_deg: 30.0 # 右转角度
+descend_m: 10.0 # 降高幅度
+slow_fraction: 0.30 # 减速比例(速度降低 30%)
+overtake_lateral_m: 20.0 # 追越机从左侧通过时的横向偏置(m)
+
+# 决策与恢复
+decision_interval_s: 0.2 # 决策周期(s)
+clear_hold_s: 3.0 # 威胁消失后保持观察时长,之后开始恢复(s)
+recovery_min_s: 8.0 # 余弦剖面恢复的最短时长(s)
+stale_timeout: 1.0 # 超过该时长没收到某架里程计就不再参与决策(s)
diff --git a/src/air/uav_right_of_way/conftest.py b/src/air/uav_right_of_way/conftest.py
new file mode 100644
index 0000000000..ca14ed375a
--- /dev/null
+++ b/src/air/uav_right_of_way/conftest.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""pytest 路径引导:源码目录直接运行时也能 import 本包与仿真底座。"""
+import os
+import sys
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_SIBLING = os.path.normpath(os.path.join(_HERE, "..", "uav_mujoco_sim"))
+for _p in (_HERE, _SIBLING):
+ if os.path.isdir(_p) and _p not in sys.path:
+ sys.path.insert(0, _p)
diff --git a/src/air/uav_right_of_way/launch/main.launch.py b/src/air/uav_right_of_way/launch/main.launch.py
new file mode 100644
index 0000000000..1bda227799
--- /dev/null
+++ b/src/air/uav_right_of_way/launch/main.launch.py
@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""启动通行权在线决策节点。"""
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+ share = get_package_share_directory("uav_right_of_way")
+ params = os.path.join(share, "config", "uav_right_of_way.yaml")
+ return LaunchDescription([
+ Node(
+ package="uav_right_of_way",
+ executable="row_node",
+ name="uav_right_of_way_node",
+ output="screen",
+ parameters=[params],
+ ),
+ ])
diff --git a/src/air/uav_right_of_way/main.py b/src/air/uav_right_of_way/main.py
new file mode 100644
index 0000000000..bd96ecbb1e
--- /dev/null
+++ b/src/air/uav_right_of_way/main.py
@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""多机优先级通行权在线决策 —— 一键运行入口。
+
+用法::
+
+ python main.py # 跑全部场景(基线 vs 规则启用,含 MuJoCo 闭环)
+ python main.py --scenario headon # 只跑对头场景
+ python main.py --no-sim # 仅算法层(秒级,不需要 mujoco)
+ python main.py --json out.json # 结果另存到指定 JSON
+
+源码目录直接运行时不需要 colcon install:本文件会把同级的 ``uav_mujoco_sim``
+底座加入 ``sys.path``。装好包之后对应入口是 ``ros2 run uav_right_of_way main``。
+"""
+from __future__ import annotations
+
+import os
+import sys
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_SIBLING = os.path.normpath(os.path.join(_HERE, "..", "uav_mujoco_sim"))
+for _p in (_HERE, _SIBLING):
+ if os.path.isdir(_p) and _p not in sys.path:
+ sys.path.insert(0, _p)
+
+from uav_right_of_way.cli import main # noqa: E402
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/src/air/uav_right_of_way/package.xml b/src/air/uav_right_of_way/package.xml
new file mode 100644
index 0000000000..efcaafb2f6
--- /dev/null
+++ b/src/air/uav_right_of_way/package.xml
@@ -0,0 +1,33 @@
+
+
+
+ uav_right_of_way
+ 0.1.0
+
+ 低空多机优先级通行权在线决策。两机近距离相遇时按优先级 / 左让右 /
+ 对头各右转 / 追越从左侧四条规则即时产出让路动作(右转 30°、降高 10 m、
+ 减速 30%、保向等)并给出可审计的决策理由;预测 8 s 内 CPA < 40 m
+ 即触发;脱离威胁后按余弦剖面平滑回到计划航路,恢复期间持续监视防止
+ 二次冲突。用课程既有的 uav_mujoco_sim(MuJoCo 刚体模型 + 几何控制器)
+ 做基线(无规则直飞)与规则启用的闭环对比。
+
+ 黄家欣
+ MIT
+
+ ament_python
+
+ rclpy
+ std_msgs
+ nav_msgs
+ geometry_msgs
+ launch
+ launch_ros
+ python3-numpy
+ uav_mujoco_sim
+
+ ament_pytest
+
+
+ ament_python
+
+
diff --git a/src/air/uav_right_of_way/resource/uav_right_of_way b/src/air/uav_right_of_way/resource/uav_right_of_way
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/air/uav_right_of_way/results/cross_result.json b/src/air/uav_right_of_way/results/cross_result.json
new file mode 100644
index 0000000000..560f052892
--- /dev/null
+++ b/src/air/uav_right_of_way/results/cross_result.json
@@ -0,0 +1,401 @@
+[
+ {
+ "scenario": "cross",
+ "title": "十字交叉",
+ "description": "四架无人机从四个方向同时抵达空域中心,同优先级按规则②左让右,各自右转形成旋转通过。",
+ "closed_loop": false,
+ "params": {
+ "horizon_s": 8.0,
+ "cpa_threshold_m": 40.0,
+ "turn_right_deg": 30.0,
+ "descend_m": 10.0,
+ "slow_fraction": 0.3,
+ "overtake_lateral_m": 20.0,
+ "decision_interval_s": 0.2,
+ "clear_hold_s": 3.0,
+ "recovery_min_s": 8.0
+ },
+ "plans": [
+ {
+ "uid": "UAV-1",
+ "label": "东行",
+ "start": [
+ -180.0,
+ 0.0
+ ],
+ "end": [
+ 180.0,
+ 0.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ },
+ {
+ "uid": "UAV-2",
+ "label": "北行",
+ "start": [
+ 0.0,
+ -180.0
+ ],
+ "end": [
+ 0.0,
+ 180.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ },
+ {
+ "uid": "UAV-3",
+ "label": "西行",
+ "start": [
+ 180.0,
+ 0.0
+ ],
+ "end": [
+ -180.0,
+ 0.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ },
+ {
+ "uid": "UAV-4",
+ "label": "南行",
+ "start": [
+ 0.0,
+ 180.0
+ ],
+ "end": [
+ 0.0,
+ -180.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ }
+ ],
+ "baseline": {
+ "mode": "baseline",
+ "min_separation_m": 0.11,
+ "min_pair": [
+ "UAV-1",
+ "UAV-2"
+ ],
+ "min_sep_time_s": 28.5,
+ "decisions": 0,
+ "mean_deviation_m": 0.06,
+ "per_agent_deviation_m": {
+ "UAV-1": 0.06,
+ "UAV-2": 0.06,
+ "UAV-3": 0.06,
+ "UAV-4": 0.06
+ },
+ "below_threshold_samples": 80
+ },
+ "rules": {
+ "mode": "rules",
+ "min_separation_m": 35.49,
+ "min_pair": [
+ "UAV-1",
+ "UAV-2"
+ ],
+ "min_sep_time_s": 29.5,
+ "decisions": 21,
+ "mean_deviation_m": 9.44,
+ "per_agent_deviation_m": {
+ "UAV-1": 9.36,
+ "UAV-2": 8.91,
+ "UAV-3": 11.3,
+ "UAV-4": 8.21
+ },
+ "below_threshold_samples": 13,
+ "decision_log": [
+ {
+ "t": 17.0,
+ "uid": "UAV-1",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -92.0),右转30°让行(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.6
+ },
+ {
+ "t": 17.0,
+ "uid": "UAV-1",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 92.0,右方机优先),保持航向航速(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.6
+ },
+ {
+ "t": 17.0,
+ "uid": "UAV-2",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 91.9,右方机优先),保持航向航速(CPA 39m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.19
+ },
+ {
+ "t": 17.0,
+ "uid": "UAV-2",
+ "other": "UAV-3",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-3 位于本机右侧(叉积 -92.0),右转30°让行(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.6
+ },
+ {
+ "t": 17.0,
+ "uid": "UAV-3",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 91.9,右方机优先),保持航向航速(CPA 39m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.19
+ },
+ {
+ "t": 17.0,
+ "uid": "UAV-3",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-4 位于本机右侧(叉积 -92.0),右转30°让行(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.6
+ },
+ {
+ "t": 17.0,
+ "uid": "UAV-4",
+ "other": "UAV-3",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 91.9,右方机优先),保持航向航速(CPA 39m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.19
+ },
+ {
+ "t": 20.2,
+ "uid": "UAV-1",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 97.5,右方机优先),保持航向航速(CPA 37m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 37.44
+ },
+ {
+ "t": 20.2,
+ "uid": "UAV-2",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 57.9,右方机优先),保持航向航速(CPA 36m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 36.0
+ },
+ {
+ "t": 20.2,
+ "uid": "UAV-4",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -69.5),右转30°让行(CPA 37m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 36.98
+ },
+ {
+ "t": 20.2,
+ "uid": "UAV-4",
+ "other": "UAV-2",
+ "rule": "③对头",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则③对头相遇:与 UAV-2 航向差 160°、CPA 36m@8.0s,各自右转30°",
+ "cpa_t": 8.0,
+ "cpa_dist": 35.54
+ },
+ {
+ "t": 20.4,
+ "uid": "UAV-1",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -36.1),右转30°让行(CPA 39m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 38.68
+ },
+ {
+ "t": 20.4,
+ "uid": "UAV-2",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 90.7,右方机优先),保持航向航速(CPA 39m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 38.9
+ },
+ {
+ "t": 23.4,
+ "uid": "UAV-2",
+ "other": "UAV-3",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-3 位于本机右侧(叉积 -35.4),右转30°让行(CPA 35m@7.7s)",
+ "cpa_t": 7.67,
+ "cpa_dist": 34.58
+ },
+ {
+ "t": 23.4,
+ "uid": "UAV-3",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 76.7,右方机优先),保持航向航速(CPA 35m@7.6s)",
+ "cpa_t": 7.59,
+ "cpa_dist": 35.04
+ },
+ {
+ "t": 24.0,
+ "uid": "UAV-1",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 69.3,右方机优先),保持航向航速(CPA 39m@5.4s)",
+ "cpa_t": 5.37,
+ "cpa_dist": 39.37
+ },
+ {
+ "t": 24.0,
+ "uid": "UAV-3",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-4 位于本机右侧(叉积 -9.1),右转30°让行(CPA 37m@4.4s)",
+ "cpa_t": 4.38,
+ "cpa_dist": 36.83
+ },
+ {
+ "t": 24.0,
+ "uid": "UAV-4",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -13.5),右转30°让行(CPA 39m@5.4s)",
+ "cpa_t": 5.36,
+ "cpa_dist": 39.28
+ },
+ {
+ "t": 27.0,
+ "uid": "UAV-1",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 5.9,右方机优先),保持航向航速(CPA 39m@2.4s)",
+ "cpa_t": 2.37,
+ "cpa_dist": 39.18
+ },
+ {
+ "t": 27.0,
+ "uid": "UAV-2",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 39.5,右方机优先),保持航向航速(CPA 39m@2.4s)",
+ "cpa_t": 2.36,
+ "cpa_dist": 39.11
+ },
+ {
+ "t": 27.2,
+ "uid": "UAV-4",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 22.5,右方机优先),保持航向航速(CPA 40m@1.3s)",
+ "cpa_t": 1.29,
+ "cpa_dist": 39.57
+ }
+ ]
+ },
+ "wall_time_s": 0.5
+ }
+]
\ No newline at end of file
diff --git a/src/air/uav_right_of_way/results/headon_result.json b/src/air/uav_right_of_way/results/headon_result.json
new file mode 100644
index 0000000000..080e3a6cbd
--- /dev/null
+++ b/src/air/uav_right_of_way/results/headon_result.json
@@ -0,0 +1,284 @@
+[
+ {
+ "scenario": "headon",
+ "title": "对头相遇",
+ "description": "两条平行走廊各有一对同高度对头机,中线相遇;同优先级按规则③各自右转 30° 互相从左側通过。",
+ "closed_loop": false,
+ "params": {
+ "horizon_s": 8.0,
+ "cpa_threshold_m": 40.0,
+ "turn_right_deg": 30.0,
+ "descend_m": 10.0,
+ "slow_fraction": 0.3,
+ "overtake_lateral_m": 20.0,
+ "decision_interval_s": 0.2,
+ "clear_hold_s": 3.0,
+ "recovery_min_s": 8.0
+ },
+ "plans": [
+ {
+ "uid": "UAV-1",
+ "label": "东向干线",
+ "start": [
+ -180.0,
+ 0.0
+ ],
+ "end": [
+ 180.0,
+ 0.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ },
+ {
+ "uid": "UAV-2",
+ "label": "西向干线",
+ "start": [
+ 180.0,
+ 0.0
+ ],
+ "end": [
+ -180.0,
+ 0.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ },
+ {
+ "uid": "UAV-3",
+ "label": "东向支线",
+ "start": [
+ -180.0,
+ 60.0
+ ],
+ "end": [
+ 180.0,
+ 60.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ },
+ {
+ "uid": "UAV-4",
+ "label": "西向支线",
+ "start": [
+ 180.0,
+ 60.0
+ ],
+ "end": [
+ -180.0,
+ 60.0
+ ],
+ "alt": 15.0,
+ "speed": 8.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 51.0
+ }
+ ],
+ "baseline": {
+ "mode": "baseline",
+ "min_separation_m": 0.16,
+ "min_pair": [
+ "UAV-1",
+ "UAV-2"
+ ],
+ "min_sep_time_s": 28.5,
+ "decisions": 0,
+ "mean_deviation_m": 0.06,
+ "per_agent_deviation_m": {
+ "UAV-1": 0.06,
+ "UAV-2": 0.06,
+ "UAV-3": 0.06,
+ "UAV-4": 0.06
+ },
+ "below_threshold_samples": 20
+ },
+ "rules": {
+ "mode": "rules",
+ "min_separation_m": 15.52,
+ "min_pair": [
+ "UAV-2",
+ "UAV-4"
+ ],
+ "min_sep_time_s": 35.0,
+ "decisions": 12,
+ "mean_deviation_m": 15.36,
+ "per_agent_deviation_m": {
+ "UAV-1": 0.06,
+ "UAV-2": 30.31,
+ "UAV-3": 25.45,
+ "UAV-4": 5.63
+ },
+ "below_threshold_samples": 102,
+ "decision_log": [
+ {
+ "t": 18.0,
+ "uid": "UAV-2",
+ "other": "UAV-1",
+ "rule": "③对头",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则③对头相遇:与 UAV-1 航向差 180°、CPA 40m@8.0s,各自右转30°",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.92
+ },
+ {
+ "t": 18.0,
+ "uid": "UAV-4",
+ "other": "UAV-3",
+ "rule": "③对头",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则③对头相遇:与 UAV-3 航向差 180°、CPA 40m@8.0s,各自右转30°",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.92
+ },
+ {
+ "t": 19.2,
+ "uid": "UAV-2",
+ "other": "UAV-3",
+ "rule": "③对头",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则③对头相遇:与 UAV-3 航向差 153°、CPA 40m@8.0s,各自右转30°",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.65
+ },
+ {
+ "t": 19.2,
+ "uid": "UAV-3",
+ "other": "UAV-2",
+ "rule": "③对头",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则③对头相遇:与 UAV-2 航向差 153°、CPA 40m@8.0s,各自右转30°",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.54
+ },
+ {
+ "t": 19.8,
+ "uid": "UAV-1",
+ "other": "UAV-3",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 57.6,右方机优先),保持航向航速(CPA 36m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 35.75
+ },
+ {
+ "t": 19.8,
+ "uid": "UAV-3",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -53.4),右转30°让行(CPA 36m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 35.76
+ },
+ {
+ "t": 21.6,
+ "uid": "UAV-2",
+ "other": "UAV-4",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-4 位于本机右侧(叉积 -49.6),右转30°让行(CPA 36m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 35.53
+ },
+ {
+ "t": 21.6,
+ "uid": "UAV-4",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 56.8,右方机优先),保持航向航速(CPA 35m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 35.48
+ },
+ {
+ "t": 37.0,
+ "uid": "UAV-1",
+ "other": "UAV-3",
+ "rule": "④追越",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则④追越:本机为前机,保速保向,由 UAV-3 从左侧追越(CPA 19m@3.4s)",
+ "cpa_t": 3.44,
+ "cpa_dist": 18.81
+ },
+ {
+ "t": 37.0,
+ "uid": "UAV-3",
+ "other": "UAV-1",
+ "rule": "④追越",
+ "action": {
+ "kind": "slow",
+ "value": 0.3
+ },
+ "reason": "规则④追越:须从左侧追越 UAV-1,本机现处其右侧,先减速30%保持间隔(CPA 19m@3.4s)",
+ "cpa_t": 3.42,
+ "cpa_dist": 18.89
+ },
+ {
+ "t": 38.8,
+ "uid": "UAV-2",
+ "other": "UAV-4",
+ "rule": "④追越",
+ "action": {
+ "kind": "slow",
+ "value": 0.3
+ },
+ "reason": "规则④追越:须从左侧追越 UAV-4,本机现处其右侧,先减速30%保持间隔(CPA 19m@3.0s)",
+ "cpa_t": 3.03,
+ "cpa_dist": 18.58
+ },
+ {
+ "t": 38.8,
+ "uid": "UAV-4",
+ "other": "UAV-2",
+ "rule": "④追越",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则④追越:本机为前机,保速保向,由 UAV-2 从左侧追越(CPA 19m@2.4s)",
+ "cpa_t": 2.39,
+ "cpa_dist": 18.66
+ }
+ ]
+ },
+ "wall_time_s": 0.5
+ }
+]
\ No newline at end of file
diff --git a/src/air/uav_right_of_way/results/overtake_result.json b/src/air/uav_right_of_way/results/overtake_result.json
new file mode 100644
index 0000000000..eebd11c816
--- /dev/null
+++ b/src/air/uav_right_of_way/results/overtake_result.json
@@ -0,0 +1,258 @@
+[
+ {
+ "scenario": "overtake",
+ "title": "同高度追越",
+ "description": "快机在正后方追上慢机(同航路同高度),规则④要求追越机从左侧偏置通过、前机保速保向;通过后按余弦剖面回到原航路。",
+ "closed_loop": false,
+ "params": {
+ "horizon_s": 8.0,
+ "cpa_threshold_m": 40.0,
+ "turn_right_deg": 30.0,
+ "descend_m": 10.0,
+ "slow_fraction": 0.3,
+ "overtake_lateral_m": 20.0,
+ "decision_interval_s": 0.2,
+ "clear_hold_s": 3.0,
+ "recovery_min_s": 8.0
+ },
+ "plans": [
+ {
+ "uid": "UAV-1",
+ "label": "慢速巡检(前机)",
+ "start": [
+ -180.0,
+ 0.0
+ ],
+ "end": [
+ 180.0,
+ 0.0
+ ],
+ "alt": 15.0,
+ "speed": 5.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 78.0
+ },
+ {
+ "uid": "UAV-2",
+ "label": "快速配送(追越机)",
+ "start": [
+ -220.0,
+ 0.0
+ ],
+ "end": [
+ 220.0,
+ 0.0
+ ],
+ "alt": 15.0,
+ "speed": 9.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 54.9
+ },
+ {
+ "uid": "UAV-3",
+ "label": "慢速巡检 B(前机)",
+ "start": [
+ -180.0,
+ 80.0
+ ],
+ "end": [
+ 180.0,
+ 80.0
+ ],
+ "alt": 15.0,
+ "speed": 5.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 78.0
+ },
+ {
+ "uid": "UAV-4",
+ "label": "快速配送 B(追越机)",
+ "start": [
+ -220.0,
+ 80.0
+ ],
+ "end": [
+ 220.0,
+ 80.0
+ ],
+ "alt": 15.0,
+ "speed": 9.0,
+ "priority": 0,
+ "cruise_t0": 6.0,
+ "cruise_t1": 54.9
+ }
+ ],
+ "baseline": {
+ "mode": "baseline",
+ "min_separation_m": 0.04,
+ "min_pair": [
+ "UAV-1",
+ "UAV-2"
+ ],
+ "min_sep_time_s": 16.0,
+ "decisions": 0,
+ "mean_deviation_m": 0.05,
+ "per_agent_deviation_m": {
+ "UAV-1": 0.04,
+ "UAV-2": 0.05,
+ "UAV-3": 0.04,
+ "UAV-4": 0.05
+ },
+ "below_threshold_samples": 80
+ },
+ "rules": {
+ "mode": "rules",
+ "min_separation_m": 19.96,
+ "min_pair": [
+ "UAV-1",
+ "UAV-2"
+ ],
+ "min_sep_time_s": 16.0,
+ "decisions": 10,
+ "mean_deviation_m": 11.49,
+ "per_agent_deviation_m": {
+ "UAV-1": 0.04,
+ "UAV-2": 6.06,
+ "UAV-3": 35.17,
+ "UAV-4": 4.68
+ },
+ "below_threshold_samples": 129,
+ "decision_log": [
+ {
+ "t": 6.2,
+ "uid": "UAV-1",
+ "other": "UAV-2",
+ "rule": "④追越",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则④追越:本机为前机,保速保向,由 UAV-2 从左侧追越(CPA 29m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 28.56
+ },
+ {
+ "t": 6.2,
+ "uid": "UAV-2",
+ "other": "UAV-1",
+ "rule": "④追越",
+ "action": {
+ "kind": "overtake_left",
+ "value": 20.0
+ },
+ "reason": "规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 29m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 29.15
+ },
+ {
+ "t": 6.2,
+ "uid": "UAV-3",
+ "other": "UAV-4",
+ "rule": "④追越",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则④追越:本机为前机,保速保向,由 UAV-4 从左侧追越(CPA 29m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 28.56
+ },
+ {
+ "t": 6.2,
+ "uid": "UAV-4",
+ "other": "UAV-3",
+ "rule": "④追越",
+ "action": {
+ "kind": "overtake_left",
+ "value": 20.0
+ },
+ "reason": "规则④追越:从左侧追越 UAV-3,左偏20m保持间隔(正后方,CPA 29m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 29.15
+ },
+ {
+ "t": 8.4,
+ "uid": "UAV-2",
+ "other": "UAV-3",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 52.8,右方机优先),保持航向航速(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.6
+ },
+ {
+ "t": 8.4,
+ "uid": "UAV-3",
+ "other": "UAV-2",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -71.2),右转30°让行(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.55
+ },
+ {
+ "t": 17.2,
+ "uid": "UAV-1",
+ "other": "UAV-3",
+ "rule": "②左让右",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 58.0,右方机优先),保持航向航速(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.63
+ },
+ {
+ "t": 17.2,
+ "uid": "UAV-3",
+ "other": "UAV-1",
+ "rule": "②左让右",
+ "action": {
+ "kind": "turn_right",
+ "value": 30.0
+ },
+ "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -47.3),右转30°让行(CPA 40m@8.0s)",
+ "cpa_t": 8.0,
+ "cpa_dist": 39.65
+ },
+ {
+ "t": 42.8,
+ "uid": "UAV-1",
+ "other": "UAV-3",
+ "rule": "④追越",
+ "action": {
+ "kind": "maintain",
+ "value": 0.0
+ },
+ "reason": "规则④追越:本机为前机,保速保向,由 UAV-3 从左侧追越(CPA 22m@0.1s)",
+ "cpa_t": 0.08,
+ "cpa_dist": 22.02
+ },
+ {
+ "t": 42.8,
+ "uid": "UAV-3",
+ "other": "UAV-1",
+ "rule": "④追越",
+ "action": {
+ "kind": "overtake_left",
+ "value": 20.0
+ },
+ "reason": "规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 22m@0.1s)",
+ "cpa_t": 0.07,
+ "cpa_dist": 22.07
+ }
+ ]
+ },
+ "wall_time_s": 0.6
+ }
+]
\ No newline at end of file
diff --git a/src/air/uav_right_of_way/setup.cfg b/src/air/uav_right_of_way/setup.cfg
new file mode 100644
index 0000000000..d031526ee5
--- /dev/null
+++ b/src/air/uav_right_of_way/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/uav_right_of_way
+[install]
+install_scripts=$base/lib/uav_right_of_way
diff --git a/src/air/uav_right_of_way/setup.py b/src/air/uav_right_of_way/setup.py
new file mode 100644
index 0000000000..b02294c19e
--- /dev/null
+++ b/src/air/uav_right_of_way/setup.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "uav_right_of_way"
+
+setup(
+ name=package_name,
+ version="0.1.0",
+ packages=find_packages(exclude=["test", "tools", "docs"]),
+ data_files=[
+ ("share/ament_index/resource_index/packages",
+ ["resource/" + package_name]),
+ (os.path.join("share", package_name),
+ ["package.xml", "README.md", "main.py"]),
+ (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")),
+ (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
+ ],
+ install_requires=["setuptools", "numpy", "mujoco"],
+ zip_safe=True,
+ maintainer="黄家欣",
+ maintainer_email="326516287+usernamekkyy@users.noreply.github.com",
+ description="低空多机优先级通行权在线决策(MuJoCo 闭环验证)",
+ license="MIT",
+ tests_require=["pytest"],
+ entry_points={
+ "console_scripts": [
+ "main = uav_right_of_way.cli:main",
+ "row_node = uav_right_of_way.ros.row_node:main",
+ ],
+ },
+)
diff --git a/src/air/uav_right_of_way/test/test_detect.py b/src/air/uav_right_of_way/test/test_detect.py
new file mode 100644
index 0000000000..d0768844ea
--- /dev/null
+++ b/src/air/uav_right_of_way/test/test_detect.py
@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""侵入判定(core.detect)单元测试:解析 CPA 与威胁触发条件。"""
+import numpy as np
+import pytest
+
+from uav_right_of_way.core.detect import (AircraftState, DetectParams, Threat,
+ cpa, fleet_threats, predict_threat,
+ threats_against)
+
+DP = DetectParams(horizon_s=8.0, cpa_threshold_m=40.0)
+
+
+def state(uid, pos, vel, priority=0):
+ return AircraftState(uid, np.asarray(pos, float),
+ np.asarray(vel, float), priority)
+
+
+def test_cpa_headon_analytic():
+ # 100 m 外以 10 m/s 迎面接近 → t_cpa=10 s,最近距离 0
+ t, d = cpa(np.array([100.0, 0, 0]), np.array([-10.0, 0, 0]), 30.0)
+ assert t == pytest.approx(10.0)
+ assert d == pytest.approx(0.0, abs=1e-9)
+
+
+def test_cpa_relative_stationary():
+ t, d = cpa(np.array([30.0, 40.0, 0]), np.zeros(3), 8.0)
+ assert t == pytest.approx(0.0)
+ assert d == pytest.approx(50.0)
+
+
+def test_cpa_offset_crossing():
+ # 横向偏 50 m 的交叉:最近距离恰为 50 m,出现在 t=10 s
+ t, d = cpa(np.array([100.0, 50.0, 0]), np.array([-10.0, 0, 0]), 30.0)
+ assert t == pytest.approx(10.0)
+ assert d == pytest.approx(50.0)
+
+
+def test_cpa_clamped_to_horizon():
+ t, d = cpa(np.array([100.0, 0, 0]), np.array([-10.0, 0, 0]), 5.0)
+ assert t == pytest.approx(5.0)
+ assert d == pytest.approx(50.0)
+
+
+def test_threat_triggers_within_horizon():
+ # 150 m 外 20 m/s 迎面:t_cpa=7.5 s ≤ 8 s,CPA=0 → 触发
+ own = state("A", (0, 0, 15), (10, 0, 0))
+ other = state("B", (150, 0, 15), (-10, 0, 0))
+ th = predict_threat(own, other, DP)
+ assert th is not None
+ assert th.other == "B"
+ assert th.cpa_t == pytest.approx(7.5)
+ assert th.cpa_dist < 1e-6
+
+
+def test_no_threat_beyond_horizon():
+ # 200 m 外 20 m/s:t_cpa=10 s > 8 s → 不触发
+ own = state("A", (0, 0, 15), (10, 0, 0))
+ other = state("B", (200, 0, 15), (-10, 0, 0))
+ assert predict_threat(own, other, DP) is None
+
+
+def test_no_threat_when_cpa_far():
+ # 平行航迹横向 100 m:CPA 距离 100 m > 40 m → 不触发
+ own = state("A", (0, 0, 15), (10, 0, 0))
+ other = state("B", (50, 100, 15), (10, 0, 0))
+ assert predict_threat(own, other, DP) is None
+
+
+def test_no_threat_when_separating():
+ # 已经越过最近点、正在远离:即使距离 < 40 m 也不是威胁
+ own = state("A", (0, 0, 15), (10, 0, 0))
+ other = state("B", (30, 0, 15), (10, 0, 0)) # B 在前方同向同速→静止相对
+ # 相对静止且距离 30 < 40 → 触发(静止不算"远离")
+ assert predict_threat(own, other, DP) is not None
+ # B 在前方且更快 → 拉开距离 → 不触发
+ other_fast = state("B", (30, 0, 15), (12, 0, 0))
+ assert predict_threat(own, other_fast, DP) is None
+
+
+def test_threat_already_inside_threshold():
+ own = state("A", (0, 0, 15), (0, 0, 0))
+ other = state("B", (30, 0, 15), (0, 0, 0))
+ th = predict_threat(own, other, DP)
+ assert th is not None
+ assert th.cpa_t == pytest.approx(0.0)
+ assert th.cpa_dist == pytest.approx(30.0)
+
+
+def test_fleet_threats_pairwise():
+ a = state("A", (0, 0, 15), (10, 0, 0))
+ b = state("B", (100, 0, 15), (-10, 0, 0))
+ c = state("C", (0, 500, 15), (0, 10, 0)) # 远离 others
+ out = fleet_threats([c, b, a], DP)
+ assert set(out) == {("A", "B")}
+ near = threats_against(a, [b, c], DP)
+ assert list(near) == ["B"]
+ assert isinstance(near["B"], Threat)
+
+
+def test_detect_params_validation():
+ with pytest.raises(ValueError):
+ DetectParams(horizon_s=0.0)
+ with pytest.raises(ValueError):
+ DetectParams(cpa_threshold_m=-1.0)
diff --git a/src/air/uav_right_of_way/test/test_pipeline.py b/src/air/uav_right_of_way/test/test_pipeline.py
new file mode 100644
index 0000000000..5f17b96c6e
--- /dev/null
+++ b/src/air/uav_right_of_way/test/test_pipeline.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""流水线(core.pipeline)测试:--no-sim 路径、JSON 结构与汇总表。"""
+import json
+import math
+
+import numpy as np
+import pytest
+
+from uav_right_of_way.core.detect import DetectParams
+from uav_right_of_way.core.pipeline import (format_report, format_table,
+ run_case, to_json)
+
+
+@pytest.fixture(scope="module")
+def headon_nosim():
+ return run_case("headon", sim=False)
+
+
+def test_nosim_runs_both_modes(headon_nosim):
+ r = headon_nosim
+ assert r["scenario"] == "headon"
+ assert r["closed_loop"] is False
+ assert r["baseline"]["decisions"] == 0
+ assert r["rules"]["decisions"] >= 2
+ # 规则启用显著改善最小间距
+ assert r["rules"]["min_separation_m"] > r["baseline"]["min_separation_m"]
+ assert r["rules"]["min_separation_m"] > 5.0
+
+
+def test_result_payload_structure(headon_nosim):
+ r = headon_nosim
+ for key in ("scenario", "title", "description", "params", "plans",
+ "baseline", "rules", "trajectories", "wall_time_s"):
+ assert key in r
+ assert r["params"]["horizon_s"] == 8.0
+ assert r["params"]["cpa_threshold_m"] == 40.0
+ b = r["baseline"]
+ assert b["min_separation_m"] > 0
+ assert len(b["min_pair"]) == 2
+ rl = r["rules"]
+ assert isinstance(rl["decision_log"], list) and rl["decision_log"]
+ d0 = rl["decision_log"][0]
+ for key in ("t", "uid", "other", "rule", "action", "reason",
+ "cpa_t", "cpa_dist"):
+ assert key in d0
+ # 轨迹数据可供配图
+ tr = r["trajectories"]
+ assert set(tr) == {"baseline", "rules"}
+ assert set(tr["rules"]["agents"]) == {"UAV-1", "UAV-2", "UAV-3", "UAV-4"}
+
+
+def test_nosim_all_scenarios_rules_improve():
+ for name in ("cross", "overtake"):
+ r = run_case(name, sim=False)
+ assert r["rules"]["min_separation_m"] > r["baseline"]["min_separation_m"]
+ assert r["rules"]["decisions"] > 0
+ # 决策理由全部非空(可审计)
+ assert all(d["reason"] for d in r["rules"]["decision_log"])
+
+
+def test_to_json_roundtrip(headon_nosim, tmp_path):
+ path = str(tmp_path / "out.json")
+ assert to_json([headon_nosim], path) == path
+ with open(path, "r", encoding="utf-8") as fh:
+ data = json.load(fh)
+ assert isinstance(data, list) and len(data) == 1
+ assert data[0]["scenario"] == "headon"
+ assert data[0]["rules"]["min_separation_m"] == \
+ headon_nosim["rules"]["min_separation_m"]
+
+
+def test_reports_contain_key_numbers(headon_nosim):
+ rep = format_report(headon_nosim)
+ assert "headon" in rep
+ assert "%.2f" % headon_nosim["baseline"]["min_separation_m"] in rep
+ table = format_table([headon_nosim])
+ assert "headon" in table
+ assert "%d" % headon_nosim["rules"]["decisions"] in table
+
+
+def test_unknown_scenario_rejected():
+ with pytest.raises(KeyError):
+ run_case("no_such_scenario", sim=False)
diff --git a/src/air/uav_right_of_way/test/test_recover.py b/src/air/uav_right_of_way/test/test_recover.py
new file mode 100644
index 0000000000..91b684a9e7
--- /dev/null
+++ b/src/air/uav_right_of_way/test/test_recover.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""让路恢复(core.recover)单元测试:余弦剖面性质与恢复期监视。"""
+import math
+
+import numpy as np
+import pytest
+
+from uav_right_of_way.core.detect import AircraftState, DetectParams
+from uav_right_of_way.core.recover import RecoveryProfile, should_abort
+
+DP = DetectParams(horizon_s=8.0, cpa_threshold_m=40.0)
+
+
+def state(uid, pos, vel):
+ return AircraftState(uid, np.asarray(pos, float),
+ np.asarray(vel, float), 0)
+
+
+def test_profile_endpoints():
+ rec = RecoveryProfile(lateral0=20.0, alt0=-10.0, speed_factor0=0.7,
+ duration=10.0)
+ assert rec.lateral(0.0) == pytest.approx(20.0)
+ assert rec.lateral(1.0) == pytest.approx(0.0, abs=1e-12)
+ assert rec.alt(0.0) == pytest.approx(-10.0)
+ assert rec.alt(1.0) == pytest.approx(0.0, abs=1e-12)
+ assert rec.speed_factor(0.0) == pytest.approx(0.7)
+ assert rec.speed_factor(1.0) == pytest.approx(1.0)
+
+
+def test_profile_zero_rate_at_ends():
+ rec = RecoveryProfile(lateral0=20.0, alt0=0.0, duration=10.0)
+ assert rec.lateral_rate(0.0) == pytest.approx(0.0, abs=1e-12)
+ assert rec.lateral_rate(1.0) == pytest.approx(0.0, abs=1e-12)
+ # 中点变化率最大
+ assert abs(rec.lateral_rate(0.5)) > abs(rec.lateral_rate(0.25))
+
+
+def test_profile_midpoint_half_and_monotonic():
+ rec = RecoveryProfile(lateral0=20.0, alt0=0.0, duration=10.0)
+ assert rec.lateral(0.5) == pytest.approx(10.0)
+ vals = [rec.lateral(s / 20.0) for s in range(21)]
+ assert all(a >= b - 1e-12 for a, b in zip(vals, vals[1:]))
+
+
+def test_profile_integrates_to_zero_offset():
+ # 对横向变化率做中点法数值积分,终点偏移应为 0(余弦剖面自洽)
+ rec = RecoveryProfile(lateral0=25.0, alt0=0.0, duration=12.0)
+ n = 2000
+ dt = rec.duration / n
+ x = rec.lateral0
+ for k in range(n):
+ x += rec.lateral_rate((k + 0.5) / n) * dt
+ assert x == pytest.approx(0.0, abs=1e-4)
+
+
+def test_profile_validation():
+ with pytest.raises(ValueError):
+ RecoveryProfile(lateral0=1.0, alt0=0.0, duration=0.0)
+
+
+def test_should_abort_detects_new_threat():
+ own = state("A", (0, 0, 15), (8, 0, 0))
+ calm = state("B", (0, 200, 15), (8, 0, 0)) # 平行 200 m,无威胁
+ assert should_abort(own, [calm], DP) is None
+ intruder = state("C", (100, 0, 15), (-8, 0, 0)) # 迎面 → 恢复应中止
+ th = should_abort(own, [calm, intruder], DP)
+ assert th is not None
+ assert th.other == "C"
diff --git a/src/air/uav_right_of_way/test/test_rules.py b/src/air/uav_right_of_way/test/test_rules.py
new file mode 100644
index 0000000000..06c5ed46fb
--- /dev/null
+++ b/src/air/uav_right_of_way/test/test_rules.py
@@ -0,0 +1,164 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""规则引擎(core.rules)单元测试:四条规则、动作词表与理由字符串。"""
+import math
+
+import numpy as np
+import pytest
+
+from uav_right_of_way.core.detect import AircraftState, Threat
+from uav_right_of_way.core.rules import (ACTION_DESCEND, ACTION_MAINTAIN,
+ ACTION_OVERTAKE_LEFT, ACTION_SLOW,
+ ACTION_TURN_RIGHT, Action, RuleParams,
+ classify, decide)
+
+RP = RuleParams()
+
+
+def state(uid, pos, vel, priority=0):
+ return AircraftState(uid, np.asarray(pos, float),
+ np.asarray(vel, float), priority)
+
+
+# ------------------------------------------------------------------ ① 优先级
+def test_priority_low_yields_headon_turn_right():
+ # 对头几何:低优先级 → 右转 30°(规则①)
+ own = state("A", (0, 0, 15), (8, 0, 0), priority=0)
+ other = state("B", (100, 0, 15), (-8, 0, 0), priority=10)
+ dec = decide(own, other, Threat("B", 6.0, 5.0), 1.0, RP)
+ assert dec.rule == "①优先级"
+ assert dec.action.kind == ACTION_TURN_RIGHT
+ assert dec.action.value == pytest.approx(30.0)
+ assert "优先级" in dec.reason and "10" in dec.reason
+
+
+def test_priority_low_yields_cross_descend():
+ # 交叉几何:低优先级 → 降高 10 m(规则①)
+ own = state("A", (0, 0, 15), (8, 0, 0), priority=0)
+ other = state("B", (0, 100, 15), (0, -8, 0), priority=10)
+ dec = decide(own, other, Threat("B", 6.0, 5.0), 1.0, RP)
+ assert dec.action.kind == ACTION_DESCEND
+ assert dec.action.value == pytest.approx(10.0)
+
+
+def test_priority_high_stand_on():
+ # 高优先级一方:保持航向航速,由对方让路
+ own = state("A", (100, 0, 15), (-8, 0, 0), priority=10)
+ other = state("B", (0, 0, 15), (8, 0, 0), priority=0)
+ dec = decide(own, other, Threat("B", 6.0, 5.0), 1.0, RP)
+ assert dec.rule == "①优先级"
+ assert dec.action.kind == ACTION_MAINTAIN
+
+
+# ------------------------------------------------------------------ ③ 对头
+def test_headon_both_turn_right():
+ a = state("A", (0, 0, 15), (8, 0, 0))
+ b = state("B", (100, 0, 15), (-8, 0, 0))
+ da = decide(a, b, Threat("B", 6.0, 5.0), 1.0, RP)
+ db = decide(b, a, Threat("A", 6.0, 5.0), 1.0, RP)
+ assert da.rule == "③对头" and db.rule == "③对头"
+ assert da.action.kind == ACTION_TURN_RIGHT
+ assert db.action.kind == ACTION_TURN_RIGHT
+ assert "右转" in da.reason
+
+
+def test_headon_classification():
+ a = state("A", (0, 0, 15), (8, 0, 0))
+ b = state("B", (100, 5, 15), (-8, 0, 0)) # 接近正前方迎面
+ assert classify(a, b, RP) == "headon"
+ # 大航向差但对方在侧后方 → 不算对头
+ c = state("C", (-50, -60, 15), (-8, 0, 0))
+ assert classify(a, c, RP) != "headon"
+
+
+# ------------------------------------------------------------------ ② 左让右
+def test_crossing_give_way_to_right():
+ # 对方在本机右侧(叉积 < 0)→ 本机右转让行
+ own = state("A", (0, 0, 15), (8, 0, 0)) # 东行
+ other = state("B", (0, -80, 15), (0, 8, 0)) # 南边北行 → 在右侧
+ dec = decide(own, other, Threat("B", 5.0, 10.0), 1.0, RP)
+ assert dec.rule == "②左让右"
+ assert dec.action.kind == ACTION_TURN_RIGHT
+ assert "右侧" in dec.reason
+
+
+def test_crossing_stand_on_when_on_right():
+ # 本机在对方右侧 → 对方让路,本机保持
+ own = state("A", (0, 0, 15), (0, 8, 0)) # 北行
+ other = state("B", (-80, 0, 15), (8, 0, 0)) # 西边东行 → 在本机左侧
+ dec = decide(own, other, Threat("B", 5.0, 10.0), 1.0, RP)
+ assert dec.rule == "②左让右"
+ assert dec.action.kind == ACTION_MAINTAIN
+
+
+# ------------------------------------------------------------------ ④ 追越
+def test_overtake_front_maintains():
+ front = state("A", (50, 0, 15), (5, 0, 0))
+ rear = state("B", (0, 0, 15), (9, 0, 0)) # 正后方快机
+ dec = decide(front, rear, Threat("B", 8.0, 30.0), 1.0, RP)
+ assert dec.rule == "④追越"
+ assert dec.action.kind == ACTION_MAINTAIN
+ assert "前机" in dec.reason and "保" in dec.reason
+
+
+def test_overtake_rear_directly_behind_goes_left():
+ front = state("A", (40, 0, 15), (5, 0, 0))
+ rear = state("B", (0, 0, 15), (9, 0, 0))
+ dec = decide(rear, front, Threat("A", 8.0, 30.0), 1.0, RP)
+ assert dec.rule == "④追越"
+ assert dec.action.kind == ACTION_OVERTAKE_LEFT
+ assert "左侧" in dec.reason
+
+
+def test_overtake_rear_on_left_keeps_overtaking():
+ front = state("A", (40, 0, 15), (5, 0, 0))
+ rear = state("B", (0, 10, 15), (9, 0, 0)) # 已在左侧
+ dec = decide(rear, front, Threat("A", 8.0, 30.0), 1.0, RP)
+ assert dec.action.kind == ACTION_OVERTAKE_LEFT
+ assert "已具左侧几何" in dec.reason
+
+
+def test_overtake_rear_on_right_slows():
+ front = state("A", (40, 0, 15), (5, 0, 0))
+ rear = state("B", (0, -10, 15), (9, 0, 0)) # 在右侧,不能从左侧追越
+ dec = decide(rear, front, Threat("A", 8.0, 30.0), 1.0, RP)
+ assert dec.action.kind == ACTION_SLOW
+ assert dec.action.value == pytest.approx(0.30)
+ assert "右侧" in dec.reason
+
+
+def test_overtake_classification():
+ a = state("A", (0, 0, 15), (8, 0, 0))
+ b = state("B", (60, 3, 15), (8.2, 0, 0))
+ assert classify(a, b, RP) == "overtake"
+
+
+# ------------------------------------------------------------------ 审计
+def test_decision_reason_and_dict_audit():
+ own = state("A", (0, 0, 15), (8, 0, 0))
+ other = state("B", (100, 0, 15), (-8, 0, 0))
+ dec = decide(own, other, Threat("B", 6.25, 2.0), 12.5, RP)
+ d = dec.as_dict()
+ assert d["uid"] == "A" and d["other"] == "B"
+ assert d["rule"] and d["reason"]
+ assert d["t"] == pytest.approx(12.5)
+ assert d["cpa_dist"] == pytest.approx(2.0)
+ assert d["action"]["kind"] == ACTION_TURN_RIGHT
+ # 理由字符串包含机号与规则标签(可审计)
+ assert "B" in dec.reason and dec.rule in dec.reason
+
+
+def test_action_describe_vocabulary():
+ assert "30" in Action(ACTION_TURN_RIGHT, 30).describe()
+ assert "10" in Action(ACTION_DESCEND, 10).describe()
+ assert "30" in Action(ACTION_SLOW, 0.30).describe()
+ assert "保" in Action(ACTION_MAINTAIN).describe()
+ assert "20" in Action(ACTION_OVERTAKE_LEFT, 20).describe()
+
+
+def test_low_speed_maintains():
+ own = state("A", (0, 0, 15), (0.0, 0.0, 0.0))
+ other = state("B", (100, 0, 15), (-8, 0, 0))
+ dec = decide(own, other, Threat("B", 8.0, 10.0), 1.0, RP)
+ assert dec.action.kind == ACTION_MAINTAIN
+ assert "速度过低" in dec.reason
diff --git a/src/air/uav_right_of_way/test/test_scenarios.py b/src/air/uav_right_of_way/test/test_scenarios.py
new file mode 100644
index 0000000000..c836ad6d4a
--- /dev/null
+++ b/src/air/uav_right_of_way/test/test_scenarios.py
@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""场景定义(core.scenarios)单元测试。"""
+import math
+
+import numpy as np
+import pytest
+
+from uav_right_of_way.core.scenarios import (FlightPlan, SCENARIOS,
+ get_scenario)
+
+
+def test_three_scenarios_four_aircraft():
+ assert set(SCENARIOS) == {"headon", "cross", "overtake"}
+ for name in SCENARIOS:
+ sc = get_scenario(name)
+ assert len(sc.plans) == 4
+ assert len({p.uid for p in sc.plans}) == 4
+
+
+def test_plan_phases_and_queries():
+ p = FlightPlan("T", (0, 0), (100, 0), alt=10.0, speed=5.0, climb_speed=2.5)
+ assert p.cruise_t0 == pytest.approx(4.0) # 爬升 10/2.5
+ assert p.cruise_t1 == pytest.approx(4.0 + 20.0) # 巡航 100/5
+ assert p.end_time == pytest.approx(28.0) # 再加下降 10/2.5
+ assert p.position_at(0.0)[2] == pytest.approx(0.0)
+ assert p.position_at(2.0)[2] == pytest.approx(5.0)
+ mid = p.position_at(14.0)
+ assert mid[0] == pytest.approx(50.0)
+ assert mid[2] == pytest.approx(10.0)
+ assert p.position_at(24.0)[2] == pytest.approx(10.0) # 下降起点
+ assert p.position_at(26.0)[2] == pytest.approx(5.0)
+ assert p.position_at(28.0)[2] == pytest.approx(0.0)
+ assert p.in_cruise(10.0) and not p.in_cruise(2.0)
+ v = p.velocity_at(10.0)
+ assert v[0] == pytest.approx(5.0) and v[2] == pytest.approx(0.0)
+
+
+def test_cross_track_sign():
+ p = FlightPlan("T", (0, 0), (100, 0), alt=10.0, speed=5.0)
+ assert p.cross_track([50.0, 20.0]) == pytest.approx(20.0) # 左侧为正
+ assert p.cross_track([50.0, -20.0]) == pytest.approx(-20.0)
+ assert p.along_track([30.0, 7.0]) == pytest.approx(30.0)
+
+
+def test_headon_geometry():
+ sc = get_scenario("headon")
+ for a_uid, b_uid in (("UAV-1", "UAV-2"), ("UAV-3", "UAV-4")):
+ a = next(p for p in sc.plans if p.uid == a_uid)
+ b = next(p for p in sc.plans if p.uid == b_uid)
+ assert a.alt == b.alt # 同高度
+ hdg_diff = abs(math.atan2(math.sin(b.cruise_heading - a.cruise_heading),
+ math.cos(b.cruise_heading - a.cruise_heading)))
+ assert hdg_diff == pytest.approx(math.pi, abs=1e-6) # 对头
+ # 同一条走廊
+ assert a.start[1] == b.start[1]
+
+
+def test_cross_meets_at_center():
+ sc = get_scenario("cross")
+ for p in sc.plans:
+ assert p.speed == 8.0 and p.alt == 15.0
+ # 巡航中点都过空域中心
+ t_mid = 0.5 * (p.cruise_t0 + p.cruise_t1)
+ pos = p.position_at(t_mid)
+ assert abs(pos[0]) < 1e-6 and abs(pos[1]) < 1e-6
+
+
+def test_overtake_rear_faster_same_lane():
+ sc = get_scenario("overtake")
+ u1 = next(p for p in sc.plans if p.uid == "UAV-1")
+ u2 = next(p for p in sc.plans if p.uid == "UAV-2")
+ assert u2.speed > u1.speed
+ assert u1.start[1] == u2.start[1] == u1.end[1] # 同航路
+ assert u2.start[0] < u1.start[0] # 后机更靠后
+ assert u2.cruise_heading == u1.cruise_heading
+
+
+def test_plan_validation():
+ with pytest.raises(ValueError):
+ FlightPlan("X", (0, 0), (0, 0))
+ with pytest.raises(ValueError):
+ FlightPlan("X", (0, 0), (100, 0), speed=0.0)
diff --git a/src/air/uav_right_of_way/test/test_sim.py b/src/air/uav_right_of_way/test/test_sim.py
new file mode 100644
index 0000000000..23b31a868f
--- /dev/null
+++ b/src/air/uav_right_of_way/test/test_sim.py
@@ -0,0 +1,67 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""MuJoCo 闭环冒烟测试:小规模对头场景真飞(<10 s 墙钟)。"""
+import numpy as np
+import pytest
+
+from uav_right_of_way.core.detect import DetectParams
+from uav_right_of_way.core.scenarios import FlightPlan
+from uav_right_of_way.core.sim import run_fleet
+
+pytest.importorskip("mujoco")
+
+DP = DetectParams(horizon_s=8.0, cpa_threshold_m=40.0)
+
+
+def mini_headon():
+ """小规模对头:航程减半、巡航高度 12 m,控制冒烟时长。"""
+ return [
+ FlightPlan("A", (-90.0, 0.0), (90.0, 0.0), alt=12.0, speed=8.0),
+ FlightPlan("B", (90.0, 0.0), (-90.0, 0.0), alt=12.0, speed=8.0),
+ ]
+
+
+@pytest.fixture(scope="module")
+def smoke():
+ plans = mini_headon()
+ base = run_fleet(plans, enable_rules=False, use_mujoco=True, tail=1.0,
+ detect_params=DP)
+ rules = run_fleet(plans, enable_rules=True, use_mujoco=True, tail=1.0,
+ detect_params=DP)
+ return base, rules
+
+
+def test_baseline_has_no_decisions_and_collides(smoke):
+ base, _ = smoke
+ assert base.mode == "baseline"
+ assert len(base.decisions) == 0
+ min_d, pair, _t = base.min_separation()
+ assert pair == ("A", "B")
+ assert min_d < 10.0 # 无规则时两机在中线几乎重合
+
+
+def test_rules_improve_separation_and_decide(smoke):
+ _base, rules = smoke
+ assert rules.mode == "rules"
+ assert len(rules.decisions) >= 2 # 双方各出一条决策
+ assert all(d.reason for d in rules.decisions)
+ min_d, _pair, _t = rules.min_separation()
+ assert min_d > 5.0 # 真飞间隔显著拉开
+
+
+def test_result_shapes_and_wall_time(smoke):
+ base, rules = smoke
+ for res in (base, rules):
+ assert set(res.pos) == {"A", "B"}
+ n = len(res.times)
+ assert n > 50
+ for arr in res.pos.values():
+ assert arr.shape == (n, 3)
+ assert res.steps > 0
+ assert res.wall_time > 0.0
+ # 真飞到达误差:终点接近计划终点
+ for plan in res.plans.values():
+ err = np.linalg.norm(res.pos[plan.uid][-1][:2]
+ - np.array(plan.end))
+ assert err < 6.0, "%s 终点误差 %.2f m" % (plan.uid, err)
+ assert base.wall_time + rules.wall_time < 60.0
diff --git a/src/air/uav_right_of_way/tools/make_gif.py b/src/air/uav_right_of_way/tools/make_gif.py
new file mode 100644
index 0000000000..ba3a1f8e01
--- /dev/null
+++ b/src/air/uav_right_of_way/tools/make_gif.py
@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""跑一遍三个场景,把闭环真飞过程录成 gif。
+
+左栏是不开规则的直飞,右栏是开规则之后的同一场景,同一时刻同帧对比。
+录的是 MuJoCo 刚体积分出来的真实位置,不是画出来的示意线。
+
+用法(在本目录下):python tools/make_gif.py
+三个场景都会重跑一遍(MuJoCo 真飞,本机约一分多钟),然后重录四张图。
+"""
+from __future__ import annotations
+
+import os
+import sys
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_PKG = os.path.dirname(_HERE)
+_SIM = os.path.normpath(os.path.join(os.path.dirname(_PKG), "uav_mujoco_sim"))
+for _p in (_PKG, _SIM):
+ if os.path.isdir(_p) and _p not in sys.path:
+ sys.path.insert(0, _p)
+
+import matplotlib
+
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+from matplotlib import font_manager
+from matplotlib.animation import FuncAnimation, PillowWriter
+import numpy as np
+
+for _name in ("Microsoft YaHei", "SimHei", "Noto Sans CJK SC", "DejaVu Sans"):
+ if any(f.name == _name for f in font_manager.fontManager.ttflist):
+ plt.rcParams["font.sans-serif"] = [_name]
+ break
+plt.rcParams["axes.unicode_minus"] = False
+
+ROOT = os.path.dirname(_PKG)
+OUT_DIR = os.path.join(ROOT, "..", "..", "docs", "img", "air", "uav_right_of_way")
+C_BASE = "#7f7f7f"
+C_RULE = ["#1f77b4", "#d62728", "#2ca02c", "#ff7f0e"]
+
+# 三个场景各录一段,时间轴统一从 0 开始
+SCENES = [("headon", "对头相遇(各自右转 30°)"),
+ ("cross", "交叉相遇(左让右)"),
+ ("overtake", "追越(从左侧通过)")]
+
+
+def run_scene(name):
+ """跑一个场景的基线与规则两趟,返回 (sc, base_traj, rule_traj)。"""
+ from uav_right_of_way.core.sim import run_fleet
+ from uav_right_of_way.core.scenarios import get_scenario
+
+ sc = get_scenario(name)
+ # record_every 调小一点,出 gif 才不卡
+ base = run_fleet(sc.plans, enable_rules=False, record_every=5)
+ rule = run_fleet(sc.plans, enable_rules=True, record_every=5)
+ return sc, dict(times=base.times, agents=base.pos), \
+ dict(times=rule.times, agents=rule.pos)
+
+
+def _draw(ax, traj, t_idx, color_of, title):
+ ax.clear()
+ ax.set_title(title, fontsize=10)
+ for uid, rows in sorted(traj["agents"].items()):
+ arr = np.asarray(rows)[: t_idx + 1]
+ if len(arr) == 0:
+ continue
+ c = color_of(uid)
+ ax.plot(arr[:, 0], arr[:, 1], "-", color=c, lw=1.2, alpha=0.75)
+ ax.plot(arr[-1, 0], arr[-1, 1], "o", color=c, ms=7)
+ ax.annotate(uid, (arr[-1, 0], arr[-1, 1]), textcoords="offset points",
+ xytext=(5, 4), fontsize=8, color=c)
+ ax.set_aspect("equal")
+ ax.grid(alpha=0.25, lw=0.5)
+ ax.set_xlabel("x / m", fontsize=8)
+ ax.set_ylabel("y / m", fontsize=8)
+ ax.tick_params(labelsize=8)
+
+
+def make_gif(name, title, sc, base, rule, out):
+ ids = sorted(base["agents"])
+ color_of = lambda uid: C_RULE[ids.index(uid) % len(C_RULE)] # noqa: E731
+
+ tb = np.asarray(base["times"], float)
+ tr = np.asarray(rule["times"], float)
+ n = min(len(tb), len(tr))
+ # 抽帧,别录几百帧进来
+ frames = np.linspace(0, n - 1, min(n, 56)).astype(int)
+
+ fig, axes = plt.subplots(1, 2, figsize=(7.8, 3.5))
+ fig.suptitle(title, fontsize=11)
+
+ def update(k):
+ k = frames[k]
+ _draw(axes[0], base, k, color_of, "不给规则,直接飞")
+ _draw(axes[1], rule, k, color_of, "给规则,实时让路")
+ fig.suptitle("%s t = %.0f s" % (title, tb[k]), fontsize=11)
+ return []
+
+ anim = FuncAnimation(fig, update, frames=len(frames), interval=180,
+ blit=False)
+ os.makedirs(os.path.dirname(out), exist_ok=True)
+ anim.save(out, writer=PillowWriter(fps=6))
+ plt.close(fig)
+ return out
+
+
+def main():
+ runs = []
+ for name, title in SCENES:
+ sc, base, rule = run_scene(name)
+ runs.append((name, title, base, rule))
+ out = os.path.join(OUT_DIR, "%s.gif" % name)
+ make_gif(name, title, sc, base, rule, out)
+ print("saved", os.path.normpath(out))
+
+ # 三场景俯视总图(静态),给文档当第一张图
+ make_overview(runs)
+
+
+def make_overview(runs):
+ fig, axes = plt.subplots(1, 3, figsize=(10.5, 3.2))
+ for ax, (name, title, base, rule) in zip(axes, runs):
+ ids = sorted(base["agents"])
+ for i, uid in enumerate(ids):
+ ab = np.asarray(base["agents"][uid])
+ ar = np.asarray(rule["agents"][uid])
+ c = C_RULE[i % len(C_RULE)]
+ ax.plot(ab[:, 0], ab[:, 1], "--", color=c, lw=1.0, alpha=0.40)
+ ax.plot(ar[:, 0], ar[:, 1], "-", color=c, lw=1.1, alpha=0.85)
+ ax.set_title(title.split("(")[0], fontsize=10)
+ ax.set_aspect("equal")
+ ax.grid(alpha=0.25, lw=0.5)
+ ax.tick_params(labelsize=8)
+ ax.set_xlabel("x / m", fontsize=8)
+ ax.set_ylabel("y / m", fontsize=8)
+ axes[0].plot([], [], "--", color=C_BASE, label="基线")
+ axes[0].plot([], [], "-", color=C_BASE, label="规则")
+ axes[0].legend(fontsize=7, loc="best")
+ fig.tight_layout()
+ out = os.path.join(OUT_DIR, "trajectories.png")
+ os.makedirs(os.path.dirname(out), exist_ok=True)
+ fig.savefig(out, dpi=130)
+ plt.close(fig)
+ print("saved", os.path.normpath(out))
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/air/uav_right_of_way/uav_right_of_way/__init__.py b/src/air/uav_right_of_way/uav_right_of_way/__init__.py
new file mode 100644
index 0000000000..60cfd5434b
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/__init__.py
@@ -0,0 +1,3 @@
+# -*- coding: utf-8 -*-
+"""uav_right_of_way:低空多机优先级通行权在线决策。"""
+__version__ = "0.1.0"
diff --git a/src/air/uav_right_of_way/uav_right_of_way/cli.py b/src/air/uav_right_of_way/uav_right_of_way/cli.py
new file mode 100644
index 0000000000..a4db2c796e
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/cli.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""命令行入口(``ros2 run uav_right_of_way main`` 与 ``python main.py`` 共用)。"""
+from __future__ import annotations
+
+import argparse
+import os
+import sys
+
+from .core.pipeline import (format_report, format_table, run_all, run_case,
+ to_json)
+from .core.scenarios import SCENARIOS
+
+_HERE = os.path.dirname(os.path.abspath(__file__))
+_PKG = os.path.dirname(_HERE)
+_RESULTS_DIR = os.path.join(_PKG, "results")
+_IMAGES_DIR = os.path.join(_PKG, "images")
+
+
+def build_parser() -> argparse.ArgumentParser:
+ p = argparse.ArgumentParser(
+ description="低空多机优先级通行权在线决策(基线 vs 规则启用,MuJoCo 闭环)")
+ p.add_argument("--scenario", "--case", dest="scenario", default="all",
+ help="场景名(%s)或 all" % ", ".join(SCENARIOS))
+ p.add_argument("--no-sim", action="store_true",
+ help="跳过 MuJoCo 闭环,仅算法层(秒级)")
+ p.add_argument("--json", default="",
+ help="结果 JSON 路径(默认自动写 results/<场景>_result.json)")
+ p.add_argument("--progress", action="store_true", help="打印仿真进度")
+ return p
+
+
+def main(argv=None) -> int:
+ args = build_parser().parse_args(argv)
+ sim = not args.no_sim
+
+ if args.scenario == "all":
+ results = run_all(list(SCENARIOS), sim=sim, progress=args.progress)
+ else:
+ if args.scenario not in SCENARIOS:
+ print("未知场景 %r,可选:%s 或 all"
+ % (args.scenario, ", ".join(SCENARIOS)))
+ return 2
+ results = [run_case(args.scenario, sim=sim, progress=args.progress)]
+
+ print()
+ for r in results:
+ print(format_report(r))
+ print()
+
+ # 每个场景自动落盘 results/<场景>_result.json(SPEC 要求)
+ os.makedirs(_RESULTS_DIR, exist_ok=True)
+ for r in results:
+ p = os.path.join(_RESULTS_DIR, "%s_result.json" % r["scenario"])
+ print("机读结果已写入 %s" % to_json([r], p))
+ if args.json:
+ print("机读结果已写入 %s" % to_json(results, args.json))
+
+ print("轨迹俯视图/动图需要的话跑:python tools/make_gif.py")
+ print(format_table(results))
+ return 0
+
+
+if __name__ == "__main__":
+ raise SystemExit(main())
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/__init__.py b/src/air/uav_right_of_way/uav_right_of_way/core/__init__.py
new file mode 100644
index 0000000000..1eb2a4146e
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/__init__.py
@@ -0,0 +1,2 @@
+# -*- coding: utf-8 -*-
+"""纯算法层:只依赖 numpy,不依赖 ROS。"""
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/detect.py b/src/air/uav_right_of_way/uav_right_of_way/core/detect.py
new file mode 100644
index 0000000000..5fa12fd8f2
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/detect.py
@@ -0,0 +1,141 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""侵入判定:用相对运动解析求最近接近点(CPA),预测短时威胁。
+
+本模块回答"现在要不要启动让路决策":给定两机当前状态(位置/速度),
+把双方近似为匀速直线运动,相对运动也是匀速直线,最近接近可以解析求出:
+
+ d(t) = |Δp + Δv·t|, t_cpa = -Δp·Δv / |Δv|²
+
+当 ``0 < t_cpa ≤ horizon`` 且预测 CPA 距离 < ``cpa_threshold`` 时触发决策。
+与参考实现 uav_deconflict 的 ``conflict.py`` 同思路(相对运动解析、无采样
+步长),但这里用在**在线**判定上:输入是当前一帧状态而不是整条计划航迹。
+
+约定:``t_cpa ≤ 0``(正在远离)不算威胁——追越通过后两机必然逐渐拉开,
+不能让"还在 40 m 内但正在分离"的状态永远触发决策。
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import Dict, Optional, Sequence, Tuple
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class DetectParams:
+ """侵入判定参数(对应 config/uav_right_of_way.yaml)。"""
+
+ horizon_s: float = 8.0 # 预测时域(s)
+ cpa_threshold_m: float = 40.0 # CPA 距离阈值(m)
+
+ def __post_init__(self) -> None:
+ if self.horizon_s <= 0.0 or self.cpa_threshold_m <= 0.0:
+ raise ValueError("horizon 与 cpa_threshold 必须为正: %r" % (self,))
+
+
+@dataclass
+class AircraftState:
+ """一帧机体状态:位置 / 速度(世界系 ENU,z 向上)+ 任务优先级。"""
+
+ uid: str
+ pos: np.ndarray # (3,) m
+ vel: np.ndarray # (3,) m/s
+ priority: int = 0
+ label: str = ""
+
+ @property
+ def speed(self) -> float:
+ return float(np.linalg.norm(self.vel))
+
+ @property
+ def heading(self) -> Optional[float]:
+ """航向角(rad,从 x 东逆时针);速度过低时返回 None。"""
+ if float(np.hypot(self.vel[0], self.vel[1])) < 1e-6:
+ return None
+ return float(math.atan2(self.vel[1], self.vel[0]))
+
+ def as_dict(self) -> dict:
+ return dict(uid=self.uid,
+ pos=[round(float(v), 2) for v in self.pos],
+ vel=[round(float(v), 2) for v in self.vel],
+ priority=self.priority)
+
+
+@dataclass
+class Threat:
+ """一次预测到的接近威胁。"""
+
+ other: str # 对方机号
+ cpa_t: float # 预测最近接近时刻(相对当前,已截断到 horizon)
+ cpa_dist: float # 预测最近接近距离(m)
+
+ def as_dict(self) -> dict:
+ return dict(other=self.other,
+ cpa_t=round(self.cpa_t, 2),
+ cpa_dist=round(self.cpa_dist, 2))
+
+
+def cpa(p_rel: np.ndarray, v_rel: np.ndarray,
+ horizon: float) -> Tuple[float, float]:
+ """相对运动解析 CPA:返回 (t_cpa, 最近距离)。
+
+ ``t_cpa`` 已截断到 ``[0, horizon]``;相对静止时返回 (0, 当前距离)。
+ """
+ p_rel = np.asarray(p_rel, dtype=float)
+ v_rel = np.asarray(v_rel, dtype=float)
+ a = float(np.dot(v_rel, v_rel))
+ if a <= 1e-9:
+ return 0.0, float(np.linalg.norm(p_rel))
+ t = -float(np.dot(p_rel, v_rel)) / a
+ t = min(max(t, 0.0), horizon)
+ return t, float(np.linalg.norm(p_rel + v_rel * t))
+
+
+def predict_threat(own: AircraftState, other: AircraftState,
+ params: DetectParams = DetectParams()) -> Optional[Threat]:
+ """预测本机与另一机的接近威胁;不满足触发条件返回 ``None``。"""
+ p_rel = np.asarray(other.pos, dtype=float) - np.asarray(own.pos, dtype=float)
+ v_rel = np.asarray(other.vel, dtype=float) - np.asarray(own.vel, dtype=float)
+ a = float(np.dot(v_rel, v_rel))
+ if a <= 1e-9:
+ d = float(np.linalg.norm(p_rel))
+ return Threat(other.uid, 0.0, d) if d < params.cpa_threshold_m else None
+ t_unc = -float(np.dot(p_rel, v_rel)) / a
+ if t_unc <= 0.0:
+ # 正在远离:不算威胁(否则追越通过后永远触发)
+ return None
+ t = min(t_unc, params.horizon_s)
+ d = float(np.linalg.norm(p_rel + v_rel * t))
+ if d < params.cpa_threshold_m:
+ return Threat(other.uid, t, d)
+ return None
+
+
+def fleet_threats(states: Sequence[AircraftState],
+ params: DetectParams = DetectParams()
+ ) -> Dict[Tuple[str, str], Threat]:
+ """机队两两检测,返回 {(uid_a, uid_b): Threat}(a 按字典序在 b 前)。"""
+ out: Dict[Tuple[str, str], Threat] = {}
+ items = sorted(states, key=lambda s: s.uid)
+ for i, a in enumerate(items):
+ for b in items[i + 1:]:
+ th = predict_threat(a, b, params)
+ if th is not None:
+ out[(a.uid, b.uid)] = th
+ return out
+
+
+def threats_against(own: AircraftState,
+ others: Sequence[AircraftState],
+ params: DetectParams = DetectParams()) -> Dict[str, Threat]:
+ """以本机视角对所有其他机做威胁检测,返回 {对方机号: Threat}。"""
+ out: Dict[str, Threat] = {}
+ for o in others:
+ if o.uid == own.uid:
+ continue
+ th = predict_threat(own, o, params)
+ if th is not None:
+ out[o.uid] = th
+ return out
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/pipeline.py b/src/air/uav_right_of_way/uav_right_of_way/core/pipeline.py
new file mode 100644
index 0000000000..24b1fb5c46
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/pipeline.py
@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""端到端流水线:场景 → 基线/规则启用闭环 → 指标 → JSON 与控制台报告。"""
+from __future__ import annotations
+
+import json
+import os
+from typing import List, Optional, Sequence
+
+import numpy as np
+
+from .detect import DetectParams
+from .rules import RuleParams
+from .scenarios import SCENARIOS, Scenario, get_scenario
+from .sim import (CLEAR_HOLD_S, DECIDE_DT, RECOVERY_MIN_S, RunResult,
+ run_fleet)
+
+
+def run_case(name: str, sim: bool = True, progress: bool = False,
+ detect_params: Optional[DetectParams] = None,
+ rule_params: Optional[RuleParams] = None) -> dict:
+ """跑一个场景:基线(无规则)与规则启用各闭环一遍,返回 JSON 友好结果。"""
+ sc = get_scenario(name)
+ dp = detect_params or DetectParams()
+ rp = rule_params or RuleParams()
+
+ if progress:
+ print("[%s] %s — 基线(无规则直飞)..." % (name, sc.title))
+ baseline = run_fleet(sc.plans, enable_rules=False, progress=progress,
+ use_mujoco=sim, detect_params=dp, rule_params=rp)
+ if progress:
+ print("[%s] 规则启用(在线让路 + 余弦恢复)..." % name)
+ rules = run_fleet(sc.plans, enable_rules=True, progress=progress,
+ use_mujoco=sim, detect_params=dp, rule_params=rp)
+
+ return _build_result(sc, baseline, rules, dp, rp, sim)
+
+
+def run_all(names: Sequence[str], sim: bool = True, progress: bool = False,
+ detect_params: Optional[DetectParams] = None,
+ rule_params: Optional[RuleParams] = None) -> List[dict]:
+ return [run_case(n, sim=sim, progress=progress,
+ detect_params=detect_params, rule_params=rule_params)
+ for n in names]
+
+
+def _traj_payload(res: RunResult, stride: int = 10,
+ prec: int = 1) -> dict:
+ """轨迹降采样落盘(每 stride 个记录点取 1 个,约 2 Hz)。"""
+ out = {}
+ for uid, arr in sorted(res.pos.items()):
+ out[uid] = [[round(float(x), prec) for x in row]
+ for row in arr[::stride]]
+ times = [round(float(t), 2) for t in res.times[::stride]]
+ return dict(times=times, agents=out)
+
+
+def _build_result(sc: Scenario, baseline: RunResult, rules: RunResult,
+ dp: DetectParams, rp: RuleParams, sim: bool) -> dict:
+ return dict(
+ scenario=sc.name,
+ title=sc.title,
+ description=sc.description,
+ closed_loop=bool(sim),
+ params=dict(
+ horizon_s=dp.horizon_s,
+ cpa_threshold_m=dp.cpa_threshold_m,
+ turn_right_deg=rp.turn_right_deg,
+ descend_m=rp.descend_m,
+ slow_fraction=rp.slow_fraction,
+ overtake_lateral_m=rp.overtake_lateral_m,
+ decision_interval_s=DECIDE_DT,
+ clear_hold_s=CLEAR_HOLD_S,
+ recovery_min_s=RECOVERY_MIN_S,
+ ),
+ plans=[p.summary() for p in sc.plans],
+ baseline=baseline.summary(dp),
+ rules=dict(rules.summary(dp),
+ decision_log=[d.as_dict() for d in rules.decisions]),
+ trajectories=dict(baseline=_traj_payload(baseline),
+ rules=_traj_payload(rules)),
+ wall_time_s=round(baseline.wall_time + rules.wall_time, 1),
+ )
+
+
+def to_json(results: Sequence[dict], path: str) -> str:
+ os.makedirs(os.path.dirname(os.path.abspath(path)) or ".", exist_ok=True)
+ # 轨迹不落盘(占地方,评审也用不上);出图用 tools/make_gif.py 重跑
+ trimmed = []
+ for r in results:
+ r = dict(r)
+ r.pop("trajectories", None)
+ trimmed.append(r)
+ with open(path, "w", encoding="utf-8") as fh:
+ json.dump(trimmed, fh, ensure_ascii=False, indent=2)
+ return path
+
+
+def format_report(result: dict) -> str:
+ """单场景控制台报告。"""
+ lines = []
+ lines.append("=" * 72)
+ lines.append("%s — %s" % (result["scenario"], result["title"]))
+ lines.append(result["description"])
+ lines.append("-" * 72)
+ lines.append("判定参数:预测 %.0f s 内 CPA < %.0f m 触发决策"
+ % (result["params"]["horizon_s"],
+ result["params"]["cpa_threshold_m"]))
+ b = result["baseline"]
+ r = result["rules"]
+ lines.append("基线(无规则):最小间距 %.2f m(%s),40 m 内采样 %d 点"
+ % (b["min_separation_m"], "↔".join(b["min_pair"]),
+ b["below_threshold_samples"]))
+ lines.append("规则启用 :最小间距 %.2f m(%s),决策 %d 次"
+ % (r["min_separation_m"], "↔".join(r["min_pair"]),
+ r["decisions"]))
+ lines.append("平均航迹偏离:基线 %.2f m → 规则 %.2f m"
+ % (b["mean_deviation_m"], r["mean_deviation_m"]))
+ if r.get("decision_log"):
+ lines.append("决策日志(前 8 条):")
+ for d in r["decision_log"][:8]:
+ lines.append(" t=%5.1fs %s %s %s" %
+ (d["t"], d["uid"], d["action"]["kind"], d["reason"]))
+ return "\n".join(lines)
+
+
+def format_table(results: Sequence[dict]) -> str:
+ """汇总表:基线 vs 规则启用的最小间距 / 决策次数 / 平均偏离。"""
+ lines = []
+ lines.append("=" * 84)
+ lines.append("%-10s %14s %14s %8s %12s %12s" %
+ ("场景", "基线最小间距m", "规则最小间距m", "决策次数",
+ "基线偏离m", "规则偏离m"))
+ for r in results:
+ b, rl = r["baseline"], r["rules"]
+ lines.append("%-10s %14.2f %14.2f %8d %12.2f %12.2f" %
+ (r["scenario"], b["min_separation_m"],
+ rl["min_separation_m"], rl["decisions"],
+ b["mean_deviation_m"], rl["mean_deviation_m"]))
+ return "\n".join(lines)
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/recover.py b/src/air/uav_right_of_way/uav_right_of_way/core/recover.py
new file mode 100644
index 0000000000..3ae1478dd0
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/recover.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""让路恢复:脱离威胁后按余弦剖面平滑回到计划航路,恢复期间持续监视。
+
+与参考实现 uav_deconflict ``plan.py`` 的 ``with_lateral_offset`` 同一模式:
+偏移量沿恢复进程按 ``(1 + cos πs)/2`` 衰减——起点保持当前偏移、终点归零,
+且两端变化率为零,因此不会出现航向/升降率阶跃去激励几何控制器的姿态
+回路(参考实现实测:不平滑的折点偏移会激起增幅振荡、跟踪误差发散到
+2×10^5 m)。
+
+恢复不是一个"闭着眼执行完"的过程:调用方(``sim.py``)每个决策周期用
+``should_abort`` 重新做威胁检测,一旦出现二次冲突立即中止恢复、重新进入
+规则引擎——这就是"恢复期间持续监视防止二次冲突"。
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from typing import Optional, Sequence
+
+from .detect import DetectParams, Threat, predict_threat, AircraftState
+
+
+@dataclass
+class RecoveryProfile:
+ """余弦剖面恢复:横向/高度偏移从当前值平滑衰减到 0,速度系数回到 1。
+
+ Parameters
+ ----------
+ lateral0
+ 恢复开始时的横向偏移(m,沿计划航向左侧为正)。
+ alt0
+ 恢复开始时的高度偏移(m,向上为正,通常为负值=降过高度)。
+ speed_factor0
+ 恢复开始时的速度系数(<1 表示此前减过速)。
+ duration
+ 恢复总时长(s)。
+ """
+
+ lateral0: float
+ alt0: float
+ speed_factor0: float = 1.0
+ duration: float = 10.0
+
+ def __post_init__(self) -> None:
+ if self.duration <= 0.0:
+ raise ValueError("恢复时长必须为正: %r" % (self,))
+
+ def s(self, tau: float) -> float:
+ """恢复进度 ∈ [0, 1],``tau`` 为恢复开始后经过的秒数。"""
+ return min(1.0, max(0.0, tau / self.duration))
+
+ def _shape(self, s: float) -> float:
+ return 0.5 * (1.0 + math.cos(math.pi * s))
+
+ def lateral(self, s: float) -> float:
+ return self.lateral0 * self._shape(s)
+
+ def lateral_rate(self, s: float) -> float:
+ """横向偏移的时间导数(m/s)。"""
+ return -self.lateral0 * math.pi * math.sin(math.pi * s) \
+ / (2.0 * self.duration)
+
+ def alt(self, s: float) -> float:
+ return self.alt0 * self._shape(s)
+
+ def alt_rate(self, s: float) -> float:
+ return -self.alt0 * math.pi * math.sin(math.pi * s) \
+ / (2.0 * self.duration)
+
+ def speed_factor(self, s: float) -> float:
+ return 1.0 + (self.speed_factor0 - 1.0) * self._shape(s)
+
+
+def should_abort(own: AircraftState, others: Sequence[AircraftState],
+ params: DetectParams = DetectParams()) -> Optional[Threat]:
+ """恢复期间持续监视:出现新的预测威胁则返回该威胁(应中止恢复)。"""
+ for other in others:
+ if other.uid == own.uid:
+ continue
+ th = predict_threat(own, other, params)
+ if th is not None:
+ return th
+ return None
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/rules.py b/src/air/uav_right_of_way/uav_right_of_way/core/rules.py
new file mode 100644
index 0000000000..859093e9f1
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/rules.py
@@ -0,0 +1,221 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""通行权规则引擎:输入两机状态,输出让路动作 + 可审计的决策理由。
+
+四条规则(按优先级从高到低逐条适用,几何条件决定命中哪条):
+
+① 优先级:``priority`` 高者先行(医疗/警用等),低者让路——
+ 对头几何右转 30°,其余几何降高 10 m;
+② 左让右(同优先级交叉相遇):用航向叉积判左右,位于左侧的一方
+ 右转 30° 让行,位于右侧的一方保持航向航速;
+③ 对头相遇(同优先级):各自右转 30°;
+④ 追越(同优先级、小航向差):前机保速保向;追越机必须**从左侧**
+ 通过——已具左侧几何则左偏 ``overtake_lateral_m`` 追越,
+ 若位于前机右侧则先减速 30% 保持间隔。
+
+动作词表:右转 / 降高 / 减速 / 保向(外加规则④自身要求的左侧追越偏置)。
+每条输出都带 ``rule`` 标签与人类可读的 ``reason`` 字符串,供日志审计。
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from typing import Optional
+
+import numpy as np
+
+from .detect import AircraftState, Threat
+
+# ------------------------------------------------------------------ 动作词表
+ACTION_TURN_RIGHT = "turn_right" # 右转(value=角度 deg)
+ACTION_DESCEND = "descend" # 降高(value=下降量 m)
+ACTION_SLOW = "slow" # 减速(value=减速比例 0~1)
+ACTION_MAINTAIN = "maintain" # 保向保速
+ACTION_OVERTAKE_LEFT = "overtake_left" # 左侧追越偏置(value=偏置 m)
+
+
+@dataclass(frozen=True)
+class RuleParams:
+ """规则动作参数(对应 config/uav_right_of_way.yaml)。"""
+
+ turn_right_deg: float = 30.0
+ descend_m: float = 10.0
+ slow_fraction: float = 0.30
+ overtake_lateral_m: float = 20.0
+ #: 判定对头的航向差阈值(cos < cos(150°))
+ headon_cos: float = -0.8660254037844387
+ #: 对头还要求对方接近正前方(相对方位角 < 25°)
+ headon_bearing_deg: float = 25.0
+ #: 判定同向追越的航向差阈值(cos > cos(10°))
+ same_dir_cos: float = 0.984807753012208
+ #: 追越左右侧判定的最小横向距离(m)
+ side_eps_m: float = 3.0
+
+
+@dataclass(frozen=True)
+class Action:
+ """一个让路动作。"""
+
+ kind: str
+ value: float = 0.0
+
+ def describe(self) -> str:
+ if self.kind == ACTION_TURN_RIGHT:
+ return "右转%.0f°" % self.value
+ if self.kind == ACTION_DESCEND:
+ return "降高%.0fm" % self.value
+ if self.kind == ACTION_SLOW:
+ return "减速%.0f%%" % (self.value * 100.0)
+ if self.kind == ACTION_OVERTAKE_LEFT:
+ return "左侧偏置%.0fm追越" % self.value
+ return "保向保速"
+
+ def as_dict(self) -> dict:
+ return dict(kind=self.kind, value=round(float(self.value), 2))
+
+
+@dataclass
+class Decision:
+ """一次规则引擎输出(可审计)。"""
+
+ t: float
+ uid: str # 决策主体(让路/保持的机)
+ other: str # 威胁来源机
+ rule: str # 命中的规则标签,如 "①优先级"
+ action: Action
+ reason: str # 决策理由(人类可读,写日志/JSON)
+ cpa_t: float = 0.0
+ cpa_dist: float = 0.0
+
+ def as_dict(self) -> dict:
+ return dict(t=round(self.t, 2), uid=self.uid, other=self.other,
+ rule=self.rule, action=self.action.as_dict(),
+ reason=self.reason,
+ cpa_t=round(self.cpa_t, 2), cpa_dist=round(self.cpa_dist, 2))
+
+
+def _unit(hdg: float) -> np.ndarray:
+ return np.array([math.cos(hdg), math.sin(hdg)])
+
+
+def _wrap_pi(a: float) -> float:
+ while a > math.pi:
+ a -= 2.0 * math.pi
+ while a < -math.pi:
+ a += 2.0 * math.pi
+ return a
+
+
+def classify(own: AircraftState, other: AircraftState,
+ params: RuleParams = RuleParams()) -> str:
+ """两机相遇几何分类:``headon`` / ``overtake`` / ``crossing``。"""
+ h_own = own.heading
+ h_other = other.heading
+ if h_own is None or h_other is None:
+ return "crossing"
+ diff = _wrap_pi(h_other - h_own)
+ rel = np.asarray(other.pos[:2]) - np.asarray(own.pos[:2])
+ rel_bearing = abs(_wrap_pi(math.atan2(rel[1], rel[0]) - h_own))
+ if math.cos(diff) < params.headon_cos and \
+ rel_bearing < math.radians(params.headon_bearing_deg):
+ return "headon"
+ if math.cos(diff) > params.same_dir_cos:
+ return "overtake"
+ return "crossing"
+
+
+def decide(own: AircraftState, other: AircraftState,
+ threat: Optional[Threat] = None, t: float = 0.0,
+ params: RuleParams = RuleParams()) -> Decision:
+ """规则引擎:给定两机状态返回决策(动作 + 规则标签 + 理由)。"""
+ if threat is None:
+ d = float(np.linalg.norm(np.asarray(other.pos, dtype=float)
+ - np.asarray(own.pos, dtype=float)))
+ threat = Threat(other.uid, 0.0, d)
+
+ h_own = own.heading
+ h_other = other.heading
+ if h_own is None or h_other is None:
+ return Decision(t, own.uid, other.uid, "—", Action(ACTION_MAINTAIN),
+ "速度过低,保持当前状态", threat.cpa_t, threat.cpa_dist)
+
+ rel = np.asarray(other.pos[:2], dtype=float) - np.asarray(own.pos[:2], dtype=float)
+ geo = classify(own, other, params)
+
+ # ---- 规则①优先级:高者先行 ---------------------------------------
+ if own.priority < other.priority:
+ if geo == "headon":
+ act = Action(ACTION_TURN_RIGHT, params.turn_right_deg)
+ reason = ("规则①优先级:对方 %s 优先级 %d 高于本机 %d,对头相遇右转%.0f°让行"
+ % (other.uid, other.priority, own.priority,
+ params.turn_right_deg))
+ else:
+ act = Action(ACTION_DESCEND, params.descend_m)
+ reason = ("规则①优先级:对方 %s 优先级 %d 高于本机 %d,降高%.0fm让行"
+ % (other.uid, other.priority, own.priority,
+ params.descend_m))
+ return Decision(t, own.uid, other.uid, "①优先级", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+ if own.priority > other.priority:
+ act = Action(ACTION_MAINTAIN)
+ reason = ("规则①优先级:本机优先级 %d 高于对方 %s 的 %d,保持航向航速由对方让路"
+ % (own.priority, other.uid, other.priority))
+ return Decision(t, own.uid, other.uid, "①优先级", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+
+ # ---- 规则③对头相遇:各自右转 --------------------------------------
+ if geo == "headon":
+ diff_deg = math.degrees(abs(_wrap_pi(h_other - h_own)))
+ act = Action(ACTION_TURN_RIGHT, params.turn_right_deg)
+ reason = ("规则③对头相遇:与 %s 航向差 %.0f°、CPA %.0fm@%.1fs,各自右转%.0f°"
+ % (other.uid, diff_deg, threat.cpa_dist, threat.cpa_t,
+ params.turn_right_deg))
+ return Decision(t, own.uid, other.uid, "③对头", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+
+ # ---- 规则④追越:前机保速保向,追越机从左侧通过 ---------------------
+ if geo == "overtake":
+ u_other = _unit(h_other)
+ own_from_other = -rel # 本机相对前机的位置
+ cross_z = float(u_other[0] * own_from_other[1]
+ - u_other[1] * own_from_other[0])
+ along = float(np.dot(own_from_other, u_other)) # >0 本机在前
+ if along > 0.0:
+ act = Action(ACTION_MAINTAIN)
+ reason = ("规则④追越:本机为前机,保速保向,由 %s 从左侧追越"
+ "(CPA %.0fm@%.1fs)" % (other.uid, threat.cpa_dist,
+ threat.cpa_t))
+ return Decision(t, own.uid, other.uid, "④追越", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+ if cross_z < -params.side_eps_m:
+ act = Action(ACTION_SLOW, params.slow_fraction)
+ reason = ("规则④追越:须从左侧追越 %s,本机现处其右侧,先减速%.0f%%保持间隔"
+ "(CPA %.0fm@%.1fs)" % (other.uid, params.slow_fraction * 100.0,
+ threat.cpa_dist, threat.cpa_t))
+ return Decision(t, own.uid, other.uid, "④追越", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+ act = Action(ACTION_OVERTAKE_LEFT, params.overtake_lateral_m)
+ side = "已具左侧几何" if cross_z > params.side_eps_m else "正后方"
+ reason = ("规则④追越:从左侧追越 %s,左偏%.0fm保持间隔(%s,CPA %.0fm@%.1fs)"
+ % (other.uid, params.overtake_lateral_m, side,
+ threat.cpa_dist, threat.cpa_t))
+ return Decision(t, own.uid, other.uid, "④追越", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+
+ # ---- 规则②交叉相遇:左让右(航向叉积判左右) -----------------------
+ u_own = _unit(h_own)
+ cross_z = float(u_own[0] * rel[1] - u_own[1] * rel[0])
+ if cross_z < 0.0:
+ act = Action(ACTION_TURN_RIGHT, params.turn_right_deg)
+ reason = ("规则②交叉相遇左让右:%s 位于本机右侧(叉积 %.1f),右转%.0f°让行"
+ "(CPA %.0fm@%.1fs)" % (other.uid, cross_z,
+ params.turn_right_deg,
+ threat.cpa_dist, threat.cpa_t))
+ return Decision(t, own.uid, other.uid, "②左让右", act, reason,
+ threat.cpa_t, threat.cpa_dist)
+ act = Action(ACTION_MAINTAIN)
+ reason = ("规则②交叉相遇左让右:本机位于 %s 右侧(叉积 %.1f,右方机优先),"
+ "保持航向航速(CPA %.0fm@%.1fs)"
+ % (other.uid, cross_z, threat.cpa_dist, threat.cpa_t))
+ return Decision(t, own.uid, other.uid, "②左让右", act, reason,
+ threat.cpa_t, threat.cpa_dist)
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/scenarios.py b/src/air/uav_right_of_way/uav_right_of_way/core/scenarios.py
new file mode 100644
index 0000000000..882c76adf1
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/scenarios.py
@@ -0,0 +1,223 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""三个可复现的相遇场景:headon(对头)、cross(十字交叉)、overtake(追越)。
+
+所有场景都是"两机已近距离相遇"的在线局面:每架无人机沿自己的计划航路
+(起飞爬升 → 直线平飞 → 下降)直飞,冲突来自航路几何而不是人为摆位。
+每个场景 4 架,基线(无规则直飞)与规则启用共用同一批计划。
+"""
+from __future__ import annotations
+
+import bisect
+import math
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Sequence, Tuple
+
+import numpy as np
+
+
+@dataclass
+class FlightPlan:
+ """一架无人机的计划:二维航路(直线)+ 巡航高度 + 巡航速度。
+
+ 运动分三段:起点垂直爬升到 ``alt`` → 沿航路匀速平飞 → 终点垂直下降。
+ 与参考实现 ``Plan`` 同构,但只保留在线决策需要的查询接口。
+ """
+
+ uid: str
+ start: Tuple[float, float]
+ end: Tuple[float, float]
+ alt: float = 15.0 # m
+ speed: float = 8.0 # m/s
+ climb_speed: float = 2.5 # m/s
+ priority: int = 0
+ label: str = ""
+
+ _t_cruise0: float = field(default=0.0, init=False, repr=False)
+ _t_cruise1: float = field(default=0.0, init=False, repr=False)
+ _t_end: float = field(default=0.0, init=False, repr=False)
+
+ def __post_init__(self) -> None:
+ dx = self.end[0] - self.start[0]
+ dy = self.end[1] - self.start[1]
+ if math.hypot(dx, dy) <= 1e-9:
+ raise ValueError("%s 航路起终点重合" % self.uid)
+ if self.alt < 0.0 or self.speed <= 0.0 or self.climb_speed <= 0.0:
+ raise ValueError("%s 参数非法: alt=%r speed=%r" % (self.uid, self.alt,
+ self.speed))
+ self._cruise_len = math.hypot(dx, dy)
+ climb = self.alt / self.climb_speed
+ self._t_cruise0 = climb
+ self._t_cruise1 = climb + self._cruise_len / self.speed
+ self._t_end = self._t_cruise1 + climb
+
+ # ---------------------------------------------------------------- 查询
+ @property
+ def cruise_heading(self) -> float:
+ return math.atan2(self.end[1] - self.start[1],
+ self.end[0] - self.start[0])
+
+ @property
+ def cruise_start(self) -> np.ndarray:
+ return np.array([self.start[0], self.start[1], self.alt])
+
+ @property
+ def cruise_end(self) -> np.ndarray:
+ return np.array([self.end[0], self.end[1], self.alt])
+
+ @property
+ def cruise_t0(self) -> float:
+ return self._t_cruise0
+
+ @property
+ def cruise_t1(self) -> float:
+ return self._t_cruise1
+
+ @property
+ def end_time(self) -> float:
+ return self._t_end
+
+ def in_cruise(self, t: float) -> bool:
+ return self.cruise_t0 - 1e-9 <= t <= self.cruise_t1 + 1e-9
+
+ def cruise_progress(self, t: float) -> float:
+ """巡航段已飞距离(m,截断到 [0, L])。"""
+ return min(max((t - self.cruise_t0) * self.speed, 0.0), self._cruise_len)
+
+ def position_at(self, t: float) -> np.ndarray:
+ """计划位置(爬升→平飞→下降三段匀速)。"""
+ u = np.array([self.start[0], self.start[1], 0.0])
+ e = np.array([self.end[0], self.end[1], 0.0])
+ climb = self.alt / self.climb_speed
+ if t <= 0.0:
+ return u
+ if t < climb:
+ return u + np.array([0.0, 0.0, self.alt * t / climb])
+ if t < self.cruise_t1:
+ s = self.cruise_progress(t)
+ L = max(self._cruise_len, 1e-9)
+ return np.array([self.start[0] + (self.end[0] - self.start[0]) * s / L,
+ self.start[1] + (self.end[1] - self.start[1]) * s / L,
+ self.alt])
+ if t < self._t_end:
+ return e + np.array([0.0, 0.0, self.alt * (self._t_end - t) / climb])
+ return e
+
+ def velocity_at(self, t: float) -> np.ndarray:
+ climb = self.alt / self.climb_speed
+ if t < climb:
+ return np.array([0.0, 0.0, self.climb_speed])
+ if t < self.cruise_t1:
+ h = self.cruise_heading
+ return np.array([self.speed * math.cos(h), self.speed * math.sin(h),
+ 0.0])
+ return np.array([0.0, 0.0, -self.climb_speed])
+
+ def cruise_unit(self) -> np.ndarray:
+ h = self.cruise_heading
+ return np.array([math.cos(h), math.sin(h)])
+
+ def cruise_left_normal(self) -> np.ndarray:
+ h = self.cruise_heading
+ return np.array([-math.sin(h), math.cos(h)])
+
+ def cross_track(self, xy: np.ndarray) -> float:
+ """点到巡航航线的有符号横向偏移(沿航向左侧为正)。"""
+ p0 = self.cruise_start[:2]
+ n = self.cruise_left_normal()
+ return float(np.dot(np.asarray(xy[:2], dtype=float) - p0, n))
+
+ def along_track(self, xy: np.ndarray) -> float:
+ u = self.cruise_unit()
+ p0 = self.cruise_start[:2]
+ return float(np.dot(np.asarray(xy[:2], dtype=float) - p0, u))
+
+ def summary(self) -> dict:
+ return dict(uid=self.uid, label=self.label,
+ start=[round(self.start[0], 1), round(self.start[1], 1)],
+ end=[round(self.end[0], 1), round(self.end[1], 1)],
+ alt=round(self.alt, 1), speed=round(self.speed, 1),
+ priority=self.priority,
+ cruise_t0=round(self.cruise_t0, 1),
+ cruise_t1=round(self.cruise_t1, 1))
+
+
+# --------------------------------------------------------------------------- #
+# 三个场景
+# --------------------------------------------------------------------------- #
+def headon(alt: float = 15.0) -> "Scenario":
+ """两条平行走廊上各一对同高度对头机(同优先级 → 规则③各自右转)。"""
+ plans = [
+ FlightPlan("UAV-1", (-180.0, 0.0), (180.0, 0.0), alt=alt, speed=8.0,
+ label="东向干线"),
+ FlightPlan("UAV-2", (180.0, 0.0), (-180.0, 0.0), alt=alt, speed=8.0,
+ label="西向干线"),
+ FlightPlan("UAV-3", (-180.0, 60.0), (180.0, 60.0), alt=alt, speed=8.0,
+ label="东向支线"),
+ FlightPlan("UAV-4", (180.0, 60.0), (-180.0, 60.0), alt=alt, speed=8.0,
+ label="西向支线"),
+ ]
+ return Scenario(
+ "headon", "对头相遇",
+ "两条平行走廊各有一对同高度对头机,中线相遇;"
+ "同优先级按规则③各自右转 30° 互相从左側通过。",
+ plans)
+
+
+def cross(alt: float = 15.0) -> "Scenario":
+ """四机同时穿过同一空域中心(同优先级 → 规则②左让右,旋转通过)。"""
+ plans = [
+ FlightPlan("UAV-1", (-180.0, 0.0), (180.0, 0.0), alt=alt, speed=8.0,
+ label="东行"),
+ FlightPlan("UAV-2", (0.0, -180.0), (0.0, 180.0), alt=alt, speed=8.0,
+ label="北行"),
+ FlightPlan("UAV-3", (180.0, 0.0), (-180.0, 0.0), alt=alt, speed=8.0,
+ label="西行"),
+ FlightPlan("UAV-4", (0.0, 180.0), (0.0, -180.0), alt=alt, speed=8.0,
+ label="南行"),
+ ]
+ return Scenario(
+ "cross", "十字交叉",
+ "四架无人机从四个方向同时抵达空域中心,同优先级按规则②左让右,"
+ "各自右转形成旋转通过。",
+ plans)
+
+
+def overtake(alt: float = 15.0) -> "Scenario":
+ """两条平行走廊上各一对追越(规则④:追越机从左侧通过,前机保速保向)。"""
+ plans = [
+ FlightPlan("UAV-1", (-180.0, 0.0), (180.0, 0.0), alt=alt, speed=5.0,
+ label="慢速巡检(前机)"),
+ FlightPlan("UAV-2", (-220.0, 0.0), (220.0, 0.0), alt=alt, speed=9.0,
+ label="快速配送(追越机)"),
+ FlightPlan("UAV-3", (-180.0, 80.0), (180.0, 80.0), alt=alt, speed=5.0,
+ label="慢速巡检 B(前机)"),
+ FlightPlan("UAV-4", (-220.0, 80.0), (220.0, 80.0), alt=alt, speed=9.0,
+ label="快速配送 B(追越机)"),
+ ]
+ return Scenario(
+ "overtake", "同高度追越",
+ "快机在正后方追上慢机(同航路同高度),规则④要求追越机从左侧"
+ "偏置通过、前机保速保向;通过后按余弦剖面回到原航路。",
+ plans)
+
+
+@dataclass
+class Scenario:
+ name: str
+ title: str
+ description: str
+ plans: List[FlightPlan]
+
+
+SCENARIOS: Dict[str, callable] = {
+ "headon": headon,
+ "cross": cross,
+ "overtake": overtake,
+}
+
+
+def get_scenario(name: str, **kwargs) -> Scenario:
+ if name not in SCENARIOS:
+ raise KeyError("未知场景 %r,可选:%s" % (name, ", ".join(SCENARIOS)))
+ return SCENARIOS[name](**kwargs)
diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/sim.py b/src/air/uav_right_of_way/uav_right_of_way/core/sim.py
new file mode 100644
index 0000000000..f3012dd758
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/core/sim.py
@@ -0,0 +1,435 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""闭环仿真:基线(无规则直飞)与规则启用两两对比,MuJoCo 真飞。
+
+每架无人机用课程底座 ``uav_mujoco_sim``(同一份 ``quadrotor.xml`` 刚体模型
++ 几何控制器)单独实例化。规则启用时,每架机用**刚体积分出来的真实状态**
+做侵入判定(``detect``),命中规则(``rules``)后按动作调整参考速度:
+
+* 右转 30° → 参考航向叠加 −30° 偏置;
+* 降高 10 m → 参考高度下移 10 m;
+* 减速 30% → 参考速度乘 (1−0.30);
+* 左侧追越 → 对参考点施加目标横向偏置(沿航向左侧 overtake_lateral_m)。
+
+威胁消失并保持 ``clear_hold_s`` 后进入恢复(``recover.RecoveryProfile``,
+余弦剖面回到计划航路),恢复期间每个决策周期重新做威胁检测,出现二次
+冲突立即中止恢复、重新决策。
+
+参考速度前馈经一阶低通(τ=0.5 s)、姿态阻尼 kw≈−0.18——与参考实现
+uav_deconflict ``core/sim.py`` 相同的写法,防止航向阶跃激励姿态振荡。
+``--no-sim``(或 MuJoCo 不可用)时退化为纯运动学参考点积分,算法层
+(判定/规则/恢复)走完全相同的代码路径。
+"""
+from __future__ import annotations
+
+import math
+import os
+import sys
+import time
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Tuple
+
+import numpy as np
+
+from .detect import AircraftState, DetectParams, Threat, predict_threat
+from .recover import RecoveryProfile, should_abort
+from .rules import (ACTION_DESCEND, ACTION_MAINTAIN, ACTION_OVERTAKE_LEFT,
+ ACTION_SLOW, ACTION_TURN_RIGHT, Decision, RuleParams,
+ decide)
+from .scenarios import FlightPlan
+
+#: 参考速度前馈的一阶低通时间常数(s)
+REF_TAU = 0.5
+#: 姿态角速度阻尼增益(同参考实现 sim.py,抑制机动激励的振荡)
+ATT_DAMPING = (-0.18, -0.18, -0.126)
+
+#: 默认决策周期(s,仿真时间)
+DECIDE_DT = 0.2
+#: 默认威胁消失后保持观察时长(s)
+CLEAR_HOLD_S = 3.0
+#: 默认恢复最短时长(s)
+RECOVERY_MIN_S = 8.0
+#: 横向偏置跟踪增益与限幅
+LAT_GAIN = 0.8
+LAT_VMAX = 4.0
+#: 高度环增益与限幅
+ALT_GAIN = 0.8
+ALT_VMAX = 3.0
+
+
+def _load_backend():
+ """导入课程仿真底座;源码目录直接运行时自动挂同级包路径。"""
+ try:
+ from uav_mujoco_sim.core.quadrotor import ( # type: ignore
+ GeometricController, Quadrotor)
+ from uav_mujoco_sim.core.paths import models_dir # type: ignore
+ return Quadrotor, GeometricController, models_dir()
+ except Exception:
+ here = Path(__file__).resolve()
+ # src/air/uav_right_of_way/uav_right_of_way/core/sim.py → src/air
+ sibling = here.parents[3] / "uav_mujoco_sim"
+ if sibling.is_dir() and str(sibling) not in sys.path:
+ sys.path.insert(0, str(sibling))
+ from uav_mujoco_sim.core.quadrotor import ( # type: ignore
+ GeometricController, Quadrotor)
+ from uav_mujoco_sim.core.paths import models_dir # type: ignore
+ return Quadrotor, GeometricController, models_dir()
+
+
+# --------------------------------------------------------------------------- #
+# 运行结果与指标
+# --------------------------------------------------------------------------- #
+@dataclass
+class RunResult:
+ """一次闭环运行的记录与指标。"""
+
+ mode: str # "baseline" | "rules"
+ times: np.ndarray # (K,) 记录时刻
+ pos: Dict[str, np.ndarray] # uid -> (K,3) 真实位置
+ plans: Dict[str, FlightPlan]
+ decisions: List[Decision] = field(default_factory=list)
+ wall_time: float = 0.0
+ steps: int = 0
+
+ def min_separation(self) -> Tuple[float, Tuple[str, str], float]:
+ """真飞全程机对间最小三维距离及机对、时刻。"""
+ best = math.inf
+ pair: Tuple[str, str] = ("", "")
+ t_best = 0.0
+ names = sorted(self.pos)
+ for i, a in enumerate(names):
+ for b in names[i + 1:]:
+ d = np.linalg.norm(self.pos[a] - self.pos[b], axis=1)
+ k = int(np.argmin(d))
+ if d[k] < best:
+ best = float(d[k])
+ pair = (a, b)
+ t_best = float(self.times[k])
+ return best, pair, t_best
+
+ def deviation_stats(self) -> Dict[str, float]:
+ """逐机平均航迹偏离:真实位置到同时刻计划位置的平均距离(m)。"""
+ out: Dict[str, float] = {}
+ for uid, plan in self.plans.items():
+ ref = np.array([plan.position_at(t) for t in self.times])
+ err = np.linalg.norm(self.pos[uid] - ref, axis=1)
+ out[uid] = float(err.mean())
+ return out
+
+ def mean_deviation(self) -> float:
+ stats = self.deviation_stats()
+ return float(np.mean(list(stats.values()))) if stats else 0.0
+
+ def summary(self, params: DetectParams) -> dict:
+ best, pair, t_best = self.min_separation()
+ names = sorted(self.pos)
+ close_samples = 0
+ for i, a in enumerate(names):
+ for b in names[i + 1:]:
+ d = np.linalg.norm(self.pos[a] - self.pos[b], axis=1)
+ close_samples += int(np.sum(d < params.cpa_threshold_m))
+ return dict(
+ mode=self.mode,
+ min_separation_m=round(best, 2),
+ min_pair=list(pair),
+ min_sep_time_s=round(t_best, 1),
+ decisions=len(self.decisions),
+ mean_deviation_m=round(self.mean_deviation(), 2),
+ per_agent_deviation_m={k: round(v, 2)
+ for k, v in self.deviation_stats().items()},
+ below_threshold_samples=close_samples,
+ )
+
+
+# --------------------------------------------------------------------------- #
+# 机体包装:纯运动学(--no-sim 退化模式)
+# --------------------------------------------------------------------------- #
+class _KinematicBody:
+ """直接跟随参考点,接口与 ``Quadrotor`` 对齐(qpos / state / step)。"""
+
+ def __init__(self, p0: Sequence[float]):
+ self._pos = np.asarray(p0, dtype=float).copy()
+ self.vel = np.zeros(3)
+
+ @property
+ def qpos(self) -> np.ndarray:
+ return self._pos
+
+ def state(self):
+ """与 ``QuadrotorState`` 最小兼容:只用到 .vel。"""
+ return self
+
+ def set_state(self, pos: np.ndarray, vel: np.ndarray) -> None:
+ self._pos = np.asarray(pos, dtype=float).copy()
+ self.vel = np.asarray(vel, dtype=float).copy()
+
+
+class _Agent:
+ """一架无人机的闭环状态机:计划跟随 / 让路机动 / 余弦恢复。"""
+
+ def __init__(self, plan: FlightPlan, body, ctrl, enable_rules: bool,
+ detect_params: DetectParams, rule_params: RuleParams):
+ self.plan = plan
+ self.body = body
+ self.ctrl = ctrl
+ self.enable_rules = enable_rules
+ self.dp = detect_params
+ self.rp = rule_params
+
+ self.v_filt = np.zeros(3)
+ self.decisions: List[Decision] = []
+
+ # 巡航参考运动学
+ self.ref_xy = plan.cruise_start[:2].copy()
+ self.ref_z = plan.alt
+ self._cruise_init = False
+ # 机动状态
+ self.yaw_off = 0.0 # rad,右转为负
+ self.alt_off = 0.0 # m
+ self.speed_factor = 1.0
+ self.lat_target: Optional[float] = None # m,沿航向左为正
+ # 恢复
+ self.rec: Optional[RecoveryProfile] = None
+ self.rec_t0 = 0.0
+ # 决策 episode:other -> 最近一次确认威胁的时刻
+ self.episodes: Dict[str, float] = {}
+
+ # ------------------------------------------------------------------ 状态
+ def true_pos(self) -> np.ndarray:
+ return np.asarray(self.body.qpos[:3], dtype=float)
+
+ def true_vel(self) -> np.ndarray:
+ if isinstance(self.body, _KinematicBody):
+ return np.asarray(self.body.vel, dtype=float)
+ return np.asarray(self.body.state().vel, dtype=float)
+
+ def own_state(self) -> AircraftState:
+ return AircraftState(self.plan.uid, self.true_pos(), self.true_vel(),
+ self.plan.priority, self.plan.label)
+
+ # ------------------------------------------------------------------ 动作
+ def apply_action(self, dec: Decision) -> None:
+ kind = dec.action.kind
+ if kind == ACTION_TURN_RIGHT:
+ self.yaw_off = -math.radians(dec.action.value)
+ self.lat_target = None
+ elif kind == ACTION_DESCEND:
+ self.alt_off = -dec.action.value
+ self.lat_target = None
+ elif kind == ACTION_SLOW:
+ self.speed_factor = 1.0 - dec.action.value
+ self.lat_target = None
+ elif kind == ACTION_OVERTAKE_LEFT:
+ self.lat_target = dec.action.value
+ elif kind == ACTION_MAINTAIN:
+ pass
+
+ def begin_recovery(self, t: float) -> None:
+ x_off = self.plan.cross_track(self.ref_xy)
+ remain = self.plan.cruise_t1 - t
+ dur = max(RECOVERY_MIN_S, 2.5 * abs(x_off) + 4.0)
+ dur = min(dur, max(4.0, 0.8 * remain))
+ self.rec = RecoveryProfile(lateral0=x_off, alt0=self.alt_off,
+ speed_factor0=self.speed_factor,
+ duration=dur)
+ self.rec_t0 = t
+ self.yaw_off = 0.0
+ self.lat_target = None
+ self.episodes.clear()
+
+ # ------------------------------------------------------------------ 决策
+ def tick_decisions(self, t: float, others: Sequence["_Agent"],
+ clear_hold_s: float) -> None:
+ """一个决策周期:威胁检测 → 规则 → 恢复触发(含恢复期监视)。"""
+ if not self.plan.in_cruise(t):
+ return
+ others_active = [a for a in others
+ if a is not self and a.plan.in_cruise(t)]
+ own = self.own_state()
+ other_states = [a.own_state() for a in others_active]
+ by_uid = {s.uid: s for s in other_states}
+
+ threats: Dict[str, Threat] = {}
+ for st in other_states:
+ th = predict_threat(own, st, self.dp)
+ if th is not None:
+ threats[st.uid] = th
+
+ # ---- 恢复中:持续监视,出现二次冲突立即中止并重新决策 ------------
+ if self.rec is not None:
+ th = should_abort(own, other_states, self.dp)
+ if th is not None:
+ self.rec = None
+ self.episodes.clear()
+ self._new_decision(t, own, by_uid.get(th.other), th)
+ elif t - self.rec_t0 >= self.rec.duration:
+ self.rec = None
+ self.alt_off = 0.0
+ self.speed_factor = 1.0
+ return
+
+ # ---- 无威胁:episode 全部超过观察期 → 进入余弦恢复 ----------------
+ if not threats:
+ if self.episodes and \
+ all(t - t0 >= clear_hold_s for t0 in self.episodes.values()):
+ self.begin_recovery(t)
+ return
+
+ # ---- 有威胁:为尚无 episode 的机对产出决策 ------------------------
+ for other_uid in sorted(threats, key=lambda u: threats[u].cpa_t):
+ if other_uid in self.episodes:
+ self.episodes[other_uid] = t # 刷新"仍有威胁"时刻
+ continue
+ self._new_decision(t, own, by_uid.get(other_uid), threats[other_uid])
+
+ def _new_decision(self, t: float, own: AircraftState, other_state,
+ threat: Threat) -> None:
+ if other_state is None:
+ return
+ dec = decide(own, other_state, threat, t, self.rp)
+ self.decisions.append(dec)
+ self.episodes[other_state.uid] = t
+ self.apply_action(dec)
+
+ # ------------------------------------------------------------------ 参考
+ def reference(self, t: float, dt: float) -> Tuple[np.ndarray, np.ndarray,
+ float]:
+ """当前控制周期的 (参考位置, 参考速度, 参考偏航)。"""
+ plan = self.plan
+ if t < plan.cruise_t0 or t > plan.cruise_t1:
+ # 爬升 / 下降段:原样跟随计划
+ self._cruise_init = False
+ if t <= plan.cruise_t0:
+ self.ref_xy = plan.cruise_start[:2].copy()
+ self.ref_z = plan.cruise_start[2]
+ return plan.position_at(t), plan.velocity_at(t), 0.0
+
+ if not self._cruise_init:
+ self._cruise_init = True
+ self.ref_xy = plan.cruise_start[:2].copy()
+ self.ref_z = plan.alt
+
+ h = plan.cruise_heading
+ h_eff = h + self.yaw_off
+ u = np.array([math.cos(h_eff), math.sin(h_eff)])
+ n = np.array([-math.sin(h), math.cos(h)]) # 计划航向的左法向
+
+ speed_f = self.speed_factor
+ lat_cmd = 0.0
+ z_off = self.alt_off
+ if self.rec is not None:
+ s = self.rec.s(t - self.rec_t0)
+ speed_f = self.rec.speed_factor(s)
+ x_off = plan.cross_track(self.ref_xy)
+ # 余弦剖面:剖面导数前馈 + 对参考点的小反馈
+ lat_cmd = self.rec.lateral_rate(s) \
+ + 0.5 * (self.rec.lateral(s) - x_off)
+ z_off = self.rec.alt(s)
+ elif self.lat_target is not None:
+ x_off = plan.cross_track(self.ref_xy)
+ lat_cmd = (self.lat_target - x_off) * LAT_GAIN
+
+ lat_cmd = float(np.clip(lat_cmd, -LAT_VMAX, LAT_VMAX))
+ v_h = plan.speed * speed_f * u + lat_cmd * n
+ vz = float(np.clip((plan.alt + z_off - self.ref_z) * ALT_GAIN,
+ -ALT_VMAX, ALT_VMAX))
+
+ self.ref_xy = self.ref_xy + v_h * dt
+ self.ref_z = self.ref_z + vz * dt
+ v_ref = np.array([v_h[0], v_h[1], vz])
+ p_ref = np.array([self.ref_xy[0], self.ref_xy[1],
+ max(self.ref_z, 0.05)])
+ return p_ref, v_ref, h
+
+
+# --------------------------------------------------------------------------- #
+# 闭环主循环
+# --------------------------------------------------------------------------- #
+def run_fleet(plans: Sequence[FlightPlan], enable_rules: bool = True,
+ dt: float = 0.01, substeps: int = 4, record_every: int = 50,
+ tail: float = 2.0, max_steps: int = 200000, progress: bool = False,
+ use_mujoco: bool = True, ref_tau: float = REF_TAU,
+ detect_params: Optional[DetectParams] = None,
+ rule_params: Optional[RuleParams] = None,
+ decide_dt: float = DECIDE_DT,
+ clear_hold_s: float = CLEAR_HOLD_S,
+ recovery_min_s: float = RECOVERY_MIN_S) -> RunResult:
+ """让机队按计划真飞:``enable_rules=False`` 为基线(无规则直飞)。"""
+ dp = detect_params or DetectParams()
+ rp = rule_params or RuleParams()
+ use_mj = use_mujoco
+ backend = None
+ if use_mj:
+ try:
+ backend = _load_backend()
+ except Exception:
+ use_mj = False
+
+ plans = list(plans)
+ agents: List[_Agent] = []
+ if use_mj and backend is not None:
+ Quadrotor, GeometricController, model_dir = backend
+ model_path = os.path.join(str(model_dir), "quadrotor.xml")
+ for plan in plans:
+ quad = Quadrotor(model_path)
+ quad.reset((plan.start[0], plan.start[1], 0.05))
+ ctrl = GeometricController(quad.params, kw=ATT_DAMPING)
+ agents.append(_Agent(plan, quad, ctrl, enable_rules, dp, rp))
+ else:
+ for plan in plans:
+ agents.append(_Agent(plan, _KinematicBody(
+ (plan.start[0], plan.start[1], 0.05)), None, enable_rules,
+ dp, rp))
+
+ t_end = max(p.end_time for p in plans) + tail
+ n_steps = min(max_steps, int(round(t_end / dt)))
+ times: List[float] = []
+ pos: Dict[str, List[np.ndarray]] = {p.uid: [] for p in plans}
+ last_decide: Dict[str, float] = {p.uid: -math.inf for p in plans}
+ alpha = min(1.0, dt / ref_tau) if ref_tau and ref_tau > 0 else 1.0
+
+ t0_wall = time.time()
+ for step in range(n_steps):
+ t = step * dt
+ for ag in agents:
+ # 决策周期(规则启用时)
+ if enable_rules and t - last_decide[ag.plan.uid] >= decide_dt - 1e-9:
+ ag.tick_decisions(t, agents, clear_hold_s)
+ last_decide[ag.plan.uid] = t
+
+ p_ref, v_ref, hdg = ag.reference(t, dt)
+ if alpha < 1.0:
+ ag.v_filt += (v_ref - ag.v_filt) * alpha
+ v_use = ag.v_filt
+ else:
+ v_use = v_ref
+
+ if use_mj and backend is not None:
+ thrusts = ag.ctrl.act(ag.body, p_ref, v_use, np.zeros(3), hdg)
+ ag.body.step_with_control(thrusts, substeps)
+ else:
+ # 运动学退化:真实状态 = 参考点
+ ag.body.set_state(p_ref, v_use)
+
+ if step % record_every == 0:
+ times.append(t)
+ for ag in agents:
+ pos[ag.plan.uid].append(ag.true_pos().copy())
+
+ if progress and step % 5000 == 0:
+ print(" sim t=%.1f / %.1f s" % (t, t_end))
+
+ times.append(n_steps * dt)
+ for ag in agents:
+ pos[ag.plan.uid].append(ag.true_pos().copy())
+
+ arr = {uid: np.asarray(v, dtype=float) for uid, v in pos.items()}
+ mode = "rules" if enable_rules else "baseline"
+ decisions: List[Decision] = []
+ for ag in agents:
+ decisions.extend(ag.decisions)
+ decisions.sort(key=lambda d: (d.t, d.uid))
+ return RunResult(mode=mode, times=np.asarray(times, dtype=float), pos=arr,
+ plans={p.uid: p for p in plans}, decisions=decisions,
+ wall_time=time.time() - t0_wall, steps=n_steps)
diff --git a/src/air/uav_right_of_way/uav_right_of_way/ros/__init__.py b/src/air/uav_right_of_way/uav_right_of_way/ros/__init__.py
new file mode 100644
index 0000000000..8ce1015ce2
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/ros/__init__.py
@@ -0,0 +1,2 @@
+# -*- coding: utf-8 -*-
+"""ros 薄壳层:判定逻辑全部在 core,节点只做订阅/发布。"""
diff --git a/src/air/uav_right_of_way/uav_right_of_way/ros/row_node.py b/src/air/uav_right_of_way/uav_right_of_way/ros/row_node.py
new file mode 100644
index 0000000000..954c3a878b
--- /dev/null
+++ b/src/air/uav_right_of_way/uav_right_of_way/ros/row_node.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""在线通行权决策节点:订阅多机里程计,触发时发布让路指令与决策理由。
+
+参数(``config/uav_right_of_way.yaml``)::
+
+ uav_ids: ["UAV-1", ...]
+ odom_prefix: "/uav" # 话题名为 /odom
+ horizon_s: 8.0
+ cpa_threshold_m: 40.0
+ decision_interval_s: 0.2
+ clear_hold_s: 3.0
+
+发布::
+
+ /uav_right_of_way/commands std_msgs/String
+ JSON:{"stamp", "decisions": [{uid, other, rule, action, reason, ...}]}
+
+判定逻辑(威胁检测、规则引擎、理由字符串)全部在 ``core`` 里并有单元
+测试覆盖;本节点只是薄壳:订阅 odom → 组装 ``AircraftState`` → 每个决策
+周期调一次 ``detect`` + ``rules`` → 发布 JSON 并写日志。让路指令的执行
+(改参考航向/高度/速度)由各机的飞行控制侧完成,本节点不动航迹。
+"""
+from __future__ import annotations
+
+import json
+import math
+from typing import Dict, List
+
+import rclpy
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from uav_right_of_way.core.detect import (AircraftState, DetectParams, Threat,
+ predict_threat)
+from uav_right_of_way.core.rules import RuleParams, decide
+
+
+class RightOfWayNode(Node):
+ def __init__(self) -> None:
+ super().__init__("uav_right_of_way_node")
+ self.declare_parameter("uav_ids", ["UAV-1", "UAV-2"])
+ self.declare_parameter("odom_prefix", "/uav")
+ self.declare_parameter("horizon_s", 8.0)
+ self.declare_parameter("cpa_threshold_m", 40.0)
+ self.declare_parameter("decision_interval_s", 0.2)
+ self.declare_parameter("clear_hold_s", 3.0)
+ self.declare_parameter("stale_timeout", 1.0)
+ self.declare_parameter("turn_right_deg", 30.0)
+ self.declare_parameter("descend_m", 10.0)
+ self.declare_parameter("slow_fraction", 0.30)
+ self.declare_parameter("overtake_lateral_m", 20.0)
+
+ ids = [str(v) for v in self.get_parameter("uav_ids").value]
+ prefix = str(self.get_parameter("odom_prefix").value).rstrip("/")
+ self.dp = DetectParams(
+ horizon_s=float(self.get_parameter("horizon_s").value),
+ cpa_threshold_m=float(self.get_parameter("cpa_threshold_m").value))
+ self.rp = RuleParams(
+ turn_right_deg=float(self.get_parameter("turn_right_deg").value),
+ descend_m=float(self.get_parameter("descend_m").value),
+ slow_fraction=float(self.get_parameter("slow_fraction").value),
+ overtake_lateral_m=float(
+ self.get_parameter("overtake_lateral_m").value))
+ self.decide_dt = float(self.get_parameter("decision_interval_s").value)
+ self.clear_hold = float(self.get_parameter("clear_hold_s").value)
+ self.stale = float(self.get_parameter("stale_timeout").value)
+
+ self.states: Dict[str, AircraftState] = {}
+ self.stamp: Dict[str, float] = {}
+ self.last_decide = -math.inf
+ # 机对 episode:{frozenset({a,b}): 最近确认威胁时刻}
+ self.episodes: Dict[frozenset, float] = {}
+
+ if len(ids) == 1:
+ topics = [("%s/odom" % prefix, ids[0])]
+ else:
+ topics = [("%s%d/odom" % (prefix, i + 1), uid)
+ for i, uid in enumerate(ids)]
+ for topic, uid in topics:
+ self.create_subscription(
+ Odometry, topic,
+ lambda msg, uid=uid: self._on_odom(uid, msg), 10)
+
+ self.pub_cmd = self.create_publisher(
+ String, "/uav_right_of_way/commands", 10)
+ self.create_timer(max(self.decide_dt, 0.05), self._tick)
+ self.get_logger().info(
+ "通行权决策已启动,机队 %s,预测 %.0f s 内 CPA < %.0f m 触发"
+ % (",".join(ids), self.dp.horizon_s, self.dp.cpa_threshold_m))
+
+ # ------------------------------------------------------------------ 回调
+ def _on_odom(self, uid: str, msg: Odometry) -> None:
+ p = msg.pose.pose.position
+ v = msg.twist.twist.linear
+ prio = 0
+ try:
+ prio = int(msg.child_frame_id.split("_")[-1]) \
+ if msg.child_frame_id.startswith("prio_") else 0
+ except Exception:
+ prio = 0
+ self.states[uid] = AircraftState(
+ uid, [p.x, p.y, p.z], [v.x, v.y, v.z], prio)
+ self.stamp[uid] = self.get_clock().now().nanoseconds * 1e-9
+
+ # ------------------------------------------------------------------ 周期
+ def _tick(self) -> None:
+ now = self.get_clock().now().nanoseconds * 1e-9
+ fresh = {uid: st for uid, st in self.states.items()
+ if now - self.stamp.get(uid, -math.inf) <= self.stale}
+ if len(fresh) < 2:
+ return
+ if now - self.last_decide < self.decide_dt - 1e-9:
+ return
+ self.last_decide = now
+
+ uids = sorted(fresh)
+ decisions: List[dict] = []
+ for i, a in enumerate(uids):
+ for b in uids[i + 1:]:
+ key = frozenset((a, b))
+ own, other = fresh[a], fresh[b]
+ th = predict_threat(own, other, self.dp)
+ if th is None:
+ # 威胁消失且超过观察期 → 结束该机对 episode
+ if key in self.episodes and \
+ now - self.episodes[key] >= self.clear_hold:
+ del self.episodes[key]
+ continue
+ # CPA 对双方对称;每个机对 episode 只产出一次决策
+ is_new = key not in self.episodes
+ self.episodes[key] = now
+ if is_new:
+ decisions.append(
+ decide(own, other, th, now, self.rp).as_dict())
+ decisions.append(
+ decide(other, own,
+ Threat(own.uid, th.cpa_t, th.cpa_dist),
+ now, self.rp).as_dict())
+
+ if decisions:
+ payload = dict(stamp=now, decisions=decisions)
+ self.pub_cmd.publish(String(data=json.dumps(
+ payload, ensure_ascii=False)))
+ for d in decisions:
+ self.get_logger().info(
+ "让路指令 %s ← %s [%s] %s:%s"
+ % (d["uid"], d["other"], d["rule"],
+ d["action"]["kind"], d["reason"]))
+
+
+def main(args=None) -> None:
+ rclpy.init(args=args)
+ node = RightOfWayNode()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()