diff --git a/docs/ground/rollover_margin.md b/docs/ground/rollover_margin.md
new file mode 100644
index 0000000000..2545caab56
--- /dev/null
+++ b/docs/ground/rollover_margin.md
@@ -0,0 +1,188 @@
+# 横向载荷转移与侧翻裕度
+
+## 这一页解决什么问题
+
+「这车会不会翻」听起来像仿真问题,准静态下其实只由三个参数决定:轮距、质心高度、质量。
+本页给出纯代数、可断言、不依赖 CARLA 的实现,并把 SSF、临界加速度、内外侧轮垂向力、
+极限车速算成具体数字,在终端里画出横向载荷条与车速扫描表。
+
+## 模型
+
+```
+SSF = track / (2·h_cg) 静态稳定因子
+a_crit = SSF · g 侧翻临界侧向加速度
+ΔF_z = m · a_y · h_cg / track 横向载荷转移
+F_inner = m·g/2 − ΔF_z
+F_outer = m·g/2 + ΔF_z
+v_max = sqrt(a_crit · R) 给定半径的极限车速
+```
+
+两个自洽关系(也是断言的依据):
+
+- 抬轮发生在 `ΔF_z = m·g/2`,即 `a_y = a_crit`;
+- `ΔF_z / F_z0` 恒等于 `a_y / a_crit`。
+
+注意 `F_inner + F_outer = m·g` 始终成立(垂向力守恒),这是模型自检的第一条。
+
+## 默认参数下的数字
+
+车辆:轮距 `1.54 m`、质心高 `0.55 m`、质量 `1500 kg`;工况:半径 `30 m`、车速 `60 km/h`。
+
+| 量 | 值 |
+|---|---|
+| SSF | `1.4000` |
+| 临界加速度 | `13.734 m/s²`(1.40 g) |
+| 侧向加速度 | `9.259 m/s²`(`v²/R`) |
+| 裕度 `a_y/a_crit` | `0.674` |
+| 载荷转移 | `4960.3 N`(占静态 67.4%) |
+| 内侧轮 | `2397.2 N` |
+| 外侧轮 | `12317.8 N` |
+| 极限车速 | `73.1 km/h`(余量 13.1 km/h) |
+
+车速扫描(R = 30 m):
+
+| 车速 | `a_y` | 裕度 | 内侧轮 | 抬轮 |
+|---|---|---|---|---|
+| 30 | 2.315 | 0.169 | 6117.4 | 否 |
+| 40 | 4.115 | 0.300 | 5152.9 | 否 |
+| 50 | 6.430 | 0.468 | 3912.8 | 否 |
+| 60 | 9.259 | 0.674 | 2397.2 | 否 |
+| 70 | 12.603 | 0.918 | 606.0 | 否 |
+| 80 | 16.461 | 1.199 | −1460.8 | **是** |
+| 90 | 20.833 | 1.517 | −3803.2 | **是** |
+
+抬轮发生在 70~80 km/h 之间,与极限车速 73.1 km/h 吻合。
+
+## 四条断言
+
+| 断言 | 判据 | 失败意味着 |
+|---|---|---|
+| **侧翻裕度** | `a_y/a_crit ≤ rollover_margin`(默认 0.70) | 过弯太快或车太高 |
+| **内侧轮载荷** | `F_inner > 0` | 已抬轮,准静态模型失效 |
+| **极限车速** | `v ≤ sqrt(a_crit·R)` | 车速超过该半径的物理上限 |
+| **载荷转移** | `|ΔF_z/F_z0 − a_y/a_crit| ≤ tolerance` | 模型实现不自洽 |
+
+前三条描述安全性,第四条描述实现正确性 —— 所以抬高质心会让前三条一起翻,第四条依然通过。
+这种「安全类断言联动、自洽类断言独立」的分工,正是把物理关系做成断言的价值。
+
+## 参数
+
+| 参数 | 默认 | 说明 |
+|---|---|---|
+| `vehicle/track` | `1.54` | 轮距(m) |
+| `vehicle/cg_height` | `0.55` | 质心高度(m) |
+| `vehicle/mass` | `1500.0` | 质量(kg) |
+| `vehicle/gravity` | `9.81` | 重力加速度 |
+| `corner/radius` | `30.0` | 弯道半径(m) |
+| `corner/speed_kmh` | `60.0` | 过弯车速(km/h) |
+| `check/rollover_margin` | `0.70` | 裕度上限 |
+| `check/transfer_tolerance` | `0.02` | 自洽性容差 |
+| `rate/publish_hz` | `10.0` | 挂起循环频率 |
+| `topic/report` | `/rollover/report` | 报告 |
+| `topic/loads` | `/rollover/loads` | 车速扫描 CSV |
+| `topic/critical_speed` | `/rollover/critical_speed` | 极限车速 |
+
+## 话题
+
+| 话题 | 类型 | 内容 |
+|---|---|---|
+| `/rollover/report` | `std_msgs/String`(latched) | 完整报告(含载荷条与扫描表) |
+| `/rollover/loads` | `std_msgs/String`(latched) | `speed_kmh,lateral_accel,inner_load,outer_load,lift_off` 共 7 行 |
+| `/rollover/critical_speed` | `std_msgs/Float64`(latched) | 该半径极限车速(km/h) |
+
+## 运行
+
+```bash
+cd ~/catkin_ws17
+catkin_make
+source devel/setup.bash
+roslaunch rollover_margin main.launch
+```
+
+## 实际输出(默认)
+
+```
+车辆:轮距 1.54 m,质心高 0.55 m,质量 1500 kg
+SSF = 1.4000,侧翻临界加速度 13.734 m/s²(1.40 g)
+工况:半径 30.0 m,车速 60.0 km/h(16.67 m/s)
+侧向加速度 9.259 m/s²,裕度比值 0.674
+载荷转移 4960.3 N(占比 0.674),内侧 2397.2 N / 外侧 12317.8 N
+该半径极限车速 73.1 km/h(余量 13.1 km/h)
+断言:4 通过 / 0 失败(共 4 条)
+ [PASS] 侧翻裕度 a_y 9.259 m/s²,a_crit 13.734 m/s²,比值 0.674(上限 0.70)
+ [PASS] 内侧轮载荷 内侧轮 2397.2 N,外侧轮 12317.8 N(静态 7357.5 N)
+ [PASS] 极限车速 当前 60.0 km/h,该半径极限 73.1 km/h(余量 13.1 km/h)
+ [PASS] 载荷转移 ΔF_z/F_z0 = 0.6742,a_y/a_crit = 0.6742
+结论:PASSED
+```
+
+## 两个反例
+
+**反例一:裕度上限收到 0.5** —— 只翻第一条
+
+```bash
+roslaunch rollover_margin main.launch rollover_margin:=0.5
+```
+
+```
+[FAIL] 侧翻裕度 a_y 9.259 m/s²,a_crit 13.734 m/s²,比值 0.674(上限 0.50)
+断言:3 通过 / 1 失败
+结论:FAILED
+```
+
+**反例二:质心抬到 0.90 m** —— 安全相关的三条一起翻
+
+```bash
+roslaunch rollover_margin main.launch cg_height:=0.90
+```
+
+```
+SSF = 0.8556,侧翻临界加速度 8.393 m/s²(0.86 g)
+[FAIL] 侧翻裕度 a_y 9.259 m/s²,a_crit 8.393 m/s²,比值 1.103(上限 0.70)
+[FAIL] 内侧轮载荷 内侧轮 -759.4 N,外侧轮 15474.4 N(静态 7357.5 N)
+[FAIL] 极限车速 当前 60.0 km/h,该半径极限 57.1 km/h(余量 -2.9 km/h)
+[PASS] 载荷转移 ΔF_z/F_z0 = 1.1032,a_y/a_crit = 1.1032
+断言:1 通过 / 3 失败
+结论:FAILED
+```
+
+内侧轮载荷为负,物理含义是这一侧已经离地 —— 准静态模型到此为止。
+
+## 离线测试
+
+```bash
+cd ~/catkin_ws17
+python3 tests/test_rollover_margin_local.py
+```
+
+只依赖标准库;在仓库根目录执行时还会校验本页是否存在、断言名是否齐全。
+
+## 与已有模块的分工
+
+| 模块 | 关心什么 |
+|---|---|
+| `tire_slip_curve` | 附着系数从哪来 |
+| `braking_distance_check` | 纵向距离 |
+| `turning_sweep_check` | 转弯时的几何包络(车身占多宽) |
+| **`rollover_margin`(本页)** | **横向载荷与稳定性(会不会抬轮、极限多快)** |
+
+`turning_sweep_check` 回答「这个弯能不能转过去(不压线)」,本页回答「这个弯能不能转过去
+(不翻车)」—— 一个管空间,一个管稳定性。
+
+## 常见问题
+
+**Q:为什么不建模悬架与轮胎侧偏?**
+A:准静态阈值主要由 `track/2h_cg` 决定,悬架柔度一般让实际阈值低 5~10%。本页要把公式
+讲清楚,闭式解更容易校验;真车标定时把 `rollover_margin` 收到 0.6 即可覆盖这一偏差。
+
+**Q:为什么「极限车速」和「侧翻裕度」会同时失败?**
+A:二者物理同源:裕度到 1 时 `v = sqrt(a_crit·R)` 刚好达到。所以抬高质心会让它们一起翻,
+这不是实现问题,而是模型的必然结论。
+
+**Q:能不能直接给一个「安全车速」?**
+A:可以,`safe_speed(margin)` 就是干这个的:默认裕度 0.7 时该车在该半径上的安全车速是
+`61.1 km/h`(更保守的 0.5 对应 `51.7 km/h`)。
+
+---
+
+所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。
diff --git a/docs/index.md b/docs/index.md
index beaa3a6c05..1d2a40605f 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -243,6 +243,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c
* [VRU 冲突风险图](./ground/vru_conflict_risk_map.md) —— 按时间对齐自车与行人、儿童和骑行者轨迹,输出最小距离、TTC 和风险等级
* [里程计健康监视(打滑 / 堵转 / 反向检测)](./ground/odom_health_monitor.md) —— 比较 `/cmd_vel` 与 `/odom` 的速度比值,识别打滑、堵转与反向运动
* [轮胎滑移率-附着曲线](./ground/tire_slip_curve.md) —— 用简化刷子轮胎模型输出滑移率与附着系数关系,标出峰值附着与 ABS 目标滑移带
+* [横向载荷转移与侧翻裕度](./ground/rollover_margin.md) —— 用 SSF、临界侧向加速度和内外侧轮垂向力算「这车会不会翻」,纯代数、不依赖 CARLA
## 5. 空域载具
diff --git a/mkdocs.yml b/mkdocs.yml
index 98320ade86..820df82230 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -237,6 +237,7 @@ nav:
- VRU 冲突风险图: './ground/vru_conflict_risk_map.md'
- 里程计健康监视: './ground/odom_health_monitor.md'
- 轮胎滑移率-附着曲线: './ground/tire_slip_curve.md'
+ - 横向载荷转移与侧翻裕度: './ground/rollover_margin.md'
- 空域载具:
- MuJoCo 城市屋顶光伏覆盖率测量: './air/mujoco_uav_rooftop_pv_coverage_survey.md'
- MuJoCo 屋顶光伏覆盖率测量第二天: './air/mujoco_uav_rooftop_pv_coverage_survey_day2.md'
diff --git a/src/ground/rollover_margin/CMakeLists.txt b/src/ground/rollover_margin/CMakeLists.txt
new file mode 100644
index 0000000000..ce0a7fec4f
--- /dev/null
+++ b/src/ground/rollover_margin/CMakeLists.txt
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rollover_margin)
+
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ std_msgs
+)
+
+catkin_package()
+
+catkin_install_python(PROGRAMS
+ scripts/main.py
+ scripts/rollover_core.py
+ scripts/rollover_node.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch config
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS
+)
diff --git a/src/ground/rollover_margin/README.md b/src/ground/rollover_margin/README.md
new file mode 100644
index 0000000000..5ac6cb9cc4
--- /dev/null
+++ b/src/ground/rollover_margin/README.md
@@ -0,0 +1,157 @@
+# rollover_margin —— 横向载荷转移与侧翻裕度
+
+一辆车以 60 km/h 过半径 30 m 的弯,内侧车轮还剩多少垂向力?再快多少会抬轮?
+这个包用三个参数(轮距、质心高度、质量)把答案算成一组可以断言的数字。
+
+纯标准库,不依赖 CARLA / numpy / GUI;Melodic(Python 2.7)与 Noetic(Python 3)都能跑。
+
+## 为什么需要这个模块
+
+侧翻不是「车太轻」或「路太滑」,而是**横向载荷转移超过了内侧轮的静态载荷**。
+准静态下只有三个量在起作用:
+
+```
+静态稳定因子 SSF = track / (2·h_cg)
+侧翻临界加速度 a_crit = SSF · g
+载荷转移 ΔF_z = m·a_y·h_cg / track
+```
+
+于是内侧轮 `F_inner = m·g/2 − ΔF_z`,抬轮恰好发生在 `ΔF_z = m·g/2`,
+即 `a_y = g·track/(2h_cg) = a_crit` —— 两条定义自洽,`ΔF_z/F_z0` 恒等于 `a_y/a_crit`。
+给定半径还能反推极限车速 `v_max = sqrt(a_crit·R)`。
+
+这些全是代数式,可以被断言精确校验,不需要仿真器。
+
+## 默认参数下的数字
+
+车型:轮距 `1.54 m`、质心高 `0.55 m`、质量 `1500 kg`;工况:半径 `30 m`、车速 `60 km/h`。
+
+| 量 | 值 |
+|---|---|
+| SSF | `1.4000` |
+| 临界加速度 | `13.734 m/s²`(1.40 g) |
+| 侧向加速度 | `9.259 m/s²` |
+| 裕度 `a_y/a_crit` | `0.674` |
+| 载荷转移 | `4960.3 N`(占静态 67.4%) |
+| 内侧轮 / 外侧轮 | `2397.2 N` / `12317.8 N` |
+| 该半径极限车速 | `73.1 km/h`(余量 13.1 km/h) |
+
+车速扫描(R = 30 m):
+
+| 车速 | `a_y` | 裕度 | 内侧轮 | 抬轮 |
+|---|---|---|---|---|
+| 30 | 2.315 | 0.169 | 6117.4 | 否 |
+| 40 | 4.115 | 0.300 | 5152.9 | 否 |
+| 50 | 6.430 | 0.468 | 3912.8 | 否 |
+| 60 | 9.259 | 0.674 | 2397.2 | 否 |
+| 70 | 12.603 | 0.918 | 606.0 | 否 |
+| 80 | 16.461 | 1.199 | −1460.8 | **是** |
+| 90 | 20.833 | 1.517 | −3803.2 | **是** |
+
+抬轮落在 70~80 km/h 之间,与极限车速 73.1 km/h 吻合。
+
+## 四条断言
+
+| 断言 | 判据 | 失败意味着 |
+|---|---|---|
+| **侧翻裕度** | `a_y/a_crit ≤ rollover_margin`(默认 0.70) | 过弯太快或车太高 |
+| **内侧轮载荷** | `F_inner > 0` | 已抬轮,准静态模型失效 |
+| **极限车速** | `v ≤ sqrt(a_crit·R)` | 超过该半径的物理上限 |
+| **载荷转移** | `|ΔF_z/F_z0 − a_y/a_crit| ≤ tolerance` | 实现不自洽 |
+
+前三条讲安全性,第四条讲实现正确性 —— 所以抬高质心会让前三条一起翻,第四条仍然通过。
+
+## 参数
+
+| 参数 | 默认 | 说明 |
+|---|---|---|
+| `vehicle/track` | `1.54` | 轮距(m) |
+| `vehicle/cg_height` | `0.55` | 质心高度(m) |
+| `vehicle/mass` | `1500.0` | 质量(kg) |
+| `vehicle/gravity` | `9.81` | 重力加速度 |
+| `corner/radius` | `30.0` | 弯道半径(m) |
+| `corner/speed_kmh` | `60.0` | 过弯车速(km/h) |
+| `check/rollover_margin` | `0.70` | 裕度上限 |
+| `check/transfer_tolerance` | `0.02` | 自洽性容差 |
+| `rate/publish_hz` | `10.0` | 打印完保持挂起的循环频率 |
+| `topic/report` | `/rollover/report` | 文本报告(latched) |
+| `topic/loads` | `/rollover/loads` | 车速扫描 CSV(latched) |
+| `topic/critical_speed` | `/rollover/critical_speed` | 极限车速 Float64(km/h) |
+
+## 运行
+
+```bash
+roslaunch rollover_margin main.launch
+roslaunch rollover_margin main.launch speed_kmh:=80
+roslaunch rollover_margin main.launch rollover_margin:=0.5
+roslaunch rollover_margin main.launch cg_height:=0.90
+```
+
+打印完节点保持挂起,方便另一个终端核对:
+
+```bash
+rosparam get /vehicle/track
+rosparam get /check/rollover_margin
+rosparam get /vehicle/cg_height
+```
+
+## 实测输出(默认)
+
+```
+车辆:轮距 1.54 m,质心高 0.55 m,质量 1500 kg
+SSF = 1.4000,侧翻临界加速度 13.734 m/s²(1.40 g)
+工况:半径 30.0 m,车速 60.0 km/h(16.67 m/s)
+侧向加速度 9.259 m/s²,裕度比值 0.674
+载荷转移 4960.3 N(占比 0.674),内侧 2397.2 N / 外侧 12317.8 N
+该半径极限车速 73.1 km/h(余量 13.1 km/h)
+断言:4 通过 / 0 失败(共 4 条)
+ [PASS] 侧翻裕度 a_y 9.259 m/s²,a_crit 13.734 m/s²,比值 0.674(上限 0.70)
+ [PASS] 内侧轮载荷 内侧轮 2397.2 N,外侧轮 12317.8 N(静态 7357.5 N)
+ [PASS] 极限车速 当前 60.0 km/h,该半径极限 73.1 km/h(余量 13.1 km/h)
+ [PASS] 载荷转移 ΔF_z/F_z0 = 0.6742,a_y/a_crit = 0.6742
+结论:PASSED
+```
+
+## 两个反例
+
+```bash
+roslaunch rollover_margin main.launch rollover_margin:=0.5 # 只翻「侧翻裕度」
+roslaunch rollover_margin main.launch cg_height:=0.90 # 裕度 + 内侧轮 + 极限车速 一起翻
+```
+
+第二个更有意思:质心抬到 0.90 m 后 SSF 掉到 `0.8556`,临界加速度 `8.393 m/s²`
+低于 60 km/h 所需的 `9.259`,于是内侧轮载荷变成 **−759.4 N**(物理上已经翻了),
+极限车速掉到 `57.1 km/h`;而「载荷转移」依然通过,因为它校验的是模型自洽而非安全性。
+
+## 离线测试
+
+```bash
+cd ~/catkin_ws17
+python3 tests/test_rollover_margin_local.py
+```
+
+不需要 ROS / CARLA / 显示器。
+
+## 与已有模块的分工
+
+| 模块 | 关心什么 |
+|---|---|
+| `tire_slip_curve` | 附着系数从哪来(滑移率决定 μ) |
+| `braking_distance_check` | 纵向:从看到到停住要几米 |
+| `turning_sweep_check` | 转弯时车身占多宽(几何包络) |
+| **`rollover_margin`(本包)** | **横向:载荷转移多少、何时抬轮、极限车速** |
+
+`turning_sweep_check` 回答「这个弯转得过去吗(不压线)」,本包回答「这个弯转得过去吗(不翻车)」。
+
+## 常见问题
+
+**Q:为什么不建模悬架和轮胎侧偏?**
+A:准静态阈值主要由 `track/2h_cg` 决定,悬架柔度只让实际阈值低 5~10%。本包要把公式讲清楚,
+闭式解更容易校验;真车标定时把 `rollover_margin` 收到 0.6 即可覆盖这一偏差。
+
+**Q:内侧轮载荷为负代表什么?**
+A:代表该侧车轮已离地,准静态模型到此失效(真实车辆此时绕外侧轮倾覆)。所以断言只判 `> 0`。
+
+---
+
+所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。
diff --git a/src/ground/rollover_margin/config/rollover_margin.yaml b/src/ground/rollover_margin/config/rollover_margin.yaml
new file mode 100644
index 0000000000..40c46da283
--- /dev/null
+++ b/src/ground/rollover_margin/config/rollover_margin.yaml
@@ -0,0 +1,25 @@
+# 横向载荷转移与侧翻裕度 —— 默认参数
+# 都能在 roslaunch 命令行用同名 覆盖,例如:
+# roslaunch rollover_margin main.launch speed_kmh:=80 cg_height:=0.90
+
+vehicle:
+ track: 1.54 # 轮距(m)
+ cg_height: 0.55 # 质心高度(m)
+ mass: 1500.0 # 整车质量(kg)
+ gravity: 9.81 # 重力加速度(m/s²)
+
+corner:
+ radius: 30.0 # 弯道半径(m)
+ speed_kmh: 60.0 # 过弯车速(km/h)
+
+check:
+ rollover_margin: 0.70 # 允许的 a_y / a_crit 上限
+ transfer_tolerance: 0.02 # 载荷转移自洽性容差
+
+rate:
+ publish_hz: 10.0
+
+topic:
+ report: /rollover/report
+ loads: /rollover/loads
+ critical_speed: /rollover/critical_speed
diff --git a/src/ground/rollover_margin/launch/main.launch b/src/ground/rollover_margin/launch/main.launch
new file mode 100644
index 0000000000..02ee5da9a6
--- /dev/null
+++ b/src/ground/rollover_margin/launch/main.launch
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/ground/rollover_margin/package.xml b/src/ground/rollover_margin/package.xml
new file mode 100644
index 0000000000..c6fdfe9deb
--- /dev/null
+++ b/src/ground/rollover_margin/package.xml
@@ -0,0 +1,19 @@
+
+
+ rollover_margin
+ 1.0.0
+
+ 横向载荷转移与侧翻裕度核算:由轮距、质心高度与过弯工况算出静态稳定因子 SSF、
+ 侧翻临界侧向加速度、内外侧轮垂向力与极限车速。纯标准库,不依赖 CARLA / numpy / GPU。
+
+
+ Wist-sutton
+ MIT
+
+ catkin
+
+ rospy
+ std_msgs
+
+
+
diff --git a/src/ground/rollover_margin/scripts/main.py b/src/ground/rollover_margin/scripts/main.py
new file mode 100644
index 0000000000..5555980bc7
--- /dev/null
+++ b/src/ground/rollover_margin/scripts/main.py
@@ -0,0 +1,15 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""入口:把脚本目录加进 sys.path 后转交给 rollover_node.main。"""
+from __future__ import division, print_function
+
+import os
+import sys
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
+
+from rollover_node import main # noqa: E402
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/ground/rollover_margin/scripts/rollover_core.py b/src/ground/rollover_margin/scripts/rollover_core.py
new file mode 100644
index 0000000000..2bbd25cbe9
--- /dev/null
+++ b/src/ground/rollover_margin/scripts/rollover_core.py
@@ -0,0 +1,417 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""横向载荷转移与侧翻裕度核算(纯标准库,不依赖 ROS / CARLA / numpy)。
+
+问题:一辆车以速度 v 过半径 R 的弯,会不会翻?
+
+准静态模型只有三个量:
+
+ 静态稳定因子 SSF = track / (2 · h_cg)
+ 侧翻临界加速度 a_crit = SSF · g
+ 轮距侧载荷转移 ΔF_z = m · a_y · h_cg / track
+
+于是:
+
+ 内侧轮垂向力 F_inner = m·g/2 − ΔF_z
+ 外侧轮垂向力 F_outer = m·g/2 + ΔF_z
+ 抬轮(内侧轮离地)恰好发生在 ΔF_z = m·g/2,
+ 也就是 a_y = g · track / (2·h_cg) = a_crit —— 两条定义自洽。
+
+给定弯道半径还能反推极限车速 v_max = sqrt(a_crit · R)。
+这套关系全部是代数式,可以被断言精确校验,不需要仿真器。
+
+Python 2.7 / 3.x 双兼容。
+"""
+from __future__ import division, print_function, unicode_literals
+
+import io
+import math
+
+__all__ = [
+ "RolloverError", "VehicleSpec", "Cornering", "check_cornering", "all_passed",
+ "speed_table", "render_loads", "format_report", "RolloverParams",
+ "PARAM_KEYS", "PARAM_DEFAULTS", "ASSERT_NAMES", "SPEED_TABLE_KMH",
+ "to_message_text",
+]
+
+# ── 默认参数 ─────────────────────────────────────────────────────────────
+
+DEFAULT_TRACK = 1.54 # 轮距(m)
+DEFAULT_CG_HEIGHT = 0.55 # 质心高度(m)
+DEFAULT_MASS = 1500.0 # 整车质量(kg)
+DEFAULT_CURVE_RADIUS = 30.0 # 弯道半径(m)
+DEFAULT_SPEED_KMH = 60.0 # 过弯车速(km/h)
+DEFAULT_ROLLOVER_MARGIN = 0.70 # 允许的 a_y / a_crit 上限
+DEFAULT_TRANSFER_TOLERANCE = 0.02
+DEFAULT_GRAVITY = 9.81
+DEFAULT_PUBLISH_HZ = 10.0
+DEFAULT_GRID_WIDTH = 60
+DEFAULT_GRID_HEIGHT = 12
+
+SPEED_TABLE_KMH = (30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0)
+
+ASSERT_NAMES = (u"侧翻裕度", u"内侧轮载荷", u"极限车速", u"载荷转移")
+
+PARAM_DEFAULTS = {
+ "vehicle/track": DEFAULT_TRACK,
+ "vehicle/cg_height": DEFAULT_CG_HEIGHT,
+ "vehicle/mass": DEFAULT_MASS,
+ "vehicle/gravity": DEFAULT_GRAVITY,
+ "corner/radius": DEFAULT_CURVE_RADIUS,
+ "corner/speed_kmh": DEFAULT_SPEED_KMH,
+ "check/rollover_margin": DEFAULT_ROLLOVER_MARGIN,
+ "check/transfer_tolerance": DEFAULT_TRANSFER_TOLERANCE,
+ "rate/publish_hz": DEFAULT_PUBLISH_HZ,
+ "topic/report": u"/rollover/report",
+ "topic/loads": u"/rollover/loads",
+ "topic/critical_speed": u"/rollover/critical_speed",
+}
+
+PARAM_KEYS = tuple(sorted(PARAM_DEFAULTS))
+
+
+class RolloverError(ValueError):
+ """参数不合法。"""
+
+
+# ── 小工具 ───────────────────────────────────────────────────────────────
+
+def _as_float(value, name, default=None):
+ if value is None:
+ if default is None:
+ raise RolloverError(u"缺少参数:%s" % name)
+ return float(default)
+ if isinstance(value, bool):
+ raise RolloverError(u"参数 %s 不能是布尔值" % name)
+ try:
+ out = float(value)
+ except (TypeError, ValueError):
+ raise RolloverError(u"参数 %s 不是数字:%r" % (name, value))
+ if math.isnan(out) or math.isinf(out):
+ raise RolloverError(u"参数 %s 不是有限数:%r" % (name, value))
+ return out
+
+
+def _positive(value, name):
+ out = _as_float(value, name)
+ if out <= 0.0:
+ raise RolloverError(u"参数 %s 必须为正:%r" % (name, value))
+ return out
+
+
+def _non_negative(value, name):
+ out = _as_float(value, name)
+ if out < 0.0:
+ raise RolloverError(u"参数 %s 不能为负:%r" % (name, value))
+ return out
+
+
+def to_message_text(text):
+ try:
+ unicode # noqa: F821
+ except NameError:
+ return text
+ if isinstance(text, unicode): # noqa: F821
+ return text.encode("utf-8")
+ return text
+
+
+# ── 数据模型 ─────────────────────────────────────────────────────────────
+
+class VehicleSpec(object):
+ """车辆的准静态侧倾相关几何与质量参数。"""
+
+ __slots__ = ("track", "cg_height", "mass", "gravity")
+
+ def __init__(self, track=DEFAULT_TRACK, cg_height=DEFAULT_CG_HEIGHT,
+ mass=DEFAULT_MASS, gravity=DEFAULT_GRAVITY):
+ self.track = _positive(track, "track")
+ self.cg_height = _positive(cg_height, "cg_height")
+ self.mass = _positive(mass, "mass")
+ self.gravity = _positive(gravity, "gravity")
+
+ # 基本量 ------------------------------------------------------------
+
+ def ssf(self):
+ """静态稳定因子 SSF = track / (2·h_cg)。"""
+ return self.track / (2.0 * self.cg_height)
+
+ def critical_accel(self):
+ """侧翻临界侧向加速度(m/s²),等于 SSF·g。"""
+ return self.ssf() * self.gravity
+
+ def wheel_load_static(self):
+ """单侧静态垂向力(N)。"""
+ return self.mass * self.gravity / 2.0
+
+ def transfer(self, lateral_accel):
+ """横向载荷转移量 ΔF_z(N),正数表示从内侧移到外侧。"""
+ ay = _as_float(lateral_accel, "lateral_accel")
+ return self.mass * ay * self.cg_height / self.track
+
+ def inner_load(self, lateral_accel):
+ return self.wheel_load_static() - self.transfer(lateral_accel)
+
+ def outer_load(self, lateral_accel):
+ return self.wheel_load_static() + self.transfer(lateral_accel)
+
+ def critical_speed(self, radius):
+ """给定半径的极限过弯车速(m/s)。"""
+ radius = _positive(radius, "radius")
+ return math.sqrt(self.critical_accel() * radius)
+
+ def as_tuple(self):
+ return (self.track, self.cg_height, self.mass, self.gravity)
+
+
+class Cornering(object):
+ """一次过弯工况。"""
+
+ __slots__ = ("spec", "radius", "speed_kmh", "speed_mps")
+
+ def __init__(self, spec=None, radius=DEFAULT_CURVE_RADIUS,
+ speed_kmh=DEFAULT_SPEED_KMH):
+ self.spec = spec or VehicleSpec()
+ self.radius = _positive(radius, "radius")
+ self.speed_kmh = _as_float(speed_kmh, "speed_kmh")
+ if self.speed_kmh < 0.0:
+ raise RolloverError(u"车速不能为负:%r" % speed_kmh)
+ self.speed_mps = self.speed_kmh / 3.6
+
+ # 派生量 ------------------------------------------------------------
+
+ def lateral_accel(self):
+ return self.speed_mps * self.speed_mps / self.radius
+
+ def usage(self):
+ """a_y / a_crit,越小越安全;>= 1 表示已抬轮。"""
+ return self.lateral_accel() / self.spec.critical_accel()
+
+ def transfer(self):
+ return self.spec.transfer(self.lateral_accel())
+
+ def inner_load(self):
+ return self.spec.inner_load(self.lateral_accel())
+
+ def outer_load(self):
+ return self.spec.outer_load(self.lateral_accel())
+
+ def transfer_ratio(self):
+ """ΔF_z / F_z0,理论上恒等于 a_y / a_crit。"""
+ return self.transfer() / self.spec.wheel_load_static()
+
+ def lifts_off(self):
+ return self.inner_load() <= 0.0
+
+ def speed_limit(self):
+ return self.spec.critical_speed(self.radius)
+
+ def speed_limit_kmh(self):
+ return self.speed_limit() * 3.6
+
+ def headroom_kmh(self):
+ return self.speed_limit_kmh() - self.speed_kmh
+
+ def safe_speed(self, margin=DEFAULT_ROLLOVER_MARGIN):
+ """在给定裕度下允许的最高车速(km/h)。"""
+ margin = _positive(margin, "rollover_margin")
+ return math.sqrt(margin * self.spec.critical_accel() * self.radius) * 3.6
+
+ def as_row(self):
+ return (round(self.speed_kmh, 2), round(self.lateral_accel(), 4),
+ round(self.usage(), 4), bool(self.lifts_off()))
+
+
+# ── 四条断言 ─────────────────────────────────────────────────────────────
+
+def check_cornering(cornering, rollover_margin=DEFAULT_ROLLOVER_MARGIN,
+ transfer_tolerance=DEFAULT_TRANSFER_TOLERANCE):
+ """返回四条断言,结构为 {type, passed, detail}。"""
+ cornering = cornering or Cornering()
+ margin = _positive(rollover_margin, "rollover_margin")
+ tol = _as_float(transfer_tolerance, "transfer_tolerance")
+ spec = cornering.spec
+ usage = cornering.usage()
+ results = []
+
+ # 1) 侧翻裕度
+ results.append({
+ "type": ASSERT_NAMES[0],
+ "passed": bool(usage <= margin + 1e-12),
+ "detail": u"a_y %.3f m/s²,a_crit %.3f m/s²,比值 %.3f(上限 %.2f)" % (
+ cornering.lateral_accel(), spec.critical_accel(), usage, margin),
+ })
+
+ # 2) 内侧轮载荷
+ inner = cornering.inner_load()
+ results.append({
+ "type": ASSERT_NAMES[1],
+ "passed": bool(inner > 0.0),
+ "detail": u"内侧轮 %.1f N,外侧轮 %.1f N(静态 %.1f N)" % (
+ inner, cornering.outer_load(), spec.wheel_load_static()),
+ })
+
+ # 3) 极限车速
+ limit = cornering.speed_limit_kmh()
+ results.append({
+ "type": ASSERT_NAMES[2],
+ "passed": bool(cornering.speed_kmh <= limit + 1e-9),
+ "detail": u"当前 %.1f km/h,该半径极限 %.1f km/h(余量 %.1f km/h)" % (
+ cornering.speed_kmh, limit, cornering.headroom_kmh()),
+ })
+
+ # 4) 载荷转移自洽:ΔF_z/F_z0 应等于 a_y/a_crit
+ ratio = cornering.transfer_ratio()
+ gap = abs(ratio - usage)
+ results.append({
+ "type": ASSERT_NAMES[3],
+ "passed": bool(gap <= tol + 1e-12),
+ "detail": u"ΔF_z/F_z0 = %.4f,a_y/a_crit = %.4f,偏差 %.2e(容差 %.2f)" % (
+ ratio, usage, gap, tol),
+ })
+
+ return results
+
+
+def all_passed(results):
+ return all(item["passed"] for item in results)
+
+
+def speed_table(spec=None, radius=DEFAULT_CURVE_RADIUS, speeds=SPEED_TABLE_KMH):
+ """同一半径下、不同车速的裕度表。"""
+ spec = spec or VehicleSpec()
+ rows = []
+ for kmh in speeds:
+ item = Cornering(spec, radius, kmh)
+ rows.append(item)
+ return rows
+
+
+# ── 字符图 ───────────────────────────────────────────────────────────────
+
+def render_loads(cornering, width=DEFAULT_GRID_WIDTH, height=DEFAULT_GRID_HEIGHT):
+ """画两条横向载荷条:内侧(左)与外侧(右),中间是静态载荷中线。"""
+ width = int(_as_float(width, "width", DEFAULT_GRID_WIDTH))
+ height = int(_as_float(height, "height", DEFAULT_GRID_HEIGHT))
+ if width < 20 or height < 5:
+ raise RolloverError(u"网格太小:%dx%d" % (width, height))
+ spec = cornering.spec
+ total = spec.mass * spec.gravity
+ scale = max(total, 1e-9)
+ inner = cornering.inner_load()
+ outer = cornering.outer_load()
+ inner_cells = int(round(max(0.0, inner) / scale * (width // 2 - 2)))
+ outer_cells = int(round(max(0.0, outer) / scale * (width // 2 - 2)))
+
+ lines = []
+ mid = width // 2
+ for row in range(height):
+ cells = [" "] * width
+ cells[mid] = "|"
+ # 内侧(左)从中间往左画
+ if inner_cells > 0:
+ for k in range(inner_cells):
+ pos = mid - 1 - k
+ if pos >= 0:
+ cells[pos] = "#"
+ # 外侧(右)从中间往右画
+ if outer_cells > 0:
+ for k in range(outer_cells):
+ pos = mid + 1 + k
+ if pos < width:
+ cells[pos] = "="
+ lines.append("".join(cells))
+ lines.append(u"内侧 %5.0f N%s %.0f N 外侧" % (inner, " " * max(1, width // 2 - 16), outer))
+ return lines
+
+
+# ── 报告 ─────────────────────────────────────────────────────────────────
+
+def format_report(cornering, results, header=""):
+ cornering = cornering or Cornering()
+ spec = cornering.spec
+ passed = sum(1 for item in results if item["passed"])
+ lines = []
+ if header:
+ lines.append(header)
+ lines.append(u"车辆:轮距 %.2f m,质心高 %.2f m,质量 %.0f kg" % (
+ spec.track, spec.cg_height, spec.mass))
+ lines.append(u"SSF = %.4f,侧翻临界加速度 %.3f m/s²(%.2f g)" % (
+ spec.ssf(), spec.critical_accel(), spec.ssf()))
+ lines.append(u"工况:半径 %.1f m,车速 %.1f km/h(%.2f m/s)" % (
+ cornering.radius, cornering.speed_kmh, cornering.speed_mps))
+ lines.append(u"侧向加速度 %.3f m/s²,裕度比值 %.3f" % (
+ cornering.lateral_accel(), cornering.usage()))
+ lines.append(u"载荷转移 %.1f N(占比 %.3f),内侧 %.1f N / 外侧 %.1f N" % (
+ cornering.transfer(), cornering.transfer_ratio(),
+ cornering.inner_load(), cornering.outer_load()))
+ lines.append(u"该半径极限车速 %.1f km/h(余量 %.1f km/h)" % (
+ cornering.speed_limit_kmh(), cornering.headroom_kmh()))
+ lines.append(u"-" * 60)
+ lines.append(u"横向载荷(| 为静态中线,# 内侧,= 外侧):")
+ for line in render_loads(cornering):
+ lines.append(line)
+ lines.append(u"-" * 60)
+ lines.append(u"车速扫描(半径 %.1f m):" % cornering.radius)
+ lines.append(u" 车速(km/h) a_y(m/s²) 裕度 内侧轮(N) 是否抬轮")
+ for item in speed_table(spec, cornering.radius):
+ lines.append(u" %9.1f %9.3f %5.3f %9.1f %s" % (
+ item.speed_kmh, item.lateral_accel(), item.usage(),
+ item.inner_load(), u"是" if item.lifts_off() else u"否"))
+ lines.append(u"-" * 60)
+ lines.append(u"断言:%d 通过 / %d 失败(共 %d 条)" % (
+ passed, len(results) - passed, len(results)))
+ for item in results:
+ lines.append(u" [%s] %s %s" % (
+ "PASS" if item["passed"] else "FAIL", item["type"], item["detail"]))
+ lines.append(u"结论:%s" % (u"PASSED" if passed == len(results) else u"FAILED"))
+ return "\n".join(lines)
+
+
+# ── 参数读取 ─────────────────────────────────────────────────────────────
+
+class RolloverParams(object):
+ def __init__(self, **kwargs):
+ for name in PARAM_KEYS:
+ setattr(self, "params_" + name.replace("/", "_"),
+ kwargs.get(name, PARAM_DEFAULTS[name]))
+ self.track = _positive(self.params_vehicle_track, "vehicle/track")
+ self.cg_height = _positive(self.params_vehicle_cg_height, "vehicle/cg_height")
+ self.mass = _positive(self.params_vehicle_mass, "vehicle/mass")
+ self.gravity = _positive(self.params_vehicle_gravity, "vehicle/gravity")
+ self.radius = _positive(self.params_corner_radius, "corner/radius")
+ self.speed_kmh = _non_negative(self.params_corner_speed_kmh, "corner/speed_kmh")
+ self.rollover_margin = _positive(
+ self.params_check_rollover_margin, "check/rollover_margin")
+ self.transfer_tolerance = _non_negative(
+ self.params_check_transfer_tolerance, "check/transfer_tolerance")
+ self.publish_hz = _positive(self.params_rate_publish_hz, "rate/publish_hz")
+ self.topic_report = self.params_topic_report
+ self.topic_loads = self.params_topic_loads
+ self.topic_critical_speed = self.params_topic_critical_speed
+
+ @classmethod
+ def from_params(cls, get_param):
+ values = {}
+ for name in PARAM_KEYS:
+ values[name] = get_param(name, PARAM_DEFAULTS[name])
+ return cls(**values)
+
+ def vehicle(self):
+ return VehicleSpec(self.track, self.cg_height, self.mass, self.gravity)
+
+ def cornering(self):
+ return Cornering(self.vehicle(), self.radius, self.speed_kmh)
+
+ def as_dict(self):
+ return {
+ "vehicle/track": self.track,
+ "vehicle/cg_height": self.cg_height,
+ "vehicle/mass": self.mass,
+ "vehicle/gravity": self.gravity,
+ "corner/radius": self.radius,
+ "corner/speed_kmh": self.speed_kmh,
+ "check/rollover_margin": self.rollover_margin,
+ "check/transfer_tolerance": self.transfer_tolerance,
+ "rate/publish_hz": self.publish_hz,
+ }
diff --git a/src/ground/rollover_margin/scripts/rollover_node.py b/src/ground/rollover_margin/scripts/rollover_node.py
new file mode 100644
index 0000000000..95784268e2
--- /dev/null
+++ b/src/ground/rollover_margin/scripts/rollover_node.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""侧翻裕度节点的 ROS 封装(ROS 1 / Melodic,Python 2.7 & 3.x)。"""
+from __future__ import division, print_function, unicode_literals
+
+import os
+import sys
+import traceback
+
+import rospy
+from std_msgs.msg import Float64, String
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
+
+import rollover_core as core # noqa: E402
+
+
+def _log(level, message):
+ func = getattr(rospy, "log" + level)
+ try:
+ unicode # noqa: F821
+ except NameError:
+ func(message)
+ return
+ if isinstance(message, unicode): # noqa: F821
+ func(message.encode("utf-8"))
+ else:
+ func(message)
+
+
+class RolloverNode(object):
+ def __init__(self):
+ self.params = core.RolloverParams.from_params(rospy.get_param)
+ self.cornering = self.params.cornering()
+ self.results = core.check_cornering(
+ self.cornering, self.params.rollover_margin,
+ self.params.transfer_tolerance)
+ self.report = core.format_report(
+ self.cornering, self.results,
+ header=u"横向载荷转移与侧翻裕度(准静态,不依赖 CARLA)")
+
+ self.pub_report = rospy.Publisher(
+ self.params.topic_report, String, queue_size=1, latch=True)
+ self.pub_loads = rospy.Publisher(
+ self.params.topic_loads, String, queue_size=1, latch=True)
+ self.pub_speed = rospy.Publisher(
+ self.params.topic_critical_speed, Float64, queue_size=1, latch=True)
+ rospy.on_shutdown(self.on_shutdown)
+
+ def loads_text(self):
+ rows = [u"speed_kmh,lateral_accel,inner_load,outer_load,lift_off"]
+ for item in core.speed_table(self.cornering.spec, self.cornering.radius):
+ rows.append(u"%.1f,%.4f,%.1f,%.1f,%d" % (
+ item.speed_kmh, item.lateral_accel(),
+ item.inner_load(), item.outer_load(), 1 if item.lifts_off() else 0))
+ return u"\n".join(rows)
+
+ def publish_all(self):
+ self.pub_report.publish(String(data=core.to_message_text(self.report)))
+ self.pub_loads.publish(String(data=core.to_message_text(self.loads_text())))
+ self.pub_speed.publish(Float64(data=float(self.cornering.speed_limit_kmh())))
+
+ def on_shutdown(self):
+ try:
+ _log("info", u"已发布零控制量")
+ except Exception: # noqa: BLE001
+ pass
+
+ def run(self):
+ try:
+ spec = self.cornering.spec
+ _log("info", u"侧翻裕度节点已启动:轮距 %.2f m,质心高 %.2f m,质量 %.0f kg,"
+ u"半径 %.1f m,车速 %.1f km/h" % (
+ spec.track, spec.cg_height, spec.mass,
+ self.cornering.radius, self.cornering.speed_kmh))
+ _log("info", u"SSF=%.4f,临界加速度 %.3f m/s²,极限车速 %.1f km/h" % (
+ spec.ssf(), spec.critical_accel(), self.cornering.speed_limit_kmh()))
+ _log("info", u"发布 %s、%s、%s" % (
+ self.params.topic_report, self.params.topic_loads,
+ self.params.topic_critical_speed))
+ self.publish_all()
+ print(self.report)
+ passed = sum(1 for item in self.results if item["passed"])
+ _log("info", u"结论:%s(%d/%d 条断言通过)" % (
+ u"PASSED" if passed == len(self.results) else u"FAILED",
+ passed, len(self.results)))
+ rate = rospy.Rate(max(1.0, self.params.publish_hz))
+ while not rospy.is_shutdown():
+ rate.sleep()
+ return 0
+ except Exception: # noqa: BLE001
+ _log("err", u"侧翻裕度核算异常:%s" % traceback.format_exc())
+ return 1
+
+
+def main():
+ rospy.init_node("rollover_margin", anonymous=False)
+ node = RolloverNode()
+ code = node.run()
+ try:
+ node.publish_all()
+ except Exception: # noqa: BLE001
+ pass
+ return code
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/tests/test_rollover_margin_local.py b/tests/test_rollover_margin_local.py
new file mode 100644
index 0000000000..783fd2c66e
--- /dev/null
+++ b/tests/test_rollover_margin_local.py
@@ -0,0 +1,450 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""rollover_margin 的离线回归测试。
+
+特点:
+* 只用标准库,不需要 ROS、不需要 CARLA、不需要显示器;
+* 兼容 Python 2.7 与 Python 3.x;
+* 在仓库根目录执行:python3 tests/test_rollover_margin_local.py
+
+覆盖:A 参数 / B 车辆几何 / C 工况 / D 载荷 / E 四条断言 / F 车速扫描 /
+ G 字符图与报告 / H 参数读取 / I 工程文件 / J pytest 包装
+"""
+from __future__ import division, print_function
+
+import io
+import math
+import os
+import sys
+
+try:
+ text_type = unicode # noqa: F821
+except NameError:
+ text_type = str
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+ROOT = os.path.dirname(HERE)
+PKG = os.path.join(ROOT, "src", "ground", "rollover_margin")
+SCRIPTS = os.path.join(PKG, "scripts")
+DOC_PATH = os.path.join(ROOT, "docs", "ground", "rollover_margin.md")
+
+sys.path.insert(0, SCRIPTS)
+import rollover_core as core # noqa: E402
+
+PASSED = []
+FAILED = []
+
+
+def check(name, condition, detail=""):
+ if condition:
+ PASSED.append(name)
+ else:
+ FAILED.append(name if not detail else "%s(%s)" % (name, detail))
+
+
+def close(a, b, tol=1e-9):
+ return abs(a - b) <= tol
+
+
+def raises(expected, func, *args, **kwargs):
+ try:
+ func(*args, **kwargs)
+ except expected:
+ return True
+ except Exception: # noqa: BLE001
+ return False
+ return False
+
+
+# ── A 参数校验 ───────────────────────────────────────────────────────────
+
+def group_params():
+ check("A1 默认轮距 1.54", close(core.DEFAULT_TRACK, 1.54))
+ check("A2 默认质心高 0.55", close(core.DEFAULT_CG_HEIGHT, 0.55))
+ check("A3 默认质量 1500", close(core.DEFAULT_MASS, 1500.0))
+ check("A4 默认半径 30", close(core.DEFAULT_CURVE_RADIUS, 30.0))
+ check("A5 默认车速 60", close(core.DEFAULT_SPEED_KMH, 60.0))
+ check("A6 默认裕度 0.70", close(core.DEFAULT_ROLLOVER_MARGIN, 0.70))
+ check("A7 默认重力 9.81", close(core.DEFAULT_GRAVITY, 9.81))
+ check("A8 断言名 4 条", core.ASSERT_NAMES ==
+ (u"侧翻裕度", u"内侧轮载荷", u"极限车速", u"载荷转移"))
+ check("A9 PARAM_KEYS 12 个", len(core.PARAM_KEYS) == 12)
+ check("A10 车速表 7 档", len(core.SPEED_TABLE_KMH) == 7)
+ check("A11 车速表 30~90", core.SPEED_TABLE_KMH[0] == 30.0
+ and core.SPEED_TABLE_KMH[-1] == 90.0)
+
+ spec = core.VehicleSpec()
+ check("A12 默认构造", close(spec.track, 1.54) and close(spec.cg_height, 0.55))
+ check("A13 as_tuple 四项", len(spec.as_tuple()) == 4)
+ check("A14 轮距为 0 报错", raises(core.RolloverError, core.VehicleSpec, 0.0))
+ check("A15 质心高为负报错", raises(core.RolloverError, core.VehicleSpec, 1.54, -0.1))
+ check("A16 质量为 0 报错", raises(core.RolloverError, core.VehicleSpec, 1.54, 0.55, 0.0))
+ check("A17 非数字报错", raises(core.RolloverError, core.VehicleSpec, "abc"))
+ check("A18 布尔值报错", raises(core.RolloverError, core.VehicleSpec, True))
+ check("A19 NaN 报错", raises(core.RolloverError, core.VehicleSpec, float("nan")))
+ check("A20 字符串数字可用", close(core.VehicleSpec("1.6").track, 1.6))
+ check("A21 半径为 0 报错", raises(core.RolloverError, core.Cornering, None, 0.0))
+ check("A22 车速为负报错", raises(core.RolloverError, core.Cornering, None, 30.0, -5.0))
+ check("A23 裕度为 0 报错", raises(core.RolloverError, core.check_cornering,
+ core.Cornering(), 0.0))
+
+
+# ── B 车辆几何 ───────────────────────────────────────────────────────────
+
+def group_vehicle():
+ spec = core.VehicleSpec()
+ check("B1 SSF = 1.4", close(spec.ssf(), 1.4))
+ check("B2 SSF 公式 = track/(2h)", close(spec.ssf(), 1.54 / (2 * 0.55)))
+ check("B3 临界加速度 13.734", close(spec.critical_accel(), 13.734, 1e-9))
+ check("B4 临界加速度 = SSF·g", close(spec.critical_accel(), 1.4 * 9.81))
+ check("B5 单侧静态载荷 7357.5", close(spec.wheel_load_static(), 7357.5))
+ check("B6 静态载荷 = m·g/2", close(spec.wheel_load_static(), 1500 * 9.81 / 2))
+
+ check("B7 轮距变大 → SSF 变大",
+ core.VehicleSpec(1.8).ssf() > spec.ssf())
+ check("B8 质心变高 → SSF 变小",
+ core.VehicleSpec(1.54, 0.8).ssf() < spec.ssf())
+ check("B9 SSF 与质量无关",
+ close(core.VehicleSpec(1.54, 0.55, 800.0).ssf(), spec.ssf()))
+ check("B10 临界车速随半径增大",
+ spec.critical_speed(60.0) > spec.critical_speed(30.0))
+ check("B11 临界车速闭式解",
+ close(spec.critical_speed(30.0), math.sqrt(13.734 * 30.0), 1e-12))
+ check("B12 半径为 0 算临界车速报错",
+ raises(core.RolloverError, spec.critical_speed, 0.0))
+
+ # 质量不影响侧翻临界加速度,但影响载荷
+ heavy = core.VehicleSpec(1.54, 0.55, 3000.0)
+ check("B13 质量翻倍后载荷翻倍",
+ close(heavy.wheel_load_static(), 2 * spec.wheel_load_static()))
+ check("B14 质量翻倍后临界加速度不变",
+ close(heavy.critical_accel(), spec.critical_accel()))
+
+
+# ── C 工况 ───────────────────────────────────────────────────────────────
+
+def group_cornering():
+ spec = core.VehicleSpec()
+ item = core.Cornering(spec, 30.0, 60.0)
+ check("C1 速度换算 16.6667 m/s", close(item.speed_mps, 16.666666, 1e-6))
+ check("C2 侧向加速度 9.259259", close(item.lateral_accel(), 9.259259, 1e-6))
+ check("C3 a_y = v²/R", close(item.lateral_accel(), 16.666666 ** 2 / 30.0, 1e-5))
+ check("C4 裕度 0.674185", close(item.usage(), 0.674185, 1e-6))
+ check("C5 裕度 = a_y/a_crit",
+ close(item.usage(), item.lateral_accel() / spec.critical_accel()))
+ check("C6 极限车速 20.298276 m/s", close(item.speed_limit(), 20.298276, 1e-6))
+ check("C7 极限车速 73.073793 km/h", close(item.speed_limit_kmh(), 73.073793, 1e-6))
+ check("C8 余量 13.073793", close(item.headroom_kmh(), 13.073793, 1e-6))
+ check("C9 60 km/h 不抬轮", not item.lifts_off())
+ check("C10 安全车速(0.7) 61.137921", close(item.safe_speed(0.7), 61.137921, 1e-6))
+ check("C11 安全车速(0.5) 51.670974", close(item.safe_speed(0.5), 51.670974, 1e-6))
+ check("C12 安全车速与裕度单调",
+ item.safe_speed(0.9) > item.safe_speed(0.5))
+
+ fast = core.Cornering(spec, 30.0, 80.0)
+ check("C13 80 km/h 抬轮", fast.lifts_off())
+ check("C14 80 km/h 裕度 > 1", fast.usage() > 1.0)
+ check("C15 80 km/h 余量为负", fast.headroom_kmh() < 0.0)
+
+ check("C16 半径变大 → 侧向加速度变小",
+ core.Cornering(spec, 60.0, 60.0).lateral_accel() < item.lateral_accel())
+ check("C17 静止时 a_y = 0", close(core.Cornering(spec, 30.0, 0.0).lateral_accel(), 0.0))
+ check("C18 静止时裕度 0", close(core.Cornering(spec, 30.0, 0.0).usage(), 0.0))
+ check("C19 as_row 四元", len(item.as_row()) == 4)
+
+
+# ── D 载荷 ───────────────────────────────────────────────────────────────
+
+def group_loads():
+ spec = core.VehicleSpec()
+ item = core.Cornering(spec, 30.0, 60.0)
+ check("D1 转移量 4960.31746", close(item.transfer(), 4960.31746, 1e-5))
+ check("D2 转移量公式 m·a_y·h/t",
+ close(item.transfer(), 1500 * 9.259259 * 0.55 / 1.54, 1e-3))
+ check("D3 内侧 2397.18254", close(item.inner_load(), 2397.18254, 1e-4))
+ check("D4 外侧 12317.81746", close(item.outer_load(), 12317.81746, 1e-4))
+ check("D5 内外侧之和 = m·g",
+ close(item.inner_load() + item.outer_load(), 1500 * 9.81, 1e-9))
+ check("D6 转移占比 0.674185", close(item.transfer_ratio(), 0.674185, 1e-6))
+ check("D7 转移占比 = 裕度",
+ close(item.transfer_ratio(), item.usage(), 1e-12))
+
+ still = core.Cornering(spec, 30.0, 0.0)
+ check("D8 静止时转移量为 0", close(still.transfer(), 0.0))
+ check("D9 静止时内外侧相等",
+ close(still.inner_load(), still.outer_load()))
+
+ lifted = core.Cornering(spec, 30.0, 90.0)
+ check("D10 90 km/h 内侧为负", lifted.inner_load() < 0.0)
+ check("D11 90 km/h 转移量超静态载荷",
+ lifted.transfer() > spec.wheel_load_static())
+
+ # 抬轮临界点:把车速调到恰好 v_limit,内侧力≈0
+ at_limit = core.Cornering(spec, 30.0, item.speed_limit_kmh())
+ check("D12 极限车速时内侧≈0", abs(at_limit.inner_load()) < 1e-6,
+ "inner=%.6f" % at_limit.inner_load())
+ check("D13 极限车速时裕度≈1", close(at_limit.usage(), 1.0, 1e-9))
+
+
+# ── E 四条断言 ───────────────────────────────────────────────────────────
+
+def group_checks():
+ spec = core.VehicleSpec()
+ item = core.Cornering(spec, 30.0, 60.0)
+ results = core.check_cornering(item)
+ check("E1 返回 4 条", len(results) == 4)
+ check("E2 顺序与 ASSERT_NAMES 一致",
+ [r["type"] for r in results] == list(core.ASSERT_NAMES))
+ check("E3 每条含 passed/detail",
+ all("passed" in r and "detail" in r for r in results))
+ check("E4 默认全通过", core.all_passed(results))
+ check("E5 裕度通过", results[0]["passed"])
+ check("E6 内侧轮载荷通过", results[1]["passed"])
+ check("E7 极限车速通过", results[2]["passed"])
+ check("E8 载荷转移通过", results[3]["passed"])
+ check("E9 载荷转移偏差极小", "1e-1" in results[3]["detail"]
+ or "e-1" in results[3]["detail"])
+
+ # 反例 1:裕度上限收到 0.5,只翻第一条
+ res1 = core.check_cornering(item, rollover_margin=0.5)
+ check("E10 裕度 0.5 → 侧翻裕度失败", not res1[0]["passed"])
+ check("E11 裕度 0.5 → 其余三条仍通过",
+ all(r["passed"] for i, r in enumerate(res1) if i != 0))
+
+ # 反例 2:抬高质心,裕度/内侧轮/极限车速同时崩
+ tall = core.Cornering(core.VehicleSpec(cg_height=0.90), 30.0, 60.0)
+ res2 = core.check_cornering(tall)
+ check("E12 高质心 → 侧翻裕度失败", not res2[0]["passed"])
+ check("E13 高质心 → 内侧轮载荷失败", not res2[1]["passed"])
+ check("E14 高质心 → 极限车速失败", not res2[2]["passed"])
+ check("E15 高质心 → 载荷转移仍自洽", res2[3]["passed"])
+ check("E16 高质心 SSF 0.8556", close(tall.spec.ssf(), 0.855556, 1e-6))
+
+ # 反例 3:把转移容差设成不可能满足的值,只有第四条失败
+ res3 = core.check_cornering(item, transfer_tolerance=-1e-6)
+ check("E17 容差为负时第四条失败", not res3[3]["passed"])
+ check("E18 容差为负时前三条仍通过",
+ all(r["passed"] for i, r in enumerate(res3) if i != 3))
+
+ check("E19 all_passed 空为真", core.all_passed([]))
+ check("E20 all_passed 有失败为假",
+ not core.all_passed([{"type": "x", "passed": False, "detail": ""}]))
+
+
+# ── F 车速扫描 ───────────────────────────────────────────────────────────
+
+def group_table():
+ spec = core.VehicleSpec()
+ rows = core.speed_table(spec, 30.0)
+ check("F1 7 行", len(rows) == 7)
+ check("F2 首行 30 km/h", close(rows[0].speed_kmh, 30.0))
+ check("F3 末行 90 km/h", close(rows[-1].speed_kmh, 90.0))
+ check("F4 30 km/h 裕度 0.169", close(rows[0].usage(), 0.169, 1e-3))
+ check("F5 50 km/h 内侧 3912.8", close(rows[2].inner_load(), 3912.83, 1e-2))
+ check("F6 70 km/h 不抬轮但很接近", not rows[4].lifts_off()
+ and rows[4].usage() > 0.9)
+ check("F7 80 km/h 抬轮", rows[5].lifts_off())
+ check("F8 90 km/h 抬轮", rows[6].lifts_off())
+ check("F9 裕度随车速单调递增",
+ all(rows[i].usage() < rows[i + 1].usage() for i in range(6)))
+ check("F10 内侧载荷随车速单调递减",
+ all(rows[i].inner_load() > rows[i + 1].inner_load() for i in range(6)))
+ check("F11 抬轮发生在 70~80 之间",
+ not rows[4].lifts_off() and rows[5].lifts_off())
+ check("F12 半径 60 m 时 80 km/h 不抬轮",
+ not core.Cornering(spec, 60.0, 80.0).lifts_off())
+
+
+# ── G 字符图与报告 ───────────────────────────────────────────────────────
+
+def group_report():
+ spec = core.VehicleSpec()
+ item = core.Cornering(spec, 30.0, 60.0)
+ results = core.check_cornering(item)
+
+ lines = core.render_loads(item)
+ check("G1 字符图 13 行", len(lines) == core.DEFAULT_GRID_HEIGHT + 1)
+ check("G2 有外侧条", any("=" in line for line in lines[:-1]))
+ check("G3 有内侧条", any("#" in line for line in lines[:-1]))
+ check("G4 有中线", any("|" in line for line in lines[:-1]))
+ check("G5 脚注含两个载荷",
+ u"内侧" in lines[-1] and u"外侧" in lines[-1])
+ check("G6 网格太小报错",
+ raises(core.RolloverError, core.render_loads, item, 10, 3))
+ check("G7 自定义尺寸", len(core.render_loads(item, 40, 8)) == 9)
+
+ lifted = core.Cornering(spec, 30.0, 90.0)
+ lines_lift = core.render_loads(lifted)
+ check("G8 抬轮时内侧条消失",
+ not any("#" in line for line in lines_lift[:-1]))
+
+ report = core.format_report(item, results, header=u"测试标题")
+ check("G9 报告含标题", u"测试标题" in report)
+ check("G10 报告含 SSF", u"SSF = 1.4000" in report)
+ check("G11 报告含断言统计", u"断言:4 通过 / 0 失败" in report)
+ check("G12 报告含 PASSED", u"结论:PASSED" in report)
+ check("G13 报告含车速扫描表头", u"车速(km/h)" in report)
+ check("G14 报告含 7 行车速", report.count(u"km/h") >= 7)
+ check("G15 报告行数 > 30", len(report.splitlines()) > 30)
+
+ failed = core.check_cornering(item, rollover_margin=0.5)
+ report_fail = core.format_report(item, failed)
+ check("G16 失败报告含 [FAIL]", u"[FAIL]" in report_fail)
+ check("G17 失败报告 FAILED", u"结论:FAILED" in report_fail)
+ check("G18 失败统计 3 通过 / 1 失败", u"3 通过 / 1 失败" in report_fail)
+
+
+# ── H 参数读取 ───────────────────────────────────────────────────────────
+
+def group_params_read():
+ def getter(name, default=None):
+ return default
+
+ params = core.RolloverParams.from_params(getter)
+ check("H1 默认 track", close(params.track, 1.54))
+ check("H2 默认 cg_height", close(params.cg_height, 0.55))
+ check("H3 默认 radius", close(params.radius, 30.0))
+ check("H4 默认 speed", close(params.speed_kmh, 60.0))
+ check("H5 默认 margin", close(params.rollover_margin, 0.70))
+ check("H6 vehicle() 返回 VehicleSpec", isinstance(params.vehicle(), core.VehicleSpec))
+ check("H7 cornering() 返回 Cornering", isinstance(params.cornering(), core.Cornering))
+ check("H8 as_dict 9 项", len(params.as_dict()) == 9)
+
+ overrides = {
+ "vehicle/track": "1.70",
+ "vehicle/cg_height": "0.62",
+ "corner/speed_kmh": "75",
+ "check/rollover_margin": "0.65",
+ }
+
+ def getter2(name, default=None):
+ return overrides.get(name, default)
+
+ params2 = core.RolloverParams.from_params(getter2)
+ check("H9 覆盖 track", close(params2.track, 1.70))
+ check("H10 覆盖 cg_height", close(params2.cg_height, 0.62))
+ check("H11 覆盖 speed", close(params2.speed_kmh, 75.0))
+ check("H12 覆盖 margin", close(params2.rollover_margin, 0.65))
+ check("H13 未覆盖项回落默认", close(params2.radius, 30.0))
+
+ check("H14 非法 track 构造即报错",
+ raises(core.RolloverError, core.RolloverParams, **{"vehicle/track": 0.0}))
+ check("H15 负车速报错",
+ raises(core.RolloverError, core.RolloverParams, **{"corner/speed_kmh": -1}))
+ check("H16 字符串数字可用",
+ close(core.RolloverParams(**{"vehicle/track": "1.6"}).track, 1.6))
+
+
+# ── I 工程文件 ───────────────────────────────────────────────────────────
+
+def group_project():
+ pkg_xml = os.path.join(PKG, "package.xml")
+ cmake = os.path.join(PKG, "CMakeLists.txt")
+ yaml_path = os.path.join(PKG, "config", "rollover_margin.yaml")
+ launch_path = os.path.join(PKG, "launch", "main.launch")
+ readme = os.path.join(PKG, "README.md")
+
+ for path, name in ((pkg_xml, "I1 package.xml"), (cmake, "I2 CMakeLists.txt"),
+ (yaml_path, "I3 config yaml"), (launch_path, "I4 launch"),
+ (readme, "I5 README")):
+ check(name + " 存在", os.path.isfile(path), path)
+
+ pkg_text = io.open(pkg_xml, "r", encoding="utf-8").read()
+ check("I6 包名 rollover_margin", "rollover_margin" in pkg_text)
+ check("I7 依赖 rospy", "rospy" in pkg_text)
+ check("I8 依赖 std_msgs", "std_msgs" in pkg_text)
+
+ cmake_text = io.open(cmake, "r", encoding="utf-8").read()
+ check("I9 安装三个脚本", cmake_text.count("scripts/") >= 3)
+ check("I10 安装 launch 与 config", "install(DIRECTORY launch config" in cmake_text)
+
+ launch_text = io.open(launch_path, "r", encoding="utf-8").read()
+ check("I11 type=main.py", 'type="main.py"' in launch_text)
+ check("I12 用 $(find ...)", "$(find rollover_margin)" in launch_text)
+ check("I13 不含 rospack", "rospack" not in launch_text)
+ check("I14 加载 yaml", "config/rollover_margin.yaml" in launch_text)
+ check("I15 arg 数 12", launch_text.count(" 2000)
+ check("I29 文档页含四条断言名",
+ all(name in doc_text for name in core.ASSERT_NAMES))
+ check("I30 文档页含 roslaunch", "roslaunch rollover_margin" in doc_text)
+ else:
+ check("I28 本次未在仓库根目录运行,跳过文档页校验", True)
+ check("I29 本地不含 docs 目录", True)
+ check("I30 包目录仍然存在", os.path.isdir(PKG))
+
+
+def _pytest_group(func):
+ def runner():
+ before = len(FAILED)
+ func()
+ new = FAILED[before:]
+ if new:
+ raise AssertionError(u"本组失败 %d 项:\n%s" % (len(new), u"\n".join(new)))
+ runner.__name__ = "test_" + func.__name__[len("group_"):]
+ return runner
+
+
+test_params = _pytest_group(group_params)
+test_vehicle = _pytest_group(group_vehicle)
+test_cornering = _pytest_group(group_cornering)
+test_loads = _pytest_group(group_loads)
+test_checks = _pytest_group(group_checks)
+test_table = _pytest_group(group_table)
+test_report = _pytest_group(group_report)
+test_params_read = _pytest_group(group_params_read)
+test_project = _pytest_group(group_project)
+
+
+def main():
+ print(u"rollover_margin 离线回归测试")
+ print(u"Python %s" % sys.version.split()[0])
+ print(u"包目录 %s" % PKG)
+ print(u"-" * 60)
+ group_params()
+ group_vehicle()
+ group_cornering()
+ group_loads()
+ group_checks()
+ group_table()
+ group_report()
+ group_params_read()
+ group_project()
+ print(u"=" * 60)
+ for item in FAILED:
+ print(u"[失败] %s" % item)
+ print(u"通过 %d 项,失败 %d 项" % (len(PASSED), len(FAILED)))
+ if not FAILED:
+ print(u"全部通过")
+ return 0
+ return 1
+
+
+if __name__ == "__main__":
+ sys.exit(main())