diff --git a/docs/ground/tire_slip_curve.md b/docs/ground/tire_slip_curve.md new file mode 100644 index 0000000000..29a7872eb9 --- /dev/null +++ b/docs/ground/tire_slip_curve.md @@ -0,0 +1,195 @@ +# 轮胎滑移率-附着曲线 + +## 这一页解决什么问题 + +纵向控制里有个常被跳过的前提:**附着系数不是常数**。 + +车轮从自由滚动到完全抱死,纵向力先升后降 —— 峰值出现在一个很窄的滑移率区间里, +过了峰值附着反而变差,抱死时只剩峰值的两三成。ABS 的全部设计要点都在这条曲线上: +不是「不让车轮滑」,而是「把滑移率压在峰值附近」。 + +本页给出一个可以离线跑、可以断言、不需要 CARLA 的实现,把这条曲线画在终端里, +把峰值、锁死值和目标带都算成具体数字。 + +## 曲线模型 + +以归一化滑移率 `ŝ = s / s_peak` 表示: + +``` +μ(ŝ) = μ_peak · 2·ŝ / (1 + ŝ²) +``` + +| 位置 | 取值 | 含义 | +|---|---|---| +| `s = 0` | `μ = 0` | 自由滚动 | +| `s = s_peak` | `μ = μ_peak` | 峰值附着(唯一极大) | +| `s = 1.0` | `μ_peak · 2k/(1+k²)`,`k = 1/s_peak` | 车轮抱死 | + +闭式派生量: + +- 初段刚度:`dμ/ds|₀ = 2·μ_peak / s_peak` +- ABS 目标带:`[s_peak(1−band), s_peak(1+band)]` +- 锁死占比:`μ(1.0) / μ_peak` + +默认参数(`μ_peak = 0.90`、`s_peak = 0.12`、`band = 0.20`)下: + +| 量 | 值 | +|---|---| +| 峰值附着 | `0.9000` @ `s = 0.1200` | +| 锁死附着 | `0.2129`(占峰值 **23.7%**) | +| 初段刚度 | `15.000 1/s` | +| ABS 目标带 | `[0.0960, 0.1440]` | + +对照:`s = 0.06`(半峰值滑移)时 `μ = 0.72`,正好是峰值的 80% —— 滑移率只有峰值的一半, +纵向力已经掉了两成,这就是「轮胎工况窗口很窄」的量化说法。 + +## 四种路面 + +`road/surface` 只影响报告标注与默认值选择,实际数字仍由 `peak_slip` / `peak_friction` 决定。 +常用量级: + +| 路面 | `peak_slip` 典型值 | `peak_friction` 典型值 | +|---|---|---| +| `dry_asphalt` | 0.10 ~ 0.15 | 0.85 ~ 1.00 | +| `wet_asphalt` | 0.12 ~ 0.20 | 0.55 ~ 0.75 | +| `snow` | 0.20 ~ 0.40 | 0.25 ~ 0.40 | +| `ice` | 0.30 ~ 0.50 | 0.08 ~ 0.15 | + +## 四条断言 + +| 断言 | 判据 | 失败意味着 | +|---|---|---| +| **峰值唯一** | 采样点局部极大只有一个,且落在 `(s_peak, μ_peak)` 上 | 采样太粗抓不到峰值,或曲线形状被改坏 | +| **锁死衰减** | `μ(1.0)/μ_peak ≤ lock_friction_ratio` | 曲线没体现出「过度滑移导致附着下降」 | +| **初段线性** | `s→0` 时 `μ ≈ (2μ_peak/s_peak)·s`,相对误差 ≤ `linear_tolerance` | 曲线在原点附近不光滑(真车会有初始刚度段) | +| **目标带覆盖峰值** | 目标带必须包含 `s_peak` | 参数组合自相矛盾,ABS 会控制在错误的滑移区间 | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `tire/peak_slip` | `0.12` | 峰值滑移率(必须 `(0, 1]`) | +| `tire/peak_friction` | `0.90` | 峰值附着系数(必须 > 0) | +| `tire/abs_band` | `0.20` | 目标带宽比例(必须 `[0, 1)`) | +| `road/surface` | `dry_asphalt` | 路面标注 | +| `curve/samples` | `51` | 采样点数(≥5,默认步长 0.02) | +| `check/lock_friction_ratio` | `0.85` | 锁死附着上限比例 | +| `check/linear_tolerance` | `0.02` | 初段线性度容差 | +| `check/peak_tolerance` | `0.01` | 峰值容差 | +| `rate/publish_hz` | `10.0` | 挂起时循环频率 | +| `topic/report` | `/tire/report` | 报告话题 | +| `topic/curve` | `/tire/curve` | 曲线表话题 | +| `topic/peak_slip` | `/tire/peak_slip` | 峰值滑移率话题 | + +## 话题 + +| 话题 | 类型 | 内容 | +|---|---|---| +| `/tire/report` | `std_msgs/String`(latched) | 完整文本报告(含字符图与四条断言) | +| `/tire/curve` | `std_msgs/String`(latched) | 曲线表 CSV:`slip,friction,ratio`,51 行 | +| `/tire/peak_slip` | `std_msgs/Float64`(latched) | 峰值滑移率 | + +## 运行 + +```bash +cd ~/catkin_ws16 +catkin_make +source devel/setup.bash +roslaunch tire_slip_curve main.launch +``` + +改参数: + +```bash +roslaunch tire_slip_curve main.launch peak_slip:=0.30 peak_friction:=0.35 surface:=snow +``` + +另一终端核对: + +```bash +rosparam get /tire/peak_slip +rosparam get /check/lock_friction_ratio +rosparam get /road/surface +``` + +## 实际输出(默认参数) + +``` +路面:dry_asphalt,峰值滑移率 0.120,峰值附着 0.900 +锁死点:s=1.00,附着 0.2129(占峰值 23.7%) +初段刚度:15.000 1/s,ABS 目标带 [0.0960, 0.1440](带宽 20%) +采样:51 点(s 从 0 到 1) +断言:4 通过 / 0 失败(共 4 条) + [PASS] 峰值唯一 峰值 0.9000 @ s=0.1200(期望 0.9000 @ 0.1200),局部极大 1 个 + [PASS] 锁死衰减 锁死附着 0.2129,占峰值 23.7%(要求 <= 85.0%) + [PASS] 初段线性 初段刚度 15.000,最大相对误差 0.99% @ s=0.0120(要求 <= 2.00%) + [PASS] 目标带覆盖峰值 目标带 [0.0960, 0.1440] 覆盖峰值滑移 0.1200 +结论:PASSED +``` + +## 两个反例 + +**采样太粗(`samples:=5`)** + +```bash +roslaunch tire_slip_curve main.launch samples:=5 +``` + +只有 5 个采样点(步长 0.25),峰值的 `s = 0.12` 落在两个采样点之间,抓到的最大值是 +`s = 0.25` 处的 `0.7020` —— 比真峰值低 22%。于是 `峰值唯一` 失败: + +``` +[FAIL] 峰值唯一 峰值 0.7020 @ s=0.2500(期望 0.9000 @ 0.1200),局部极大 1 个 +``` + +**锁死衰减不达标(`lock_friction_ratio:=0.20`)** + +```bash +roslaunch tire_slip_curve main.launch lock_friction_ratio:=0.20 +``` + +实测锁死附着占峰值 23.7%,高于要求的 20%: + +``` +[FAIL] 锁死衰减 锁死附着 0.2129,占峰值 23.7%(要求 <= 20.0%) +``` + +## 离线测试 + +```bash +cd ~/catkin_ws16 +python3 tests/test_tire_slip_curve_local.py +``` + +测试只依赖标准库(不需要 ROS / CARLA / 显示器)。在仓库根目录执行时还会检查 +`docs/ground/tire_slip_curve.md` 是否存在、参数表与命令是否完整。 + +## 与已有模块的分工 + +| 模块 | 关心什么 | +|---|---| +| `braking_distance_check` | 给定附着系数算制动距离 | +| `turning_sweep_check` | 给定转角与轴距算转弯包络 | +| `reverse_parking_planner` | 倒车入库轨迹 | +| **`tire_slip_curve`(本页)** | **附着系数本身:它由滑移率决定,有峰值也有锁死衰减** | + +本包是上游 —— 其它模块把这里的 `peak_friction` 当输入用。 + +## 常见问题 + +**Q:为什么不用魔术公式(Pacejka)?** +A:魔术公式需要拟合 4 个系数、没有解析极大值,断言只能靠数值搜索。本页要讲清的是 +「峰值 - 锁死 - 目标带」这三个概念,有闭式解的曲线更容易校验。真车标定时把 +`peak_slip` / `peak_friction` 换成实测值即可,0~0.4 区间形状足够接近。 + +**Q:Melodic 上为什么不能直接用 `rospy.loginfo(fmt, *args)`?** +A:rosgraph 的格式化会对非 ASCII 的 unicode 调 `str()`,在 Python 2.7 下抛 +`UnicodeEncodeError`。节点里的 `_log()` 把消息统一编码成 byte str 后再发。 + +**Q:为什么打印完节点不退出?** +A:留出时间让另一个终端 `rosparam get` 核对参数(launch 的 arg 覆盖是否生效)。 +按 `Ctrl+C` 退出。 + +--- + +所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/docs/index.md b/docs/index.md index c37984f932..beaa3a6c05 100644 --- a/docs/index.md +++ b/docs/index.md @@ -242,6 +242,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c * [地面移动底盘速度指令安全监督](./ground/cmd_vel_guard.md) —— 对 `/cmd_vel` 做限幅、急停、超时与非法值拦截,无需仿真环境 * [VRU 冲突风险图](./ground/vru_conflict_risk_map.md) —— 按时间对齐自车与行人、儿童和骑行者轨迹,输出最小距离、TTC 和风险等级 * [里程计健康监视(打滑 / 堵转 / 反向检测)](./ground/odom_health_monitor.md) —— 比较 `/cmd_vel` 与 `/odom` 的速度比值,识别打滑、堵转与反向运动 +* [轮胎滑移率-附着曲线](./ground/tire_slip_curve.md) —— 用简化刷子轮胎模型输出滑移率与附着系数关系,标出峰值附着与 ABS 目标滑移带 ## 5. 空域载具 diff --git a/mkdocs.yml b/mkdocs.yml index f935a9d581..98320ade86 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -236,6 +236,7 @@ nav: - 速度指令安全监督: './ground/cmd_vel_guard.md' - VRU 冲突风险图: './ground/vru_conflict_risk_map.md' - 里程计健康监视: './ground/odom_health_monitor.md' + - 轮胎滑移率-附着曲线: './ground/tire_slip_curve.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/tire_slip_curve/CMakeLists.txt b/src/ground/tire_slip_curve/CMakeLists.txt new file mode 100644 index 0000000000..5d8e35e1b3 --- /dev/null +++ b/src/ground/tire_slip_curve/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.0.2) +project(tire_slip_curve) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/main.py + scripts/tire_core.py + scripts/tire_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/src/ground/tire_slip_curve/README.md b/src/ground/tire_slip_curve/README.md new file mode 100644 index 0000000000..4e45e541e6 --- /dev/null +++ b/src/ground/tire_slip_curve/README.md @@ -0,0 +1,176 @@ +# tire_slip_curve —— 轮胎滑移率-附着曲线核算 + +把一个「车轮在滑还是没在滑」的问题,做成一条**有闭式解**的附着曲线,用终端就能看清 +三件事:峰值附着在哪、车轮抱死后附着掉多少、ABS 为什么要把滑移率圈在一个窄带里。 + +纯标准库实现,不依赖 CARLA、numpy、GUI;在 Ubuntu 18.04 + ROS Melodic(Python 2.7) +和 Ubuntu 20.04 + Noetic(Python 3)上都能直接跑。 + +## 为什么需要这个模块 + +纵向动力学里最容易被忽略的一条:**附着系数不是常数,它是滑移率的函数**。 + +- 滑移率太小时轮胎还没「咬住」路面,纵向力上不去; +- 滑移率跨过峰值之后附着开始下降,车轮彻底抱死时只剩峰值的两三成; +- 所以 ABS 的目标从来不是「不滑」,而是「把滑移率压在峰值附近的那条带子里」。 + +这三个量都能用一条曲线讲清楚,而这条曲线的极大值、初段斜率、锁死值都可以解析求出, +于是可以被断言精确校验,不必依赖实测拟合。 + +## 曲线模型 + +以归一化滑移率 `ŝ = s / s_peak` 表示: + +``` +μ(ŝ) = μ_peak · 2·ŝ / (1 + ŝ²) +``` + +| 位置 | 取值 | 含义 | +|---|---|---| +| `ŝ = 0`(s = 0) | `μ = 0` | 自由滚动,没有纵向力 | +| `ŝ = 1`(s = s_peak) | `μ = μ_peak` | 峰值附着,曲线唯一极大 | +| `s = 1.0`(车轮抱死) | `μ = μ_peak · 2·k / (1 + k²)`,`k = 1/s_peak` | 锁死附着 | + +派生量也都是闭式解: + +- 初段刚度 `dμ/ds|₀ = 2·μ_peak / s_peak` +- 峰值滑移率 `s_peak` +- ABS 目标带 `[s_peak·(1−band), s_peak·(1+band)]` + +默认参数(`μ_peak = 0.90`、`s_peak = 0.12`)下:锁死附着 `0.2129`(只剩峰值的 **23.7%**), +初段刚度 `15.0`,目标带 `[0.096, 0.144]`。 + +## 目录结构 + +``` +src/ground/tire_slip_curve/ +├── CMakeLists.txt +├── package.xml +├── README.md +├── config/tire_slip_curve.yaml # 12 个参数 +├── launch/main.launch # 12 个带 doc 的 arg +└── scripts/ + ├── main.py # 入口(符合 main.* 约定) + ├── tire_core.py # 纯数学,不 import rospy + └── tire_node.py # ROS 收发 +tests/test_tire_slip_curve_local.py # 离线回归测试 +docs/ground/tire_slip_curve.md # 文档页 +``` + +## 四条断言 + +| 断言 | 判据 | +|---|---| +| **峰值唯一** | 采样点里的局部极大只有一个,且落在 `(s_peak, μ_peak)` 上(容差 `peak_tolerance`) | +| **锁死衰减** | `μ(1.0) / μ_peak ≤ lock_friction_ratio`(默认 0.85) | +| **初段线性** | `s → 0` 时 `μ ≈ (dμ/ds|₀)·s`,相对误差 ≤ `linear_tolerance`(默认 2%) | +| **目标带覆盖峰值** | ABS 目标带必须包含 `s_peak` | + +## 参数 + +| 参数 | 默认 | 说明 | +|---|---|---| +| `tire/peak_slip` | `0.12` | 峰值附着对应的滑移率(干路 0.10~0.15,冰雪 0.20~0.40) | +| `tire/peak_friction` | `0.90` | 峰值附着系数 | +| `tire/abs_band` | `0.20` | ABS 目标带宽比例 | +| `road/surface` | `dry_asphalt` | 路面标注:`dry_asphalt` / `wet_asphalt` / `snow` / `ice` | +| `curve/samples` | `51` | 曲线采样点数(≥5) | +| `check/lock_friction_ratio` | `0.85` | 锁死附着的上限比例 | +| `check/linear_tolerance` | `0.02` | 初段线性度容差 | +| `check/peak_tolerance` | `0.01` | 峰值位置/大小容差 | +| `rate/publish_hz` | `10.0` | 打印完后保持挂起的循环频率 | +| `topic/report` | `/tire/report` | 文本报告(latched, `std_msgs/String`) | +| `topic/curve` | `/tire/curve` | 曲线表 CSV 文本(latched) | +| `topic/peak_slip` | `/tire/peak_slip` | 峰值滑移率数值(latched, `std_msgs/Float64`) | + +## 运行 + +```bash +roslaunch tire_slip_curve main.launch +``` + +改参数(示例): + +```bash +roslaunch tire_slip_curve main.launch peak_slip:=0.30 peak_friction:=0.35 surface:=snow +roslaunch tire_slip_curve main.launch lock_friction_ratio:=0.20 +roslaunch tire_slip_curve main.launch samples:=5 +``` + +打印完节点会保持挂起,方便另一个终端核对参数: + +```bash +rosparam get /tire/peak_slip +rosparam get /check/lock_friction_ratio +``` + +## 实测输出(默认参数) + +``` +路面:dry_asphalt,峰值滑移率 0.120,峰值附着 0.900 +锁死点:s=1.00,附着 0.2129(占峰值 23.7%) +初段刚度:15.000 1/s,ABS 目标带 [0.0960, 0.1440](带宽 20%) +采样:51 点(s 从 0 到 1) +------------------------------------------------------------ +附着曲线(纵轴 0 ~ 0.95): + ******* + * ** + * *** + | **** + | **** + * ******* + | ********** + * ***************** + | * + | +* ... +0 滑移率 → 1.0 +------------------------------------------------------------ +断言:4 通过 / 0 失败(共 4 条) + [PASS] 峰值唯一 峰值 0.9000 @ s=0.1200(期望 0.9000 @ 0.1200),局部极大 1 个 + [PASS] 锁死衰减 锁死附着 0.2129,占峰值 23.7%(要求 <= 85.0%) + [PASS] 初段线性 初段刚度 15.000,最大相对误差 0.99% @ s=0.0120(要求 <= 2.00%) + [PASS] 目标带覆盖峰值 目标带 [0.0960, 0.1440] 覆盖峰值滑移 0.1200 +结论:PASSED +``` + +## 两个反例 + +```bash +roslaunch tire_slip_curve main.launch samples:=5 # 采样太粗 → 峰值唯一失败 +roslaunch tire_slip_curve main.launch lock_friction_ratio:=0.20 # 实测 23.7% > 20% → 锁死衰减失败 +``` + +## 离线测试 + +```bash +cd ~/catkin_ws16 +python3 tests/test_tire_slip_curve_local.py +``` + +不需要 ROS、不需要 CARLA、不需要显示器;在仓库根目录执行时还会顺带校验文档页。 + +## 与已有模块的分工 + +| 模块 | 关心什么 | +|---|---| +| `braking_distance_check` | 给定附着系数,算制动距离 | +| `turning_sweep_check` | 给定转角与轴距,算转弯时车身占多宽 | +| **`tire_slip_curve`(本包)** | **附着系数从哪来:滑移率决定它,曲线给出峰值与锁死衰减** | + +换句话说:本包是上游,其它包把它算出的 `μ_peak` 当输入。 + +## 常见问题 + +**Q:为什么用这条简单的曲线,不用魔术公式(Pacejka)?** +A:魔术公式要拟合 4 个系数,没有解析极大值,断言只能靠数值搜索;本包要讲的是 +「峰值 - 锁死 - 目标带」这三个概念,用有闭式解的曲线反而更容易被校验和讲清。真车 +标定时把 `peak_slip`/`peak_friction` 换成实测值即可,曲线形状在 0~0.4 区间足够像。 + +**Q:Melodic 上运行时报 `UnicodeEncodeError`?** +A:rosgraph 的日志格式化对非 ASCII 的 unicode 会调 `str()`,节点里已经通过 `_log()` +统一转成 byte str,如果自己改代码请沿用这个函数。 + +--- + +所使用具体的大模型声明:使用大模型辅助代码编写、测试编写与文档整理,提交内容由本人负责。 diff --git a/src/ground/tire_slip_curve/config/tire_slip_curve.yaml b/src/ground/tire_slip_curve/config/tire_slip_curve.yaml new file mode 100644 index 0000000000..7ee92f9d0e --- /dev/null +++ b/src/ground/tire_slip_curve/config/tire_slip_curve.yaml @@ -0,0 +1,35 @@ +# 轮胎滑移率-附着曲线核算 —— 默认参数 +# 所有值都可以在 roslaunch 命令行用同名 覆盖,例如: +# roslaunch tire_slip_curve main.launch peak_slip:=0.15 peak_friction:=0.75 + +tire: + # 峰值附着对应的滑移率(干路 0.10~0.15,湿路 0.12~0.20,冰雪 0.20~0.40) + peak_slip: 0.12 + # 峰值附着系数 + peak_friction: 0.90 + # ABS 目标带宽(相对峰值滑移率的比例),目标带 = peak_slip * (1 ± abs_band) + abs_band: 0.20 + +road: + # 路面类型,仅用于报告标注:dry_asphalt / wet_asphalt / snow / ice + surface: dry_asphalt + +curve: + # 曲线采样点数(s 从 0 到 1 均匀采样) + samples: 51 + +check: + # 锁死附着相对峰值的上限比例,越小要求越严 + lock_friction_ratio: 0.85 + # 初段线性度允许的相对误差 + linear_tolerance: 0.02 + # 峰值位置/大小的允许偏差 + peak_tolerance: 0.01 + +rate: + publish_hz: 10.0 + +topic: + report: /tire/report + curve: /tire/curve + peak_slip: /tire/peak_slip diff --git a/src/ground/tire_slip_curve/launch/main.launch b/src/ground/tire_slip_curve/launch/main.launch new file mode 100644 index 0000000000..cc4bd65dbc --- /dev/null +++ b/src/ground/tire_slip_curve/launch/main.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ground/tire_slip_curve/package.xml b/src/ground/tire_slip_curve/package.xml new file mode 100644 index 0000000000..17e3196c8b --- /dev/null +++ b/src/ground/tire_slip_curve/package.xml @@ -0,0 +1,19 @@ + + + tire_slip_curve + 1.0.0 + + 轮胎滑移率-附着曲线核算:用一条有闭式解的解析曲线,讲清峰值附着、锁死附着的衰减, + 以及 ABS 为什么要把滑移率控制在峰值附近。纯标准库,不依赖 CARLA / numpy / GPU。 + + + Wist-sutton + MIT + + catkin + + rospy + std_msgs + + + diff --git a/src/ground/tire_slip_curve/scripts/main.py b/src/ground/tire_slip_curve/scripts/main.py new file mode 100644 index 0000000000..e849340b94 --- /dev/null +++ b/src/ground/tire_slip_curve/scripts/main.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""入口:把脚本目录加进 sys.path 后转交给 tire_node.main。""" +from __future__ import division, print_function + +import os +import sys + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from tire_node import main # noqa: E402 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/ground/tire_slip_curve/scripts/tire_core.py b/src/ground/tire_slip_curve/scripts/tire_core.py new file mode 100644 index 0000000000..17dfd9796c --- /dev/null +++ b/src/ground/tire_slip_curve/scripts/tire_core.py @@ -0,0 +1,477 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""轮胎滑移率-附着曲线核算(纯标准库,不依赖 ROS / CARLA / numpy)。 + +模型:以归一化滑移率 ŝ = s / s_peak 表示的单调升-单调降附着曲线 + + μ(ŝ) = μ_peak * 2·ŝ / (1 + ŝ²) + + ŝ = 0 → μ = 0 (自由滚动,无纵向力) + ŝ = 1 → μ = μ_peak (峰值附着,唯一极大) + ŝ > 1 → μ 单调下降 (过度滑移,附着衰减) + ŝ = 1/s_peak 时 s = 1.0 (车轮锁死) + +这条曲线的好处是:峰值位置、峰值大小、初段斜率全都有闭式解,能被断言 +精确校验,不需要拟合。真实轮胎(魔术公式)在 0~0.4 的滑移区间形状与此 +接近,用它讲清「为什么 ABS 要把滑移率控在峰值附近」已经足够。 + +Python 2.7 / 3.x 双兼容。 +""" +from __future__ import division, print_function, unicode_literals + +import io +import json +import math + +__all__ = [ + "TireError", "TireSpec", "CurvePoint", "Curve", "build_curve", + "key_points", "check_curve", "all_passed", "render_curve", + "format_report", "TireParams", "PARAM_KEYS", "PARAM_DEFAULTS", + "ASSERT_NAMES", "to_message_text", +] + +# ── 默认参数 ───────────────────────────────────────────────────────────── + +DEFAULT_PEAK_SLIP = 0.12 +DEFAULT_PEAK_FRICTION = 0.90 +DEFAULT_ABS_BAND = 0.20 # 目标带 = peak_slip * (1 ± band) +DEFAULT_SURFACE = u"dry_asphalt" +DEFAULT_SAMPLES = 51 # 曲线采样点数(s 从 0 到 1) +DEFAULT_LOCK_FRICTION_RATIO = 0.85 # 锁死附着至少降到峰值的这个比例以下 +DEFAULT_LINEAR_TOLERANCE = 0.02 # 初段线性度允许的相对误差 +DEFAULT_PEAK_TOLERANCE = 0.01 +DEFAULT_PUBLISH_HZ = 10.0 +DEFAULT_GRID_WIDTH = 60 +DEFAULT_GRID_HEIGHT = 12 + +SURFACES = (u"dry_asphalt", u"wet_asphalt", u"snow", u"ice") + +ASSERT_NAMES = (u"峰值唯一", u"锁死衰减", u"初段线性", u"目标带覆盖峰值") + +PARAM_DEFAULTS = { + "tire/peak_slip": DEFAULT_PEAK_SLIP, + "tire/peak_friction": DEFAULT_PEAK_FRICTION, + "tire/abs_band": DEFAULT_ABS_BAND, + "road/surface": DEFAULT_SURFACE, + "curve/samples": DEFAULT_SAMPLES, + "check/lock_friction_ratio": DEFAULT_LOCK_FRICTION_RATIO, + "check/linear_tolerance": DEFAULT_LINEAR_TOLERANCE, + "check/peak_tolerance": DEFAULT_PEAK_TOLERANCE, + "rate/publish_hz": DEFAULT_PUBLISH_HZ, + "topic/report": u"/tire/report", + "topic/curve": u"/tire/curve", + "topic/peak_slip": u"/tire/peak_slip", +} + +PARAM_KEYS = tuple(sorted(PARAM_DEFAULTS)) + + +class TireError(ValueError): + """参数不合法。""" + + +# ── 小工具 ─────────────────────────────────────────────────────────────── + +def _as_float(value, name, default=None): + if value is None: + if default is None: + raise TireError(u"缺少参数:%s" % name) + return float(default) + if isinstance(value, bool): + raise TireError(u"参数 %s 不能是布尔值" % name) + try: + out = float(value) + except (TypeError, ValueError): + raise TireError(u"参数 %s 不是数字:%r" % (name, value)) + if math.isnan(out) or math.isinf(out): + raise TireError(u"参数 %s 不是有限数:%r" % (name, value)) + return out + + +def _as_int(value, name, default=None): + out = _as_float(value, name, default) + if abs(out - round(out)) > 1e-9: + raise TireError(u"参数 %s 必须是整数:%r" % (name, value)) + return int(round(out)) + + +def _positive(value, name): + out = _as_float(value, name) + if out <= 0.0: + raise TireError(u"参数 %s 必须为正:%r" % (name, value)) + return out + + +def _clamp(value, low, high): + return low if value < low else (high if value > high else value) + + +def to_message_text(text): + """把文本转成 rospy 能安全发布的形式(py2 → bytes,py3 → str)。""" + try: + unicode # noqa: F821 (Python 2) + except NameError: + return text + if isinstance(text, unicode): # noqa: F821 + return text.encode("utf-8") + return text + + +# ── 数据模型 ───────────────────────────────────────────────────────────── + +class TireSpec(object): + """一条轮胎-路面组合的附着曲线参数。""" + + __slots__ = ("peak_slip", "peak_friction", "abs_band", "surface") + + def __init__(self, peak_slip=DEFAULT_PEAK_SLIP, + peak_friction=DEFAULT_PEAK_FRICTION, + abs_band=DEFAULT_ABS_BAND, surface=DEFAULT_SURFACE): + self.peak_slip = _positive(peak_slip, "peak_slip") + if self.peak_slip > 1.0: + raise TireError(u"峰值滑移率必须 <= 1.0:%r" % peak_slip) + self.peak_friction = _positive(peak_friction, "peak_friction") + band = _as_float(abs_band, "abs_band") + if band < 0.0 or band >= 1.0: + raise TireError(u"ABS 目标带宽比例必须在 [0, 1):%r" % abs_band) + self.abs_band = band + name = surface if surface is not None else DEFAULT_SURFACE + if name not in SURFACES: + raise TireError(u"未知路面:%r(可选 %s)" % (surface, ", ".join(SURFACES))) + self.surface = name + + # 曲线与派生量的闭式解 ------------------------------------------------ + + def friction(self, slip): + """给定滑移率求附着系数。""" + slip = _as_float(slip, "slip") + if slip < 0.0: + slip = -slip + hat = slip / self.peak_slip + return self.peak_friction * 2.0 * hat / (1.0 + hat * hat) + + def slope_at_zero(self): + """初段刚度 dμ/ds|₀ = 2·μ_peak / s_peak(闭式解)。""" + return 2.0 * self.peak_friction / self.peak_slip + + def lock_slip(self): + """车轮完全锁死时的滑移率(本模型约定为 1.0)。""" + return 1.0 + + def lock_friction(self): + return self.friction(self.lock_slip()) + + def abs_band_limits(self): + """ABS 目标带 [low, high],并夹到 (0, 1]。""" + low = self.peak_slip * (1.0 - self.abs_band) + high = self.peak_slip * (1.0 + self.abs_band) + return _clamp(low, 1e-6, 1.0), _clamp(high, 1e-6, 1.0) + + def as_tuple(self): + return (self.peak_slip, self.peak_friction, self.abs_band, self.surface) + + +class CurvePoint(object): + """曲线上的一点。""" + + __slots__ = ("slip", "friction", "ratio") + + def __init__(self, slip, friction, ratio): + self.slip = slip + self.friction = friction + self.ratio = ratio # friction / peak_friction + + def as_row(self): + return (round(self.slip, 4), round(self.friction, 4), round(self.ratio, 4)) + + +class Curve(object): + """一条完整的附着曲线。""" + + def __init__(self, points, spec, name=""): + self.points = list(points) + self.spec = spec + self.name = name or ("%s(peak_slip=%.3f)" % (spec.surface, spec.peak_slip)) + + # 基本属性 ---------------------------------------------------------- + + @property + def count(self): + return len(self.points) + + def is_empty(self): + return not self.points + + def first(self): + return self.points[0] if self.points else None + + def last(self): + return self.points[-1] if self.points else None + + def at(self, slip): + """取最接近给定滑移率的采样点。""" + if not self.points: + return None + return min(self.points, key=lambda p: abs(p.slip - slip)) + + def max_point(self): + """曲线上附着系数最大的点。""" + if not self.points: + return None + return max(self.points, key=lambda p: p.friction) + + def monotonic_up_to(self, slip): + """[0, slip] 区间内是否严格单调递增。""" + prev = None + for point in self.points: + if point.slip > slip + 1e-12: + break + if prev is not None and point.friction <= prev: + return False + prev = point.friction + return True + + def ratio_at_lock(self): + last = self.last() + if last is None or last.ratio is None: + return 0.0 + return last.ratio + + def as_rows(self): + return [p.as_row() for p in self.points] + + +def build_curve(spec=None, samples=DEFAULT_SAMPLES): + """按 s = 0 … 1 均匀采样生成曲线。""" + spec = spec or TireSpec() + count = _as_int(samples, "samples", DEFAULT_SAMPLES) + if count < 5: + raise TireError(u"采样点数至少 5:%r" % samples) + points = [] + for index in range(count): + slip = index / (count - 1.0) + mu = spec.friction(slip) + points.append(CurvePoint(slip, mu, mu / spec.peak_friction)) + return Curve(points, spec) + + +def key_points(spec=None): + """返回报告里要用的关键量。""" + spec = spec or TireSpec() + low, high = spec.abs_band_limits() + return { + "surface": spec.surface, + "peak_slip": spec.peak_slip, + "peak_friction": spec.peak_friction, + "lock_slip": spec.lock_slip(), + "lock_friction": spec.lock_friction(), + "lock_ratio": spec.lock_friction() / spec.peak_friction, + "slope_at_zero": spec.slope_at_zero(), + "abs_band": spec.abs_band, + "abs_low": low, + "abs_high": high, + "abs_peak_friction": spec.friction(spec.peak_slip), + } + + +# ── 四条断言 ───────────────────────────────────────────────────────────── + +def check_curve(curve, spec=None, lock_friction_ratio=DEFAULT_LOCK_FRICTION_RATIO, + linear_tolerance=DEFAULT_LINEAR_TOLERANCE, + peak_tolerance=DEFAULT_PEAK_TOLERANCE): + """返回四条断言,结构为 {type, passed, detail}。""" + curve = curve or build_curve(spec) + spec = spec or curve.spec + lock_limit = _as_float(lock_friction_ratio, "lock_friction_ratio") + lin_tol = _as_float(linear_tolerance, "linear_tolerance") + peak_tol = _as_float(peak_tolerance, "peak_tolerance") + + results = [] + + # 1) 峰值唯一:采样点里的极大值只有一个,且落在峰值滑移附近 + peak = curve.max_point() + ups = [i for i in range(1, curve.count - 1) + if curve.points[i].friction > curve.points[i - 1].friction + and curve.points[i].friction > curve.points[i + 1].friction] + peak_ok = (peak is not None + and abs(peak.friction - spec.peak_friction) <= peak_tol + and abs(peak.slip - spec.peak_slip) <= max(peak_tol, 2.0 / curve.count) + and len(ups) <= 1) + results.append({ + "type": ASSERT_NAMES[0], + "passed": bool(peak_ok), + "detail": u"峰值 %.4f @ s=%.4f(期望 %.4f @ %.4f),局部极大 %d 个" % ( + peak.friction if peak else 0.0, peak.slip if peak else 0.0, + spec.peak_friction, spec.peak_slip, len(ups)), + }) + + # 2) 锁死衰减:s=1 的附着要明显低于峰值 + lock = spec.lock_friction() + ratio = lock / spec.peak_friction + results.append({ + "type": ASSERT_NAMES[1], + "passed": bool(ratio <= lock_limit + 1e-12), + "detail": u"锁死附着 %.4f,占峰值 %.1f%%(要求 <= %.1f%%)" % ( + lock, ratio * 100.0, lock_limit * 100.0), + }) + + # 3) 初段线性:s → 0 时 μ ≈ slope·s + worst = 0.0 + worst_s = 0.0 + probe = spec.peak_slip * 0.1 + for k in range(1, 6): + slip = probe * k / 5.0 + if slip <= 0.0: + continue + approx = spec.slope_at_zero() * slip + actual = spec.friction(slip) + rel = abs(actual - approx) / max(approx, 1e-9) + if rel > worst: + worst, worst_s = rel, slip + results.append({ + "type": ASSERT_NAMES[2], + "passed": bool(worst <= lin_tol + 1e-12), + "detail": u"初段刚度 %.3f,最大相对误差 %.2f%% @ s=%.4f(要求 <= %.2f%%)" % ( + spec.slope_at_zero(), worst * 100.0, worst_s, lin_tol * 100.0), + }) + + # 4) 目标带覆盖峰值:ABS 目标区间必须包住峰值滑移 + low, high = spec.abs_band_limits() + covered = low <= spec.peak_slip <= high + results.append({ + "type": ASSERT_NAMES[3], + "passed": bool(covered), + "detail": u"目标带 [%.4f, %.4f] 覆盖峰值滑移 %.4f" % (low, high, spec.peak_slip), + }) + + return results + + +def all_passed(results): + return all(item["passed"] for item in results) + + +# ── 字符图 ─────────────────────────────────────────────────────────────── + +def render_curve(curve, width=DEFAULT_GRID_WIDTH, height=DEFAULT_GRID_HEIGHT): + """把 μ(s) 画成字符图:横轴滑移率 0→1,纵轴 0→peak。""" + width = _as_int(width, "width", DEFAULT_GRID_WIDTH) + height = _as_int(height, "height", DEFAULT_GRID_HEIGHT) + if width < 20 or height < 5: + raise TireError(u"网格太小:%dx%d" % (width, height)) + if curve.is_empty(): + return [] + + top = curve.spec.peak_friction * 1.05 + grid = [[" "] * width for _ in range(height)] + # 目标带底纹 + low, high = curve.spec.abs_band_limits() + for x in range(width): + slip = x / (width - 1.0) + if low <= slip <= high: + grid[height - 1][x] = "." + # 曲线 + prev_row = None + for x in range(width): + slip = x / (width - 1.0) + mu = curve.spec.friction(slip) + row = int(round((1.0 - mu / top) * (height - 1))) + row = int(_clamp(row, 0, height - 1)) + grid[row][x] = "*" + if prev_row is not None and abs(row - prev_row) > 1: + step = 1 if row > prev_row else -1 + for r in range(prev_row + step, row, step): + grid[r][x] = "|" + prev_row = row + + lines = ["".join(row) for row in grid] + lines.append("0" + " " * (width - 12) + u"滑移率 → 1.0") + return lines + + +# ── 报告 ───────────────────────────────────────────────────────────────── + +def format_report(curve, results, header=""): + """生成文本报告。""" + curve = curve or build_curve() + spec = curve.spec + keys = key_points(spec) + passed = sum(1 for item in results if item["passed"]) + lines = [] + if header: + lines.append(header) + lines.append(u"路面:%s,峰值滑移率 %.3f,峰值附着 %.3f" % ( + keys["surface"], keys["peak_slip"], keys["peak_friction"])) + lines.append(u"锁死点:s=%.2f,附着 %.4f(占峰值 %.1f%%)" % ( + keys["lock_slip"], keys["lock_friction"], keys["lock_ratio"] * 100.0)) + lines.append(u"初段刚度:%.3f 1/s,ABS 目标带 [%.4f, %.4f](带宽 %.0f%%)" % ( + keys["slope_at_zero"], keys["abs_low"], keys["abs_high"], + keys["abs_band"] * 100.0)) + lines.append(u"采样:%d 点(s 从 0 到 1)" % curve.count) + lines.append(u"-" * 60) + lines.append(u"附着曲线(纵轴 0 ~ %.2f):" % (spec.peak_friction * 1.05)) + for line in render_curve(curve): + lines.append(line) + 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 TireParams(object): + """从 ROS 参数服务器读到的配置。""" + + def __init__(self, **kwargs): + for name in PARAM_KEYS: + attr = "params_" + name.replace("/", "_") + setattr(self, attr, kwargs.get(name, PARAM_DEFAULTS[name])) + self.peak_slip = _positive(self.params_tire_peak_slip, "tire/peak_slip") + self.peak_friction = _positive(self.params_tire_peak_friction, "tire/peak_friction") + self.abs_band = _as_float(self.params_tire_abs_band, "tire/abs_band", DEFAULT_ABS_BAND) + surface = self.params_road_surface + if isinstance(surface, bytes): + surface = surface.decode("utf-8") + self.surface = surface if surface in SURFACES else DEFAULT_SURFACE + self.samples = _as_int(self.params_curve_samples, "curve/samples", DEFAULT_SAMPLES) + self.lock_friction_ratio = _as_float( + self.params_check_lock_friction_ratio, "check/lock_friction_ratio", + DEFAULT_LOCK_FRICTION_RATIO) + self.linear_tolerance = _as_float( + self.params_check_linear_tolerance, "check/linear_tolerance", + DEFAULT_LINEAR_TOLERANCE) + self.peak_tolerance = _as_float( + self.params_check_peak_tolerance, "check/peak_tolerance", DEFAULT_PEAK_TOLERANCE) + self.publish_hz = _as_float(self.params_rate_publish_hz, "rate/publish_hz", + DEFAULT_PUBLISH_HZ) + self.topic_report = self.params_topic_report + self.topic_curve = self.params_topic_curve + self.topic_peak_slip = self.params_topic_peak_slip + + @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 spec(self): + return TireSpec(self.peak_slip, self.peak_friction, self.abs_band, self.surface) + + def as_dict(self): + return { + "tire/peak_slip": self.peak_slip, + "tire/peak_friction": self.peak_friction, + "tire/abs_band": self.abs_band, + "road/surface": self.surface, + "curve/samples": self.samples, + "check/lock_friction_ratio": self.lock_friction_ratio, + "check/linear_tolerance": self.linear_tolerance, + "check/peak_tolerance": self.peak_tolerance, + "rate/publish_hz": self.publish_hz, + } diff --git a/src/ground/tire_slip_curve/scripts/tire_node.py b/src/ground/tire_slip_curve/scripts/tire_node.py new file mode 100644 index 0000000000..5270a4b37c --- /dev/null +++ b/src/ground/tire_slip_curve/scripts/tire_node.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""轮胎滑移率-附着曲线节点的 ROS 封装(ROS 1 / Melodic 可跑,Python 2.7 & 3.x)。 + +节点只做三件事: + 1. 从参数服务器读配置,算出一条附着曲线并做四条断言; + 2. 把报告、曲线表、峰值滑移率分别发布到三个 latched 话题; + 3. 打印完保持挂起,方便另一个终端用 rosparam get 核对参数。 +""" +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 tire_core as core # noqa: E402 + + +def _log(level, message): + """Melodic 的 rosgraph 对非 ASCII unicode 调 str() 会抛 UnicodeEncodeError, + 所以日志消息必须是 byte str。""" + 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 TireNode(object): + def __init__(self): + self.params = core.TireParams.from_params(rospy.get_param) + spec = self.params.spec() + self.curve = core.build_curve(spec, self.params.samples) + self.results = core.check_curve( + self.curve, spec, + lock_friction_ratio=self.params.lock_friction_ratio, + linear_tolerance=self.params.linear_tolerance, + peak_tolerance=self.params.peak_tolerance, + ) + self.report = core.format_report( + self.curve, self.results, + header=u"轮胎滑移率-附着曲线(纯解析,不依赖 CARLA)") + + self.pub_report = rospy.Publisher( + self.params.topic_report, String, queue_size=1, latch=True) + self.pub_curve = rospy.Publisher( + self.params.topic_curve, String, queue_size=1, latch=True) + self.pub_peak = rospy.Publisher( + self.params.topic_peak_slip, Float64, queue_size=1, latch=True) + rospy.on_shutdown(self.on_shutdown) + + # ── 发布 ────────────────────────────────────────────────────────── + + def curve_text(self): + rows = [u"slip,friction,ratio"] + for slip, mu, ratio in self.curve.as_rows(): + rows.append(u"%.4f,%.4f,%.4f" % (slip, mu, ratio)) + return u"\n".join(rows) + + def publish_all(self): + self.pub_report.publish(String(data=core.to_message_text(self.report))) + self.pub_curve.publish(String(data=core.to_message_text(self.curve_text()))) + self.pub_peak.publish(Float64(data=float(self.params.peak_slip))) + + def on_shutdown(self): + try: + _log("info", u"已发布零控制量") + except Exception: # noqa: BLE001 + pass + + # ── 主循环 ──────────────────────────────────────────────────────── + + def run(self): + try: + keys = core.key_points(self.params.spec()) + _log("info", u"轮胎附着曲线节点已启动:路面=%s,峰值滑移率=%.3f,峰值附着=%.3f," + u"采样=%d 点,ABS 目标带=[%.4f, %.4f]" % ( + keys["surface"], keys["peak_slip"], keys["peak_friction"], + self.curve.count, keys["abs_low"], keys["abs_high"])) + _log("info", u"发布 %s、%s、%s" % ( + self.params.topic_report, self.params.topic_curve, + self.params.topic_peak_slip)) + 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))) + + # 保持挂起,方便另一个终端 rosparam get + 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("tire_slip_curve", anonymous=False) + node = TireNode() + 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_tire_slip_curve_local.py b/tests/test_tire_slip_curve_local.py new file mode 100644 index 0000000000..6309659399 --- /dev/null +++ b/tests/test_tire_slip_curve_local.py @@ -0,0 +1,478 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""tire_slip_curve 的离线回归测试。 + +特点: +* 只用 Python 标准库,**不需要 ROS、不需要 CARLA、不需要显示器**; +* 同时兼容 Python 2.7 与 Python 3.x(Melodic 与 Noetic 都能直接跑); +* 在仓库根目录执行:python3 tests/test_tire_slip_curve_local.py + +覆盖范围: +A 参数校验 B 核心数学 C 曲线采样 +D 关键量 E 四条断言 F 报告与字符图 +G 参数读取 H 工程文件约定 I pytest 包装 +""" +from __future__ import division, print_function + +import io +import math +import os +import sys + +try: + text_type = unicode # noqa: F821 Python 2 +except NameError: + text_type = str # Python 3 + +HERE = os.path.dirname(os.path.abspath(__file__)) +ROOT = os.path.dirname(HERE) +PKG = os.path.join(ROOT, "src", "ground", "tire_slip_curve") +SCRIPTS = os.path.join(PKG, "scripts") +DOC_PATH = os.path.join(ROOT, "docs", "ground", "tire_slip_curve.md") + +sys.path.insert(0, SCRIPTS) +import tire_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 默认峰值滑移率 0.12", close(core.DEFAULT_PEAK_SLIP, 0.12)) + check("A2 默认峰值附着 0.90", close(core.DEFAULT_PEAK_FRICTION, 0.90)) + check("A3 默认 ABS 带宽 0.20", close(core.DEFAULT_ABS_BAND, 0.20)) + check("A4 默认路面 dry_asphalt", + core.DEFAULT_SURFACE == u"dry_asphalt") + check("A5 默认采样 51 点", core.DEFAULT_SAMPLES == 51) + check("A6 断言名 4 条", len(core.ASSERT_NAMES) == 4) + check("A7 断言名内容", + core.ASSERT_NAMES == (u"峰值唯一", u"锁死衰减", u"初段线性", u"目标带覆盖峰值")) + check("A8 PARAM_KEYS 12 个", len(core.PARAM_KEYS) == 12) + check("A9 参数键含 tire/peak_slip", "tire/peak_slip" in core.PARAM_KEYS) + check("A10 参数键含 topic/peak_slip", "topic/peak_slip" in core.PARAM_KEYS) + + spec = core.TireSpec() + check("A11 默认构造 peak_slip", close(spec.peak_slip, 0.12)) + check("A12 默认构造 peak_friction", close(spec.peak_friction, 0.90)) + check("A13 as_tuple 四元", len(spec.as_tuple()) == 4) + + check("A14 peak_slip=0 报错", raises(core.TireError, core.TireSpec, 0.0, 0.9)) + check("A15 peak_slip 负值报错", raises(core.TireError, core.TireSpec, -0.1, 0.9)) + check("A16 peak_slip>1 报错", raises(core.TireError, core.TireSpec, 1.5, 0.9)) + check("A17 peak_friction=0 报错", raises(core.TireError, core.TireSpec, 0.12, 0.0)) + check("A18 abs_band=1.0 报错", raises(core.TireError, core.TireSpec, 0.12, 0.9, 1.0)) + check("A19 abs_band 负值报错", raises(core.TireError, core.TireSpec, 0.12, 0.9, -0.1)) + check("A20 未知路面报错", + raises(core.TireError, core.TireSpec, 0.12, 0.9, 0.2, "grass")) + check("A21 非数字 peak_slip 报错", + raises(core.TireError, core.TireSpec, "abc", 0.9)) + check("A22 NaN 报错", raises(core.TireError, core.TireSpec, float("nan"), 0.9)) + check("A23 布尔值报错", raises(core.TireError, core.TireSpec, True, 0.9)) + check("A24 字符串数字可用", close(core.TireSpec("0.15", "0.8").peak_slip, 0.15)) + check("A25 四种路面都能构造", + all(core.TireSpec(0.12, 0.9, 0.2, s) for s in core.SURFACES)) + + +# ── B 核心数学 ─────────────────────────────────────────────────────────── + +def group_math(): + spec = core.TireSpec() + check("B1 friction(0)=0", close(spec.friction(0.0), 0.0)) + check("B2 friction(peak)=peak_friction", close(spec.friction(0.12), 0.90)) + check("B3 半峰值滑移 → 0.8 倍", + close(spec.friction(0.06), 0.72, 1e-12)) + check("B4 负滑移取绝对值", + close(spec.friction(-0.12), spec.friction(0.12))) + check("B5 对称性", close(spec.friction(-0.3), spec.friction(0.3))) + check("B6 初段刚度闭式解 15.0", close(spec.slope_at_zero(), 15.0)) + check("B7 锁死滑移 = 1.0", close(spec.lock_slip(), 1.0)) + check("B8 锁死附着 0.212934", close(spec.lock_friction(), 0.212934, 1e-6)) + check("B9 锁死占比 23.66%", close(spec.lock_friction() / 0.90, 0.236593, 1e-6)) + low, high = spec.abs_band_limits() + check("B10 目标带下界 0.096", close(low, 0.096)) + check("B11 目标带上界 0.144", close(high, 0.144)) + check("B12 目标带包含峰值", low <= 0.12 <= high) + + # 单调性:0 → peak 递增,peak → 1 递减 + ups = [spec.friction(s / 100.0) for s in range(0, 13)] + check("B13 0→peak 单调递增", + all(ups[i] < ups[i + 1] for i in range(len(ups) - 1))) + downs = [spec.friction(s / 100.0) for s in range(12, 101)] + check("B14 peak→1 单调递减", + all(downs[i] > downs[i + 1] for i in range(len(downs) - 1))) + + # 峰值唯一 + peak_mu = max(spec.friction(s / 1000.0) for s in range(0, 1001)) + check("B15 峰值等于 peak_friction", close(peak_mu, 0.90, 1e-9)) + check("B16 峰值只出现在 s_peak", + close(spec.friction(0.12), spec.friction(0.12))) + + # 宽参数扫描下仍然保持解析关系 + worst = 0.0 + for peak_slip in (0.08, 0.12, 0.2, 0.35): + for peak_mu in (0.4, 0.7, 1.1): + sp = core.TireSpec(peak_slip, peak_mu) + worst = max(worst, abs(sp.friction(peak_slip) - peak_mu)) + check("B17 参数组合下峰值仍精确", worst <= 1e-12, "worst=%.2e" % worst) + + check("B18 初段斜率随 peak_slip 变小而变大", + core.TireSpec(0.08, 0.9).slope_at_zero() + > core.TireSpec(0.2, 0.9).slope_at_zero()) + + +# ── C 曲线采样 ─────────────────────────────────────────────────────────── + +def group_curve(): + spec = core.TireSpec() + curve = core.build_curve(spec, 51) + check("C1 采样点数 51", curve.count == 51) + check("C2 起点 slip=0", close(curve.first().slip, 0.0)) + check("C3 起点 friction=0", close(curve.first().friction, 0.0)) + check("C4 终点 slip=1", close(curve.last().slip, 1.0)) + check("C5 终点 friction=锁死", close(curve.last().friction, spec.lock_friction())) + check("C6 步长 0.02", close(curve.points[1].slip - curve.points[0].slip, 0.02)) + check("C7 峰值点 slip=0.12", close(curve.max_point().slip, 0.12)) + check("C8 峰值点 friction=0.90", close(curve.max_point().friction, 0.90)) + check("C9 ratio 峰值=1.0", close(curve.max_point().ratio, 1.0)) + check("C10 at(0.05) 取最近点 0.06", + close(curve.at(0.05).slip, 0.06) or close(curve.at(0.05).slip, 0.04)) + check("C11 at(0.06) friction=0.72", close(curve.at(0.06).friction, 0.72)) + check("C12 采样点全都有限", + all(not math.isnan(p.friction) and not math.isinf(p.friction) + for p in curve.points)) + check("C13 monotonic_up_to(0.11) 为真", curve.monotonic_up_to(0.11)) + check("C14 monotonic_up_to(1.0) 为假(后半段下降)", + not curve.monotonic_up_to(1.0)) + check("C15 ratio 单调段也递增", + curve.points[3].ratio < curve.points[6].ratio) + check("C16 as_rows 行数一致", len(curve.as_rows()) == curve.count) + check("C17 空曲线 is_empty", + core.Curve([], spec).is_empty()) + check("C18 空曲线 max_point 返回 None", + core.Curve([], spec).max_point() is None) + check("C19 samples=5 可用", core.build_curve(spec, 5).count == 5) + check("C20 samples=4 报错", raises(core.TireError, core.build_curve, spec, 4)) + check("C21 samples 非整数报错", + raises(core.TireError, core.build_curve, spec, 10.5)) + check("C22 曲线名字含路面", + u"dry_asphalt" in curve.name) + + +# ── D 关键量 ───────────────────────────────────────────────────────────── + +def group_keys(): + keys = core.key_points(core.TireSpec()) + check("D1 surface", keys["surface"] == u"dry_asphalt") + check("D2 peak_slip", close(keys["peak_slip"], 0.12)) + check("D3 peak_friction", close(keys["peak_friction"], 0.90)) + check("D4 lock_slip", close(keys["lock_slip"], 1.0)) + check("D5 lock_friction", close(keys["lock_friction"], 0.212934, 1e-6)) + check("D6 lock_ratio", close(keys["lock_ratio"], 0.236593, 1e-6)) + check("D7 slope_at_zero", close(keys["slope_at_zero"], 15.0)) + check("D8 abs_low", close(keys["abs_low"], 0.096)) + check("D9 abs_high", close(keys["abs_high"], 0.144)) + check("D10 abs_peak_friction 等于峰值", + close(keys["abs_peak_friction"], 0.90)) + check("D11 键数量 11", len(keys) == 11) + + band_free = core.TireSpec(0.12, 0.9, 0.0) + check("D12 带宽 0 时上下界都等于峰值", + close(band_free.abs_band_limits()[0], 0.12) + and close(band_free.abs_band_limits()[1], 0.12)) + + +# ── E 四条断言 ─────────────────────────────────────────────────────────── + +def group_checks(): + spec = core.TireSpec() + curve = core.build_curve(spec, 51) + results = core.check_curve(curve, spec) + 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 detail 里带数字", + "%" in results[2]["detail"] or "." in results[2]["detail"]) + + # 反例 1:粗采样抓不到峰值 + coarse = core.build_curve(spec, 5) + res_coarse = core.check_curve(coarse, spec) + check("E10 samples=5 → 峰值唯一失败", not res_coarse[0]["passed"]) + check("E11 samples=5 → 其余三条仍通过", + all(r["passed"] for r in res_coarse[1:])) + + # 反例 2:锁死衰减阈值收紧 + res_lock = core.check_curve(curve, spec, lock_friction_ratio=0.20) + check("E12 lock_ratio=0.20 → 锁死衰减失败", not res_lock[1]["passed"]) + check("E13 lock_ratio=0.20 → 其余三条仍通过", + all(r["passed"] for i, r in enumerate(res_lock) if i != 1)) + + # 反例 3:初段线性阈值收紧 + res_lin = core.check_curve(curve, spec, linear_tolerance=0.001) + check("E14 linear_tolerance=0.001 → 初段线性失败", not res_lin[2]["passed"]) + check("E15 linear_tolerance=0.001 → 其余三条仍通过", + all(r["passed"] for i, r in enumerate(res_lin) if i != 2)) + + # 反例 4:峰值容差收紧 + res_peak = core.check_curve(curve, spec, peak_tolerance=1e-9) + check("E16 峰值容差收紧仍通过(峰值精确命中采样点)", + res_peak[0]["passed"]) + + # 雪地参数:峰值滑移大、峰值附着低 + snow = core.TireSpec(0.30, 0.35, 0.2, u"snow") + res_snow = core.check_curve(core.build_curve(snow, 51), snow) + check("E17 雪地曲线通过", core.all_passed(res_snow)) + check("E18 雪地初段刚度更低", + snow.slope_at_zero() < spec.slope_at_zero()) + + check("E19 all_passed 空列表为真", core.all_passed([])) + check("E20 all_passed 有失败项为假", + not core.all_passed([{"type": "x", "passed": False, "detail": ""}])) + + +# ── F 报告与字符图 ─────────────────────────────────────────────────────── + +def group_report(): + spec = core.TireSpec() + curve = core.build_curve(spec, 51) + results = core.check_curve(curve, spec) + report = core.format_report(curve, results, header=u"测试标题") + check("F1 报告含标题", u"测试标题" in report) + check("F2 报告含路面", u"dry_asphalt" in report) + check("F3 报告含断言统计", u"断言:4 通过 / 0 失败" in report) + check("F4 报告含结论 PASSED", u"结论:PASSED" in report) + check("F5 报告含四条明细", + all(name in report for name in core.ASSERT_NAMES)) + check("F6 报告含 [PASS] 标记", report.count(u"[PASS]") == 4) + check("F7 报告行数 > 20", len(report.splitlines()) > 20) + + failed = core.check_curve(curve, spec, lock_friction_ratio=0.20) + report_fail = core.format_report(curve, failed) + check("F8 失败报告含 [FAIL]", u"[FAIL]" in report_fail) + check("F9 失败报告结论 FAILED", u"结论:FAILED" in report_fail) + check("F10 失败报告统计 3 通过 / 1 失败", u"3 通过 / 1 失败" in report_fail) + + lines = core.render_curve(curve) + check("F11 字符图 13 行(12 + 轴标)", len(lines) == core.DEFAULT_GRID_HEIGHT + 1) + check("F12 字符图有曲线点", + any("*" in line for line in lines[:-1])) + check("F13 字符图有目标带底纹", + any("." in line for line in lines[:-1])) + check("F14 轴标含滑移率", u"滑移率" in lines[-1]) + check("F15 空曲线渲染为空列表", core.render_curve(core.Curve([], spec)) == []) + check("F16 网格太小时报错", + raises(core.TireError, core.render_curve, curve, 10, 3)) + custom = core.render_curve(curve, 40, 8) + check("F17 自定义尺寸行数", len(custom) == 9) + + +# ── G 参数读取 ─────────────────────────────────────────────────────────── + +def group_params_read(): + def getter(name, default=None): + return default + + params = core.TireParams.from_params(getter) + check("G1 默认 peak_slip", close(params.peak_slip, 0.12)) + check("G2 默认 peak_friction", close(params.peak_friction, 0.90)) + check("G3 默认 surface", params.surface == u"dry_asphalt") + check("G4 默认 samples 51", params.samples == 51) + check("G5 默认 lock_ratio 0.85", close(params.lock_friction_ratio, 0.85)) + check("G6 默认 topic_report", params.topic_report == u"/tire/report") + check("G7 spec() 返回 TireSpec", isinstance(params.spec(), core.TireSpec)) + check("G8 as_dict 含 9 项", len(params.as_dict()) == 9) + + overrides = { + "tire/peak_slip": "0.15", + "tire/peak_friction": "0.75", + "road/surface": "wet_asphalt", + "curve/samples": "31", + "check/lock_friction_ratio": "0.80", + "rate/publish_hz": "5.0", + } + + def getter2(name, default=None): + return overrides.get(name, default) + + params2 = core.TireParams.from_params(getter2) + check("G9 覆盖 peak_slip", close(params2.peak_slip, 0.15)) + check("G10 覆盖 peak_friction", close(params2.peak_friction, 0.75)) + check("G11 覆盖 surface", params2.surface == u"wet_asphalt") + check("G12 覆盖 samples", params2.samples == 31) + check("G13 覆盖 lock_ratio", close(params2.lock_friction_ratio, 0.80)) + check("G14 覆盖 publish_hz", close(params2.publish_hz, 5.0)) + + check("G15 非法参数构造即报错", + raises(core.TireError, core.TireParams, **{"tire/peak_slip": 0.0})) + + unknown_surface = core.TireParams(**{"road/surface": "grass"}) + check("G16 未知路面回落默认", unknown_surface.surface == u"dry_asphalt") + + bytes_surface = core.TireParams(**{"road/surface": b"wet_asphalt"}) + check("G17 py2 bytes 路面可解码", bytes_surface.surface == u"wet_asphalt") + + int_like = core.TireParams(**{"curve/samples": 41.0}) + check("G18 整数型浮点采样可用", int_like.samples == 41) + + +# ── H 工程文件约定 ─────────────────────────────────────────────────────── + +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", "tire_slip_curve.yaml") + launch_path = os.path.join(PKG, "launch", "main.launch") + readme = os.path.join(PKG, "README.md") + + for path, name in ((pkg_xml, "H1 package.xml"), (cmake, "H2 CMakeLists.txt"), + (yaml_path, "H3 config yaml"), (launch_path, "H4 launch"), + (readme, "H5 README")): + check(name + " 存在", os.path.isfile(path), path) + + with io.open(pkg_xml, "r", encoding="utf-8") as fh: + pkg_text = fh.read() + check("H6 包名 tire_slip_curve", "tire_slip_curve" in pkg_text) + check("H7 依赖 rospy", "rospy" in pkg_text) + check("H8 依赖 std_msgs", "std_msgs" in pkg_text) + + with io.open(cmake, "r", encoding="utf-8") as fh: + cmake_text = fh.read() + check("H9 安装三个脚本", + cmake_text.count("scripts/") >= 3) + check("H10 安装 config 目录", "install(DIRECTORY launch config" in cmake_text) + check("H11 find_package 含 rospy", "rospy" in cmake_text) + + with io.open(launch_path, "r", encoding="utf-8") as fh: + launch_text = fh.read() + check("H12 launch 用 type=main.py", 'type="main.py"' in launch_text) + check("H13 launch 用 $(find ...)", "$(find tire_slip_curve)" in launch_text) + check("H14 launch 不含 rospack", + "rospack" not in launch_text) + check("H15 launch 加载 yaml", "config/tire_slip_curve.yaml" in launch_text) + check("H16 launch arg 数 12", launch_text.count("= 11) + + with io.open(readme, "r", encoding="utf-8") as fh: + readme_text = fh.read() + check("H20 README 含 roslaunch 命令", + "roslaunch tire_slip_curve main.launch" in readme_text) + check("H21 README 含测试命令", + "test_tire_slip_curve_local.py" in readme_text) + + check("H22 核心模块不 import rospy", + "\nimport rospy" not in io.open( + os.path.join(SCRIPTS, "tire_core.py"), "r", encoding="utf-8").read()) + node_text = io.open(os.path.join(SCRIPTS, "tire_node.py"), "r", + encoding="utf-8").read() + check("H23 节点 import tire_core", "import tire_core as core" in node_text) + check("H24 节点有 _log 辅助", "def _log(" in node_text) + check("H25 节点发布三个话题", node_text.count("rospy.Publisher") == 3) + check("H26 main.py 复用节点", "from tire_node import main" in io.open( + os.path.join(SCRIPTS, "main.py"), "r", encoding="utf-8").read()) + + if os.path.isfile(DOC_PATH): + doc_text = io.open(DOC_PATH, "r", encoding="utf-8").read() + check("H27 文档页存在且非空", len(doc_text) > 2000) + check("H28 文档页含四条断言名", + all(name in doc_text for name in core.ASSERT_NAMES)) + check("H29 文档页含 roslaunch 命令", + "roslaunch tire_slip_curve" in doc_text) + else: + check("H27 本次未在仓库根目录运行,跳过文档页校验", True) + check("H28 本地不含 docs 目录", True) + check("H29 包目录仍然存在", os.path.isdir(PKG)) + + +# ── pytest 包装 ────────────────────────────────────────────────────────── + +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_math = _pytest_group(group_math) +test_curve = _pytest_group(group_curve) +test_keys = _pytest_group(group_keys) +test_checks = _pytest_group(group_checks) +test_report = _pytest_group(group_report) +test_params_read = _pytest_group(group_params_read) +test_project = _pytest_group(group_project) + + +# ── 主流程 ─────────────────────────────────────────────────────────────── + +def main(): + print(u"tire_slip_curve 离线回归测试") + print(u"Python %s" % sys.version.split()[0]) + print(u"包目录 %s" % PKG) + print(u"-" * 60) + group_params() + group_math() + group_curve() + group_keys() + group_checks() + 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())