[tools] 整理导航打点工具与抽样PCD
This commit is contained in:
@@ -37,6 +37,8 @@ wandb/
|
||||
sim2sim_log_*.txt
|
||||
**/sim2sim_temp.xml
|
||||
**/route_check_runs/
|
||||
**/route_experiments/suite_*/
|
||||
**/tools/nav_tools/points/auto_candidates/
|
||||
|
||||
# IDE and operating system files
|
||||
.idea/
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
| `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
@@ -61,3 +62,12 @@
|
||||
- 增加 PT→ONNX 导出、IK 补偿扫描、纯 IK 绕桩和 ONNX 批量路线检查入口。
|
||||
- 归档比赛最终 Rough 策略 `model_6800.onnx`；其 SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
|
||||
- Crawl 权重、运行日志、临时 XML 和大量重复路线实验不在本阶段归档。
|
||||
|
||||
## `v0.8.1` 的导航打点工具
|
||||
|
||||
- 补充 Pygame 地图/PCD/航点综合编辑器、避障区域编辑器和坐标变换工具。
|
||||
- 补充路线安全检查、候选航点优化、XML/航点镜像和批量 Sim2Sim 实验入口。
|
||||
- 按源文件时间保留 14 份比赛路线 JSON，不将开发期文件名误解释为正式版本号。
|
||||
- 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
|
||||
- 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
|
||||
- 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。
|
||||
|
||||
@@ -28,7 +28,7 @@ MJCF + mjlab task
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
`rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。
|
||||
|
||||
当前内容对应 `v0.8.0`：训练代码保持 `v0.6.0` 的比赛架构，包含 `v0.7.0` 的 MuJoCo 独立工具，并新增后期 Sim2Sim、路线检查和比赛最终 Rough ONNX 策略。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
|
||||
当前内容对应 `v0.8.1`：训练代码保持 `v0.6.0` 的比赛架构，包含后期 MuJoCo、Sim2Sim、比赛最终 Rough ONNX 策略，并补充完整导航打点工具、路线迭代和抽样 PCD。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
|
||||
|
||||
## 内容
|
||||
|
||||
@@ -10,6 +10,7 @@
|
||||
- `mjcf`：轮足机器人 MuJoCo 模型和网格
|
||||
- `sim2sim`：策略加载、交互控制和比赛地形验证
|
||||
- `mujoco_sim`：不依赖训练循环的姿态、IK、动力学和 MPC 分析
|
||||
- `tools/nav_tools`：地图/PCD 查看、航点编辑、路线检查和比赛路线数据
|
||||
- `mjlab`：固定版本的本地训练框架依赖
|
||||
- `model_rough.pt`：本阶段 Rough 策略权重
|
||||
- `model_6800.onnx`：比赛最终使用的 Rough 策略
|
||||
@@ -25,4 +26,6 @@
|
||||
|
||||
`v0.8.0` 继续保持训练架构和 MJCF 不变，归档后期 Sim2Sim 增量与比赛 Rough ONNX 策略；入口和归档边界见 [`rc_mjlab/sim2sim/README.md`](rc_mjlab/sim2sim/README.md)。
|
||||
|
||||
`v0.8.1` 补充完整导航打点工具和小体积预览点云；入口、路线清单和抽样边界见 [`rc_mjlab/tools/nav_tools/README.md`](rc_mjlab/tools/nav_tools/README.md)。
|
||||
|
||||
工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
- PyTorch CUDA 12.8 环境
|
||||
- `pynput`
|
||||
- 后期 Sim2Sim 可选依赖：Pygame、ONNX Runtime
|
||||
- 导航打点可选依赖：Pygame、Pillow
|
||||
|
||||
精确解析结果保存在 `uv.lock`。项目使用本地可编辑的 `mjlab`：
|
||||
|
||||
@@ -46,4 +47,10 @@ uv run play Robot-Rough-v0
|
||||
uv run --with-requirements sim2sim/requirements.txt python sim2sim/nav_sim2sim.py
|
||||
```
|
||||
|
||||
导航打点工具同样不修改历史锁文件：
|
||||
|
||||
```bash
|
||||
uv run --with-requirements tools/nav_tools/requirements.txt python tools/nav_tools/nav_map_viewer.py
|
||||
```
|
||||
|
||||
GPU、CUDA、MuJoCo development wheel 和驱动版本必须满足 `pyproject.toml` 与 `uv.lock` 的约束。
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。
|
||||
|
||||
> 当前目录对应 `v0.8.0`：保留 `v0.6.0` 的比赛训练架构与 `v0.7.0` 的 MuJoCo 独立工具集，加入后期 Sim2Sim 工具和比赛最终 Rough 策略 `model_6800.onnx`。`model_rough.pt` 仍作为早期参考权重保留。
|
||||
> 当前目录对应 `v0.8.1`：保留比赛训练架构、后期 MuJoCo/Sim2Sim 和比赛最终 Rough 策略，并补充导航地图、打点工具、路线迭代与抽样 PCD。`model_rough.pt` 仍作为早期参考权重保留。
|
||||
|
||||
---
|
||||
|
||||
@@ -58,7 +58,7 @@ rc_mjlab/
|
||||
│ ├── scene.xml # mjlab 场景入口文件
|
||||
│ └── meshes/ # STL/OBJ 碰撞与外观网格
|
||||
├── mujoco_sim/ # 姿态、IK、动力学和 MPC 独立工具
|
||||
├── tools/nav_tools/ # 路线安全检查公共模块
|
||||
├── tools/nav_tools/ # 地图/PCD/航点编辑、路线检查与比赛路线数据
|
||||
├── model_rough.pt # 早期 Rough 参考 checkpoint
|
||||
├── model_6800.onnx # 比赛最终 Rough 策略
|
||||
├── pyproject.toml # 项目依赖（uv 管理，含清华镜像源加速）
|
||||
@@ -97,6 +97,8 @@ uv run python nav_sim2sim.py
|
||||
|
||||
后期 Sim2Sim 的入口、模型边界和批量检查命令见 [`sim2sim/README.md`](sim2sim/README.md)。
|
||||
|
||||
导航打点工具、路线快照和抽样点云说明见 [`tools/nav_tools/README.md`](tools/nav_tools/README.md)。
|
||||
|
||||
---
|
||||
|
||||
## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)
|
||||
|
||||
@@ -53,6 +53,6 @@ uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\export_on
|
||||
- `../model_6800.onnx` 是 `last_not_slalom_1050` 最终真机工程使用的比赛 Rough 策略，SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
|
||||
- `../model_rough.pt` 是较早阶段的参考 checkpoint，两者不是同一版本的权重。
|
||||
- Crawl 模型未在本阶段归档；需要 Crawl 策略的入口会查找 `model_crawl.onnx` 或 `model_crawl.pt`。
|
||||
- `nav_route_sim2sim_check.py` 依赖 `../tools/nav_tools/route_safety_check.py` 的航点和避障几何定义。
|
||||
- `nav_route_sim2sim_check.py` 依赖 `../tools/nav_tools/route_safety_check.py` 的航点和避障几何定义；默认使用 `points_20260715_120154.json` 与 `1hao.xml`。
|
||||
|
||||
运行时生成的日志、临时 XML 和 `route_check_runs/` 不纳入版本库。源目录中的大量路线试验结果也未复制；它们包含重复轨迹和本机绝对路径，不属于可复用程序源码。
|
||||
运行时生成的日志、临时 XML、`route_check_runs/` 和批量实验输出不纳入版本库。人工打点形成的路线快照保存在 `../tools/nav_tools/points/`，大量重复仿真轨迹仍不复制。
|
||||
|
||||
@@ -1235,8 +1235,8 @@ class SlalomScriptFollower(NavGoodFollower):
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Run MuJoCo+ONNX route validation.")
|
||||
parser.add_argument("--terrain-xml", type=Path, default=PROJECT_ROOT / "tools/nav_tools/xml/A.xml")
|
||||
parser.add_argument("--points", type=Path, default=PROJECT_ROOT / "tools/nav_tools/points/points_20260705_174627.json")
|
||||
parser.add_argument("--terrain-xml", type=Path, default=PROJECT_ROOT / "tools/nav_tools/xml/1hao.xml")
|
||||
parser.add_argument("--points", type=Path, default=PROJECT_ROOT / "tools/nav_tools/points/points_20260715_120154.json")
|
||||
parser.add_argument("--onnx", type=Path, default=PROJECT_ROOT / "model_6800.onnx")
|
||||
parser.add_argument("--crawl-onnx", type=Path, default=PROJECT_ROOT / "model_crawl.onnx")
|
||||
parser.add_argument(
|
||||
|
||||
@@ -1,7 +1,93 @@
|
||||
# 路线检查公共模块
|
||||
# 导航地图与打点工具
|
||||
|
||||
`route_safety_check.py` 提供航点、避障区域、机器人平面包络和几何距离计算，供 `sim2sim/nav_route_sim2sim_check.py` 复用。
|
||||
本目录保存比赛后期使用的独立导航地图、PCD 查看、航点编辑、避障区域编辑和路线验证工具。它同时为 `sim2sim/nav_route_sim2sim_check.py` 提供航点与避障几何定义。
|
||||
|
||||
该模块只依赖 Python 标准库。原开发目录中的地图编辑器、PCD、比赛路线 JSON、备份和批量实验结果不属于本次后期 Sim2Sim 里程碑，未在这里复制。
|
||||
## 目录
|
||||
|
||||
外部路线文件需要包含 `waypoints`（或 `segments[].waypoints`）以及 `regions` / `avoid_regions`。也可以不提供路线文件，直接使用 Sim2Sim 检查器的内置任务。
|
||||
```text
|
||||
nav_tools/
|
||||
├─ nav_map_viewer.py # 地图、PCD、XML、航点和避障区综合编辑器
|
||||
├─ avoid_region_tool.py # 独立避障多边形编辑器
|
||||
├─ route_safety_check.py # 离线路线净空检查
|
||||
├─ route_candidate_optimizer.py # 航点候选优化
|
||||
├─ run_route_experiments.py # 安全检查与 Sim2Sim 批量实验
|
||||
├─ mirror_nav_xml_points.py # XML 与航点镜像
|
||||
├─ pcd_transform_tool.py # PCD 平移和旋转
|
||||
├─ transform_pcd_xy.py # PCD 坐标原点变换
|
||||
├─ downsample_ascii_pcd.py # ASCII PCD 确定性抽样
|
||||
├─ annotate_odin1_relocalization_frame.py
|
||||
├─ pcd/ # 小于 10 MB 的打点预览点云
|
||||
├─ points/ # 比赛期间的路线迭代 JSON
|
||||
├─ xml/ # 1 号、2 号和 A/B 场地 XML
|
||||
├─ regions/ # 独立避障区域输出目录
|
||||
└─ assets/ # 坐标和机构示意图
|
||||
```
|
||||
|
||||
## 运行
|
||||
|
||||
在 `rc_mjlab` 根目录执行：
|
||||
|
||||
```powershell
|
||||
# 地图与航点综合编辑器
|
||||
uv run --with-requirements .\tools\nav_tools\requirements.txt `
|
||||
python .\tools\nav_tools\nav_map_viewer.py
|
||||
|
||||
# 指定 PCD 的独立避障区域编辑器
|
||||
uv run --with-requirements .\tools\nav_tools\requirements.txt `
|
||||
python .\tools\nav_tools\avoid_region_tool.py --pcd 1hao.pcd
|
||||
|
||||
# 默认检查 points_20260715_120154.json 与 1hao.xml
|
||||
uv run python .\tools\nav_tools\route_safety_check.py
|
||||
|
||||
# 使用最终 Rough 策略进行默认路线 Sim2Sim 检查
|
||||
uv run --with-requirements .\sim2sim\requirements.txt `
|
||||
python .\sim2sim\nav_route_sim2sim_check.py
|
||||
```
|
||||
|
||||
候选路线和批量实验必须显式保存为新文件，不应覆盖历史路线：
|
||||
|
||||
```powershell
|
||||
uv run python .\tools\nav_tools\route_candidate_optimizer.py
|
||||
|
||||
uv run --with-requirements .\sim2sim\requirements.txt `
|
||||
python .\tools\nav_tools\run_route_experiments.py `
|
||||
--points .\tools\nav_tools\points\points_20260715_120154.json
|
||||
```
|
||||
|
||||
## PCD 抽样
|
||||
|
||||
仓库中的两份 PCD 从原始 ASCII 点云按固定步长均匀抽样，字段、坐标和 PCD 头结构保持不变。它们面向地图显示和人工打点，不替代原始高密度点云用于建图、定位精度评估或点云算法基准。
|
||||
|
||||
| 文件 | 原始点数 | 抽样步长 | 仓库点数 | 仓库大小 | SHA-256 |
|
||||
| --- | ---: | ---: | ---: | ---: | --- |
|
||||
| `pcd/1hao.pcd` | 9,163,893 | 46 | 199,215 | 9,876,010 B | `48B231C52BECA51316F352300C8B2046133E92359E0855227D93DEB0D927AD34` |
|
||||
| `pcd/2hao.pcd` | 10,043,048 | 52 | 193,136 | 9,719,736 B | `714516A7A726D46311A58507149FBC93D622616BE79C59FFED622274D6526B1F` |
|
||||
|
||||
复现抽样：
|
||||
|
||||
```powershell
|
||||
python .\tools\nav_tools\downsample_ascii_pcd.py `
|
||||
<原始PCD> <输出PCD> --max-bytes 9900000
|
||||
```
|
||||
|
||||
## 航点数据
|
||||
|
||||
`points/` 保留原文件名和时间顺序，没有把多个路线重命名成 `old`、`new` 或 `final`。各文件的航点数、避障区和 XML 绑定见 [`points/README.md`](points/README.md)。
|
||||
|
||||
- 请求目录中的 `route_safety_check.py` 是较早版本，因此保留 `v0.8.0` 已归档的后期兼容版本。
|
||||
- `1B_FF.json` 引用了请求目录中缺失的 `xml/B_C.xml`，本次从后期整合目录补齐该文件。
|
||||
- `.uv-cache`、`__pycache__`、自动候选、实验日志和重复轨迹输出不归档。
|
||||
|
||||
整理时使用默认圆形包络和 `0.05 m` 额外净空检查 `points_20260715_120154.json`，报告了 3 组线段—避障区净空不足。该结果按原样保留，未自动移动航点；它表示保守几何检查仍有待复核，不等同于路线没有经过实机使用。
|
||||
|
||||
## 主要操作
|
||||
|
||||
- `Pan`：拖动地图；滚轮缩放；`F` 适应窗口。
|
||||
- `Point`：添加、插入、选择和编辑航点；`Backspace` 删除。
|
||||
- `Terrain`：选择并调整 XML 障碍组的位置和偏航。
|
||||
- `Avoid`：绘制避障多边形；`Enter` 闭合；`Delete` 删除。
|
||||
- `Load JSON`：载入 `waypoints` 或 `segments[].waypoints`。
|
||||
- `Save JSON` / `Save All JSON`：同时保存航点和避障区域。
|
||||
- `M`：切换 XML；`O`：切换坐标原点；`Esc`：退出。
|
||||
|
||||
任务字段约定：`none`、`slalom`、`gravel`、`wall`、`low_bar`、`stairs`、`ramp_bridge`、`spawn` 和 `return`。
|
||||
|
||||
@@ -0,0 +1,145 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate an annotated Odin1 relocalization-frame PNG.
|
||||
|
||||
The wiki defines Odin1 frames as:
|
||||
I: IMU frame
|
||||
L: LiDAR / point-cloud frame
|
||||
C = camera frame
|
||||
|
||||
Relocalization poses are tied to the SLAM point-cloud map, so the device pose
|
||||
should be treated as T_map_L unless a driver-specific TF remaps it.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from PIL import Image, ImageDraw, ImageFont
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
IMG_DIR = TOOL_DIR / "assets"
|
||||
OUT = TOOL_DIR / "odin1_relocalization_frame.png"
|
||||
|
||||
|
||||
def font(size: int, bold: bool = False) -> ImageFont.FreeTypeFont | ImageFont.ImageFont:
|
||||
return ImageFont.load_default()
|
||||
|
||||
|
||||
def arrow(draw: ImageDraw.ImageDraw, start: tuple[int, int], end: tuple[int, int], color: tuple[int, int, int], width: int = 5) -> None:
|
||||
draw.line([start, end], fill=color, width=width)
|
||||
sx, sy = start
|
||||
ex, ey = end
|
||||
dx = ex - sx
|
||||
dy = ey - sy
|
||||
length = max((dx * dx + dy * dy) ** 0.5, 1.0)
|
||||
ux, uy = dx / length, dy / length
|
||||
px, py = -uy, ux
|
||||
head = 18
|
||||
wing = 9
|
||||
points = [
|
||||
(ex, ey),
|
||||
(int(ex - ux * head + px * wing), int(ey - uy * head + py * wing)),
|
||||
(int(ex - ux * head - px * wing), int(ey - uy * head - py * wing)),
|
||||
]
|
||||
draw.polygon(points, fill=color)
|
||||
|
||||
|
||||
def label_box(
|
||||
draw: ImageDraw.ImageDraw,
|
||||
xy: tuple[int, int],
|
||||
text: str,
|
||||
fill: tuple[int, int, int],
|
||||
text_color: tuple[int, int, int] = (255, 255, 255),
|
||||
size: int = 24,
|
||||
) -> None:
|
||||
x, y = xy
|
||||
fnt = font(size, bold=True)
|
||||
lines = text.splitlines()
|
||||
widths = [draw.textbbox((0, 0), line, font=fnt)[2] for line in lines]
|
||||
heights = [draw.textbbox((0, 0), line, font=fnt)[3] - draw.textbbox((0, 0), line, font=fnt)[1] for line in lines]
|
||||
box_w = max(widths) + 28
|
||||
box_h = sum(heights) + 14 * (len(lines) - 1) + 24
|
||||
draw.rounded_rectangle((x, y, x + box_w, y + box_h), radius=10, fill=fill, outline=(255, 255, 255), width=2)
|
||||
cy = y + 12
|
||||
for line, h in zip(lines, heights):
|
||||
draw.text((x + 14, cy), line, fill=text_color, font=fnt)
|
||||
cy += h + 14
|
||||
|
||||
|
||||
def main() -> int:
|
||||
coordinate = Image.open(IMG_DIR / "coordinate.png").convert("RGB")
|
||||
structure = Image.open(IMG_DIR / "structure1.png").convert("RGB")
|
||||
|
||||
target_w = 1180
|
||||
coordinate = coordinate.resize((target_w, int(coordinate.height * target_w / coordinate.width)))
|
||||
structure = structure.resize((target_w, int(structure.height * target_w / structure.width)))
|
||||
|
||||
gap = 24
|
||||
margin = 30
|
||||
title_h = 118
|
||||
canvas = Image.new("RGB", (target_w + margin * 2, title_h + coordinate.height + gap + structure.height + margin), (245, 247, 250))
|
||||
draw = ImageDraw.Draw(canvas)
|
||||
|
||||
title_font = font(34, bold=True)
|
||||
body_font = font(22)
|
||||
draw.text((margin, 22), "Odin1 relocalization frame: LiDAR / point-cloud frame L", fill=(15, 23, 42), font=title_font)
|
||||
draw.text(
|
||||
(margin, 68),
|
||||
"Use the relocalization pose as T_map_L. Convert to robot base with your measured T_L_base.",
|
||||
fill=(51, 65, 85),
|
||||
font=body_font,
|
||||
)
|
||||
|
||||
top_y = title_h
|
||||
bot_y = title_h + coordinate.height + gap
|
||||
canvas.paste(coordinate, (margin, top_y))
|
||||
canvas.paste(structure, (margin, bot_y))
|
||||
|
||||
red = (220, 38, 38)
|
||||
blue = (37, 99, 235)
|
||||
green = (22, 163, 74)
|
||||
amber = (217, 119, 6)
|
||||
|
||||
# coordinate.png positions after scaling to 1180 px.
|
||||
lidar_origin = (margin + 690, top_y + 255)
|
||||
imu_origin = (margin + 286, top_y + 655)
|
||||
camera_origin = (margin + 885, top_y + 500)
|
||||
|
||||
draw.ellipse((lidar_origin[0] - 12, lidar_origin[1] - 12, lidar_origin[0] + 12, lidar_origin[1] + 12), fill=red, outline=(255, 255, 255), width=3)
|
||||
arrow(draw, (margin + 900, top_y + 150), lidar_origin, red, width=6)
|
||||
label_box(draw, (margin + 725, top_y + 70), "Relocalization pose\nis here: frame L", red)
|
||||
|
||||
draw.ellipse((imu_origin[0] - 9, imu_origin[1] - 9, imu_origin[0] + 9, imu_origin[1] + 9), fill=blue)
|
||||
arrow(draw, (margin + 160, top_y + 555), imu_origin, blue, width=4)
|
||||
label_box(draw, (margin + 40, top_y + 460), "IMU frame I\nnot the relocalization origin", blue, size=20)
|
||||
|
||||
draw.ellipse((camera_origin[0] - 9, camera_origin[1] - 9, camera_origin[0] + 9, camera_origin[1] + 9), fill=green)
|
||||
arrow(draw, (margin + 1015, top_y + 565), camera_origin, green, width=4)
|
||||
label_box(draw, (margin + 880, top_y + 585), "Camera frame C\nseparate optical frame", green, size=20)
|
||||
|
||||
# structure1.png positions after scaling to 1180 px.
|
||||
struct_imu = (margin + 398, bot_y + 357)
|
||||
struct_lidar_hint = (margin + 677, bot_y + 350)
|
||||
draw.ellipse((struct_imu[0] - 9, struct_imu[1] - 9, struct_imu[0] + 9, struct_imu[1] + 9), fill=blue)
|
||||
arrow(draw, (margin + 245, bot_y + 250), struct_imu, blue, width=4)
|
||||
label_box(draw, (margin + 45, bot_y + 165), "Wiki marks IMU separately", blue, size=20)
|
||||
|
||||
arrow(draw, (margin + 820, bot_y + 230), struct_lidar_hint, red, width=5)
|
||||
label_box(draw, (margin + 830, bot_y + 145), "L is the point-cloud/LiDAR frame\nshown in the coordinate diagram", red, size=20)
|
||||
|
||||
note = (
|
||||
"Fixed wiki extrinsic: T^imu_lidar translation = [-0.02663, 0.03447, 0.02174] m, rotation = identity.\n"
|
||||
"So L and I axes are parallel, but their origins are offset. Do not use shell center as pose origin."
|
||||
)
|
||||
note_box = (margin, canvas.height - margin - 78, canvas.width - margin, canvas.height - margin)
|
||||
draw.rounded_rectangle(note_box, radius=10, fill=(255, 251, 235), outline=amber, width=2)
|
||||
draw.text((margin + 16, canvas.height - margin - 64), note, fill=(120, 53, 15), font=font(19))
|
||||
|
||||
canvas.save(OUT)
|
||||
print(OUT)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 208 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 28 KiB |
@@ -0,0 +1,373 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Pygame tool for marking no-go / avoid regions on a PCD map."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import pkgutil
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
|
||||
os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")
|
||||
|
||||
if not hasattr(pkgutil, "ImpImporter"):
|
||||
pkgutil.ImpImporter = pkgutil.zipimporter # type: ignore[attr-defined]
|
||||
|
||||
import pygame
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
PCD_DIR = TOOL_DIR / "pcd"
|
||||
REGION_DIR = TOOL_DIR / "regions"
|
||||
|
||||
COLOR_BG = (10, 15, 30)
|
||||
COLOR_GRID = (22, 29, 48)
|
||||
COLOR_PANEL = (18, 24, 42)
|
||||
COLOR_BORDER = (50, 65, 95)
|
||||
COLOR_TEXT = (248, 250, 252)
|
||||
COLOR_MUTED = (148, 163, 184)
|
||||
COLOR_PCD = (92, 160, 255)
|
||||
COLOR_REGION = (244, 63, 94)
|
||||
COLOR_CURRENT = (234, 179, 8)
|
||||
|
||||
|
||||
@dataclass
|
||||
class PointCloud:
|
||||
path: Path | None = None
|
||||
points: list[tuple[float, float, float]] = field(default_factory=list)
|
||||
total_count: int = 0
|
||||
sampled_count: int = 0
|
||||
error: str = ""
|
||||
|
||||
|
||||
@dataclass
|
||||
class Region:
|
||||
name: str
|
||||
points: list[tuple[float, float]]
|
||||
kind: str = "avoid"
|
||||
|
||||
|
||||
class Camera:
|
||||
def __init__(self, width: int, height: int, panel_width: int = 330) -> None:
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.panel_width = panel_width
|
||||
self.zoom = 55.0
|
||||
self.pan_x = 0.0
|
||||
self.pan_y = 0.0
|
||||
|
||||
@property
|
||||
def map_width(self) -> int:
|
||||
return max(200, self.width - self.panel_width)
|
||||
|
||||
def world_to_screen(self, x: float, y: float) -> tuple[int, int]:
|
||||
return int(self.map_width / 2 + x * self.zoom + self.pan_x), int(self.height / 2 - y * self.zoom + self.pan_y)
|
||||
|
||||
def screen_to_world(self, sx: float, sy: float) -> tuple[float, float]:
|
||||
return (sx - self.map_width / 2 - self.pan_x) / self.zoom, -(sy - self.height / 2 - self.pan_y) / self.zoom
|
||||
|
||||
def zoom_at(self, factor: float, pos: tuple[int, int]) -> None:
|
||||
before = self.screen_to_world(*pos)
|
||||
self.zoom = max(8.0, min(260.0, self.zoom * factor))
|
||||
after = self.screen_to_world(*pos)
|
||||
self.pan_x += (after[0] - before[0]) * self.zoom
|
||||
self.pan_y -= (after[1] - before[1]) * self.zoom
|
||||
|
||||
|
||||
def get_font(size: int, bold: bool = False) -> pygame.font.Font:
|
||||
return pygame.font.Font(None, size)
|
||||
|
||||
|
||||
def scan_pcd_files() -> list[Path]:
|
||||
PCD_DIR.mkdir(parents=True, exist_ok=True)
|
||||
return sorted(PCD_DIR.glob("*.pcd"), key=lambda item: item.name.lower())
|
||||
|
||||
|
||||
def iter_ascii_pcd_points(path: Path):
|
||||
with path.open("r", encoding="utf-8", errors="ignore") as handle:
|
||||
data = False
|
||||
for raw in handle:
|
||||
line = raw.strip()
|
||||
if data and line:
|
||||
parts = line.split()
|
||||
if len(parts) >= 3:
|
||||
yield float(parts[0]), float(parts[1]), float(parts[2])
|
||||
elif line.upper().startswith("DATA"):
|
||||
if "ascii" not in line.lower():
|
||||
raise RuntimeError(f"Only ASCII PCD is supported: {path}")
|
||||
data = True
|
||||
|
||||
|
||||
def load_pcd(path: Path, max_points: int = 90000, z_min: float = -5.0, z_max: float = 0.5) -> PointCloud:
|
||||
cloud = PointCloud(path=path)
|
||||
try:
|
||||
filtered = []
|
||||
for x, y, z in iter_ascii_pcd_points(path):
|
||||
cloud.total_count += 1
|
||||
if z_min <= z <= z_max:
|
||||
filtered.append((x, y, z))
|
||||
stride = max(1, math.ceil(len(filtered) / max_points)) if filtered else 1
|
||||
cloud.points = filtered[::stride]
|
||||
cloud.sampled_count = len(cloud.points)
|
||||
except Exception as exc:
|
||||
cloud.error = str(exc)
|
||||
return cloud
|
||||
|
||||
|
||||
def fit_camera(camera: Camera, cloud: PointCloud, regions: list[Region]) -> None:
|
||||
xs = [p[0] for p in cloud.points]
|
||||
ys = [p[1] for p in cloud.points]
|
||||
for region in regions:
|
||||
xs.extend(p[0] for p in region.points)
|
||||
ys.extend(p[1] for p in region.points)
|
||||
if not xs or not ys:
|
||||
return
|
||||
min_x, max_x = min(xs), max(xs)
|
||||
min_y, max_y = min(ys), max(ys)
|
||||
span_x = max(max_x - min_x, 1.0)
|
||||
span_y = max(max_y - min_y, 1.0)
|
||||
margin = 70
|
||||
camera.zoom = max(8.0, min(260.0, min((camera.map_width - margin * 2) / span_x, (camera.height - margin * 2) / span_y)))
|
||||
camera.pan_x = -((min_x + max_x) / 2) * camera.zoom
|
||||
camera.pan_y = ((min_y + max_y) / 2) * camera.zoom
|
||||
|
||||
|
||||
def draw_grid(surface: pygame.Surface, camera: Camera, font: pygame.font.Font) -> None:
|
||||
min_x, min_y = camera.screen_to_world(0, camera.height)
|
||||
max_x, max_y = camera.screen_to_world(camera.map_width, 0)
|
||||
for gx in range(math.floor(min_x), math.ceil(max_x) + 1):
|
||||
sx, _ = camera.world_to_screen(gx, 0)
|
||||
color = (38, 50, 78) if gx == 0 else COLOR_GRID
|
||||
pygame.draw.line(surface, color, (sx, 0), (sx, camera.height), 2 if gx == 0 else 1)
|
||||
if gx % 2 == 0 and 0 < sx < camera.map_width - 30:
|
||||
surface.blit(font.render(f"{gx}m", True, COLOR_MUTED), (sx + 4, camera.height - 22))
|
||||
for gy in range(math.floor(min_y), math.ceil(max_y) + 1):
|
||||
_, sy = camera.world_to_screen(0, gy)
|
||||
color = (38, 50, 78) if gy == 0 else COLOR_GRID
|
||||
pygame.draw.line(surface, color, (0, sy), (camera.map_width, sy), 2 if gy == 0 else 1)
|
||||
|
||||
|
||||
def draw_pcd(surface: pygame.Surface, camera: Camera, cloud: PointCloud) -> None:
|
||||
for x, y, z in cloud.points:
|
||||
sx, sy = camera.world_to_screen(x, y)
|
||||
if 0 <= sx < camera.map_width and 0 <= sy < camera.height:
|
||||
color = (148, 210, 255) if z > 0.3 else COLOR_PCD
|
||||
surface.set_at((sx, sy), color)
|
||||
|
||||
|
||||
def draw_regions(surface: pygame.Surface, camera: Camera, regions: list[Region], current: list[tuple[float, float]]) -> None:
|
||||
overlay = pygame.Surface(surface.get_size(), pygame.SRCALPHA)
|
||||
for region in regions:
|
||||
points = [camera.world_to_screen(x, y) for x, y in region.points]
|
||||
if len(points) >= 3:
|
||||
pygame.draw.polygon(overlay, (*COLOR_REGION, 75), points)
|
||||
pygame.draw.polygon(surface, COLOR_REGION, points, width=2)
|
||||
if current:
|
||||
points = [camera.world_to_screen(x, y) for x, y in current]
|
||||
for point in points:
|
||||
pygame.draw.circle(surface, COLOR_CURRENT, point, 5)
|
||||
if len(points) >= 2:
|
||||
pygame.draw.lines(surface, COLOR_CURRENT, False, points, width=2)
|
||||
surface.blit(overlay, (0, 0))
|
||||
|
||||
|
||||
def save_regions(regions: list[Region], cloud: PointCloud) -> Path:
|
||||
REGION_DIR.mkdir(parents=True, exist_ok=True)
|
||||
stamp = time.strftime("%Y%m%d_%H%M%S")
|
||||
path = REGION_DIR / f"avoid_regions_{stamp}.json"
|
||||
payload = {
|
||||
"name": f"avoid_regions_{stamp}",
|
||||
"map": cloud.path.stem if cloud.path else "",
|
||||
"frame_id": "map",
|
||||
"regions": [
|
||||
{
|
||||
"id": index,
|
||||
"name": region.name,
|
||||
"kind": region.kind,
|
||||
"polygon": [{"x": x, "y": y} for x, y in region.points],
|
||||
}
|
||||
for index, region in enumerate(regions, start=1)
|
||||
],
|
||||
}
|
||||
path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
return path
|
||||
|
||||
|
||||
def draw_panel(
|
||||
surface: pygame.Surface,
|
||||
camera: Camera,
|
||||
title_font: pygame.font.Font,
|
||||
small_font: pygame.font.Font,
|
||||
pcd_files: list[Path],
|
||||
selected_pcd: int,
|
||||
cloud: PointCloud,
|
||||
regions: list[Region],
|
||||
current: list[tuple[float, float]],
|
||||
saved: Path | None,
|
||||
) -> list[tuple[pygame.Rect, str]]:
|
||||
x0 = camera.map_width
|
||||
pygame.draw.rect(surface, COLOR_PANEL, (x0, 0, camera.panel_width, camera.height))
|
||||
pygame.draw.line(surface, COLOR_BORDER, (x0, 0), (x0, camera.height), 2)
|
||||
x = x0 + 18
|
||||
y = 16
|
||||
buttons: list[tuple[pygame.Rect, str]] = []
|
||||
|
||||
surface.blit(title_font.render("avoid regions", True, COLOR_TEXT), (x, y))
|
||||
y += 34
|
||||
label = pcd_files[selected_pcd].name if pcd_files and selected_pcd >= 0 else "put .pcd in nav_tools/pcd"
|
||||
surface.blit(small_font.render(f"PCD: {label[:32]}", True, COLOR_MUTED), (x, y))
|
||||
y += 24
|
||||
if cloud.error:
|
||||
surface.blit(small_font.render(cloud.error[:36], True, COLOR_REGION), (x, y))
|
||||
else:
|
||||
surface.blit(small_font.render(f"points: {cloud.sampled_count}/{cloud.total_count}", True, COLOR_MUTED), (x, y))
|
||||
y += 34
|
||||
|
||||
for text, action in (("Prev PCD", "prev_pcd"), ("Next PCD", "next_pcd"), ("Fit", "fit"), ("Save JSON", "save")):
|
||||
rect = pygame.Rect(x, y, 128, 26)
|
||||
pygame.draw.rect(surface, (30, 41, 59), rect, border_radius=5)
|
||||
pygame.draw.rect(surface, COLOR_BORDER, rect, width=1, border_radius=5)
|
||||
surface.blit(small_font.render(text, True, COLOR_TEXT), (rect.x + 10, rect.y + 6))
|
||||
buttons.append((rect, action))
|
||||
y += 32
|
||||
|
||||
y += 12
|
||||
lines = [
|
||||
f"regions: {len(regions)}",
|
||||
f"current vertices: {len(current)}",
|
||||
"",
|
||||
"Left click: add vertex",
|
||||
"Enter: close polygon",
|
||||
"Backspace: undo vertex",
|
||||
"Delete: remove last region",
|
||||
"Right/Middle drag: pan",
|
||||
"Wheel: zoom",
|
||||
"S: save, F: fit",
|
||||
]
|
||||
for line in lines:
|
||||
color = COLOR_TEXT if line and ":" not in line else COLOR_MUTED
|
||||
surface.blit(small_font.render(line, True, color), (x, y))
|
||||
y += 20
|
||||
if saved:
|
||||
y += 8
|
||||
surface.blit(small_font.render(f"saved: {saved.name[:28]}", True, (16, 185, 129)), (x, y))
|
||||
return buttons
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Mark avoid/no-go regions on a PCD map.")
|
||||
parser.add_argument("--width", type=int, default=1280)
|
||||
parser.add_argument("--height", type=int, default=820)
|
||||
parser.add_argument("--pcd", type=Path, help="PCD file name/path. Relative paths are resolved from tools/nav_tools/pcd.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
pcd_files = scan_pcd_files()
|
||||
selected_pcd = 0
|
||||
if args.pcd:
|
||||
if args.pcd.is_absolute():
|
||||
raise SystemExit("Use a PCD file name under tools/nav_tools/pcd, not an absolute path.")
|
||||
requested = PCD_DIR / args.pcd
|
||||
if requested.exists() and requested not in pcd_files:
|
||||
pcd_files.append(requested)
|
||||
if requested in pcd_files:
|
||||
selected_pcd = pcd_files.index(requested)
|
||||
cloud = load_pcd(pcd_files[selected_pcd]) if pcd_files else PointCloud()
|
||||
regions: list[Region] = []
|
||||
current: list[tuple[float, float]] = []
|
||||
saved: Path | None = None
|
||||
|
||||
pygame.init()
|
||||
pygame.font.init()
|
||||
screen = pygame.display.set_mode((args.width, args.height), pygame.RESIZABLE)
|
||||
pygame.display.set_caption("nav_tools - avoid region editor")
|
||||
title_font = get_font(18, bold=True)
|
||||
small_font = get_font(13)
|
||||
map_font = get_font(12)
|
||||
camera = Camera(args.width, args.height)
|
||||
fit_camera(camera, cloud, regions)
|
||||
clock = pygame.time.Clock()
|
||||
dragging = False
|
||||
last_mouse = (0, 0)
|
||||
buttons: list[tuple[pygame.Rect, str]] = []
|
||||
running = True
|
||||
|
||||
while running:
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
running = False
|
||||
elif event.type == pygame.VIDEORESIZE:
|
||||
camera.width, camera.height = event.w, event.h
|
||||
screen = pygame.display.set_mode((event.w, event.h), pygame.RESIZABLE)
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
running = False
|
||||
elif event.key in (pygame.K_RETURN, pygame.K_KP_ENTER):
|
||||
if len(current) >= 3:
|
||||
regions.append(Region(f"avoid_{len(regions) + 1}", current[:]))
|
||||
current.clear()
|
||||
elif event.key == pygame.K_BACKSPACE and current:
|
||||
current.pop()
|
||||
elif event.key == pygame.K_DELETE and regions:
|
||||
regions.pop()
|
||||
elif event.key == pygame.K_f:
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif event.key == pygame.K_s:
|
||||
saved = save_regions(regions, cloud)
|
||||
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||||
clicked = None
|
||||
for rect, action in buttons:
|
||||
if rect.collidepoint(event.pos):
|
||||
clicked = action
|
||||
break
|
||||
if event.button == 1 and clicked:
|
||||
if clicked == "prev_pcd" and pcd_files:
|
||||
selected_pcd = (selected_pcd - 1) % len(pcd_files)
|
||||
cloud = load_pcd(pcd_files[selected_pcd])
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif clicked == "next_pcd" and pcd_files:
|
||||
selected_pcd = (selected_pcd + 1) % len(pcd_files)
|
||||
cloud = load_pcd(pcd_files[selected_pcd])
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif clicked == "fit":
|
||||
fit_camera(camera, cloud, regions)
|
||||
elif clicked == "save":
|
||||
saved = save_regions(regions, cloud)
|
||||
elif event.button == 1 and event.pos[0] < camera.map_width:
|
||||
current.append(camera.screen_to_world(*event.pos))
|
||||
elif event.button in (2, 3) or (event.button == 1 and event.pos[0] >= camera.map_width):
|
||||
dragging = True
|
||||
last_mouse = event.pos
|
||||
elif event.button == 4 and event.pos[0] < camera.map_width:
|
||||
camera.zoom_at(1.12, event.pos)
|
||||
elif event.button == 5 and event.pos[0] < camera.map_width:
|
||||
camera.zoom_at(1 / 1.12, event.pos)
|
||||
elif event.type == pygame.MOUSEBUTTONUP:
|
||||
dragging = False
|
||||
elif event.type == pygame.MOUSEMOTION and dragging:
|
||||
dx = event.pos[0] - last_mouse[0]
|
||||
dy = event.pos[1] - last_mouse[1]
|
||||
camera.pan_x += dx
|
||||
camera.pan_y += dy
|
||||
last_mouse = event.pos
|
||||
|
||||
screen.fill(COLOR_BG)
|
||||
draw_grid(screen, camera, map_font)
|
||||
draw_pcd(screen, camera, cloud)
|
||||
draw_regions(screen, camera, regions, current)
|
||||
buttons = draw_panel(screen, camera, title_font, small_font, pcd_files, selected_pcd, cloud, regions, current, saved)
|
||||
pygame.display.flip()
|
||||
clock.tick(60)
|
||||
|
||||
pygame.quit()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Deterministically downsample an ASCII PCD file to a byte-size limit."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
DEFAULT_MAX_BYTES = 9_900_000
|
||||
|
||||
|
||||
def read_header(path: Path) -> tuple[list[bytes], int, int]:
|
||||
header: list[bytes] = []
|
||||
declared_points: int | None = None
|
||||
|
||||
with path.open("rb") as stream:
|
||||
while True:
|
||||
line = stream.readline()
|
||||
if not line:
|
||||
raise ValueError(f"PCD header has no DATA line: {path}")
|
||||
header.append(line)
|
||||
fields = line.strip().split(maxsplit=1)
|
||||
if fields and fields[0].upper() == b"POINTS" and len(fields) == 2:
|
||||
declared_points = int(fields[1])
|
||||
if fields and fields[0].upper() == b"DATA":
|
||||
if len(fields) != 2 or fields[1].lower() != b"ascii":
|
||||
raise ValueError("Only DATA ascii PCD files are supported")
|
||||
data_offset = stream.tell()
|
||||
break
|
||||
|
||||
if declared_points is None:
|
||||
raise ValueError(f"PCD header has no POINTS field: {path}")
|
||||
return header, data_offset, declared_points
|
||||
|
||||
|
||||
def render_header(header: list[bytes], point_count: int) -> bytes:
|
||||
rendered: list[bytes] = []
|
||||
replaced_width = False
|
||||
replaced_points = False
|
||||
|
||||
for line in header:
|
||||
newline = b"\r\n" if line.endswith(b"\r\n") else b"\n"
|
||||
fields = line.strip().split(maxsplit=1)
|
||||
key = fields[0].upper() if fields else b""
|
||||
if key == b"WIDTH":
|
||||
rendered.append(f"WIDTH {point_count}".encode("ascii") + newline)
|
||||
replaced_width = True
|
||||
elif key == b"POINTS":
|
||||
rendered.append(f"POINTS {point_count}".encode("ascii") + newline)
|
||||
replaced_points = True
|
||||
elif key == b"HEIGHT":
|
||||
rendered.append(b"HEIGHT 1" + newline)
|
||||
else:
|
||||
rendered.append(line)
|
||||
|
||||
if not replaced_width or not replaced_points:
|
||||
raise ValueError("PCD header must contain WIDTH and POINTS fields")
|
||||
return b"".join(rendered)
|
||||
|
||||
|
||||
def measure_sample(
|
||||
path: Path,
|
||||
data_offset: int,
|
||||
stride: int,
|
||||
) -> tuple[int, int, int]:
|
||||
offset = stride // 2
|
||||
source_count = 0
|
||||
selected_count = 0
|
||||
selected_bytes = 0
|
||||
|
||||
with path.open("rb") as stream:
|
||||
stream.seek(data_offset)
|
||||
for line in stream:
|
||||
if not line.strip():
|
||||
continue
|
||||
if source_count % stride == offset:
|
||||
selected_count += 1
|
||||
selected_bytes += len(line)
|
||||
source_count += 1
|
||||
|
||||
return source_count, selected_count, selected_bytes
|
||||
|
||||
|
||||
def write_sample(
|
||||
source: Path,
|
||||
output: Path,
|
||||
header: bytes,
|
||||
data_offset: int,
|
||||
stride: int,
|
||||
) -> None:
|
||||
offset = stride // 2
|
||||
point_index = 0
|
||||
temporary = output.with_suffix(output.suffix + ".tmp")
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
try:
|
||||
with source.open("rb") as src, temporary.open("wb") as dst:
|
||||
dst.write(header)
|
||||
src.seek(data_offset)
|
||||
for line in src:
|
||||
if not line.strip():
|
||||
continue
|
||||
if point_index % stride == offset:
|
||||
dst.write(line)
|
||||
point_index += 1
|
||||
os.replace(temporary, output)
|
||||
finally:
|
||||
if temporary.exists():
|
||||
temporary.unlink()
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("source", type=Path, help="Source DATA ascii PCD file")
|
||||
parser.add_argument("output", type=Path, help="Downsampled output PCD file")
|
||||
parser.add_argument(
|
||||
"--max-bytes",
|
||||
type=int,
|
||||
default=DEFAULT_MAX_BYTES,
|
||||
help=f"Maximum output size in bytes (default: {DEFAULT_MAX_BYTES})",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
source = args.source.expanduser().resolve()
|
||||
output = args.output.expanduser().resolve()
|
||||
max_bytes = int(args.max_bytes)
|
||||
|
||||
if source == output:
|
||||
raise ValueError("Source and output paths must be different")
|
||||
if max_bytes <= 8192:
|
||||
raise ValueError("--max-bytes must be greater than 8192")
|
||||
|
||||
header, data_offset, declared_points = read_header(source)
|
||||
data_bytes = source.stat().st_size - data_offset
|
||||
payload_budget = max_bytes - 8192
|
||||
stride = max(1, math.ceil(data_bytes / payload_budget))
|
||||
|
||||
while True:
|
||||
source_count, selected_count, selected_bytes = measure_sample(
|
||||
source,
|
||||
data_offset,
|
||||
stride,
|
||||
)
|
||||
if source_count != declared_points:
|
||||
raise ValueError(
|
||||
f"POINTS declares {declared_points}, but {source_count} data rows were read"
|
||||
)
|
||||
output_header = render_header(header, selected_count)
|
||||
if len(output_header) + selected_bytes <= max_bytes:
|
||||
break
|
||||
stride += 1
|
||||
|
||||
write_sample(source, output, output_header, data_offset, stride)
|
||||
output_bytes = output.stat().st_size
|
||||
if output_bytes > max_bytes:
|
||||
raise RuntimeError(f"Generated file exceeds limit: {output_bytes} > {max_bytes}")
|
||||
|
||||
print(f"source={source}")
|
||||
print(f"output={output}")
|
||||
print(f"source_points={source_count}")
|
||||
print(f"output_points={selected_count}")
|
||||
print(f"stride={stride}")
|
||||
print(f"output_bytes={output_bytes}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,215 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Mirror a nav_tools terrain XML and route JSON together."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
XML_DIR = TOOL_DIR / "xml"
|
||||
POINTS_DIR = TOOL_DIR / "points"
|
||||
DEFAULT_XML = XML_DIR / "1hao.xml"
|
||||
DEFAULT_POINTS = POINTS_DIR / "points_20260715_120154.json"
|
||||
|
||||
|
||||
def format_float(value: float) -> str:
|
||||
if abs(value) < 0.0000005:
|
||||
value = 0.0
|
||||
return f"{value:.6f}".rstrip("0").rstrip(".")
|
||||
|
||||
|
||||
def parse_float_tuple(text: str | None) -> tuple[float, ...]:
|
||||
if not text:
|
||||
return ()
|
||||
return tuple(float(item) for item in text.split())
|
||||
|
||||
|
||||
def mirror_coord(value: float, axis_value: float) -> float:
|
||||
return 2.0 * axis_value - value
|
||||
|
||||
|
||||
def normalize_yaw_deg(value: float) -> float:
|
||||
while value > 180.0:
|
||||
value -= 360.0
|
||||
while value <= -180.0:
|
||||
value += 360.0
|
||||
return value
|
||||
|
||||
|
||||
def mirror_yaw_deg(value: float, axis: str) -> float:
|
||||
if axis == "x":
|
||||
return normalize_yaw_deg(180.0 - value)
|
||||
return normalize_yaw_deg(-value)
|
||||
|
||||
|
||||
def mirror_quat(quat: tuple[float, ...], axis: str) -> tuple[float, float, float, float]:
|
||||
if axis == "x":
|
||||
return quat[0], quat[1], -quat[2], quat[3]
|
||||
return quat[0], -quat[1], quat[2], quat[3]
|
||||
|
||||
|
||||
def mirror_xml(source: Path, target: Path, axis: str, axis_value: float) -> None:
|
||||
parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True))
|
||||
tree = ET.parse(source, parser=parser)
|
||||
mirrored = copy.deepcopy(tree)
|
||||
root = mirrored.getroot()
|
||||
model_name = root.get("model", source.stem)
|
||||
if "mirror" not in model_name.lower():
|
||||
root.set("model", f"{model_name} mirror")
|
||||
|
||||
for element in root.iter():
|
||||
pos = parse_float_tuple(element.get("pos"))
|
||||
if len(pos) >= 1:
|
||||
mirrored_pos = list(pos)
|
||||
coord_index = 0 if axis == "x" else 1
|
||||
if len(mirrored_pos) > coord_index:
|
||||
mirrored_pos[coord_index] = mirror_coord(mirrored_pos[coord_index], axis_value)
|
||||
element.set("pos", " ".join(format_float(value) for value in mirrored_pos))
|
||||
|
||||
center = parse_float_tuple(element.get("center"))
|
||||
if len(center) >= 1:
|
||||
mirrored_center = list(center)
|
||||
coord_index = 0 if axis == "x" else 1
|
||||
if len(mirrored_center) > coord_index:
|
||||
mirrored_center[coord_index] = mirror_coord(mirrored_center[coord_index], axis_value)
|
||||
element.set("center", " ".join(format_float(value) for value in mirrored_center))
|
||||
|
||||
quat = parse_float_tuple(element.get("quat"))
|
||||
if len(quat) == 4:
|
||||
mirrored_quat = mirror_quat(quat, axis)
|
||||
element.set("quat", " ".join(format_float(value) for value in mirrored_quat))
|
||||
|
||||
ET.indent(mirrored, space=" ")
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
mirrored.write(target, encoding="utf-8", xml_declaration=False)
|
||||
|
||||
|
||||
def mirror_point_fields(row: dict[str, Any], axis: str, axis_value: float) -> None:
|
||||
coord_keys = ("x", "world_x") if axis == "x" else ("y", "world_y")
|
||||
for key in coord_keys:
|
||||
if key in row and isinstance(row[key], (int, float)):
|
||||
row[key] = mirror_coord(float(row[key]), axis_value)
|
||||
if "yawDeg" in row and isinstance(row["yawDeg"], (int, float)):
|
||||
row["yawDeg"] = mirror_yaw_deg(float(row["yawDeg"]), axis)
|
||||
if "yaw" in row and isinstance(row["yaw"], (int, float)):
|
||||
row["yaw"] = mirror_yaw_deg(float(row["yaw"]), axis)
|
||||
|
||||
|
||||
def mirror_polygon(points: Any, axis: str, axis_value: float) -> None:
|
||||
if not isinstance(points, list):
|
||||
return
|
||||
for point in points:
|
||||
if isinstance(point, dict):
|
||||
key = "x" if axis == "x" else "y"
|
||||
if isinstance(point.get(key), (int, float)):
|
||||
point[key] = mirror_coord(float(point[key]), axis_value)
|
||||
elif isinstance(point, list):
|
||||
coord_index = 0 if axis == "x" else 1
|
||||
if len(point) > coord_index and isinstance(point[coord_index], (int, float)):
|
||||
point[coord_index] = mirror_coord(float(point[coord_index]), axis_value)
|
||||
|
||||
|
||||
def mirror_regions(payload: dict[str, Any], axis: str, axis_value: float) -> None:
|
||||
for key in ("regions", "avoid_regions"):
|
||||
rows = payload.get(key)
|
||||
if not isinstance(rows, list):
|
||||
continue
|
||||
for region in rows:
|
||||
if isinstance(region, dict):
|
||||
mirror_polygon(region.get("polygon", region.get("points")), axis, axis_value)
|
||||
|
||||
|
||||
def mirror_waypoint_rows(rows: Any, axis: str, axis_value: float) -> None:
|
||||
if not isinstance(rows, list):
|
||||
return
|
||||
for row in rows:
|
||||
if isinstance(row, dict):
|
||||
mirror_point_fields(row, axis, axis_value)
|
||||
|
||||
|
||||
def mirror_points_json(source: Path, target: Path, axis: str, axis_value: float, mirrored_xml: Path | None) -> None:
|
||||
payload = json.loads(source.read_text(encoding="utf-8"))
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"Expected top-level JSON object: {source}")
|
||||
|
||||
payload["name"] = f"{payload.get('name', source.stem)}_mirror"
|
||||
payload["mirroredFrom"] = source.name
|
||||
payload["mirror"] = {
|
||||
"axis": axis,
|
||||
f"axis_{axis}": axis_value,
|
||||
"createdAt": time.strftime("%Y-%m-%dT%H:%M:%S"),
|
||||
}
|
||||
if mirrored_xml is not None:
|
||||
payload["xml"] = f"xml/{mirrored_xml.name}"
|
||||
|
||||
origin = payload.get("origin")
|
||||
if isinstance(origin, dict):
|
||||
coord_key = "x" if axis == "x" else "y"
|
||||
if isinstance(origin.get(coord_key), (int, float)):
|
||||
origin[coord_key] = mirror_coord(float(origin[coord_key]), axis_value)
|
||||
if isinstance(origin.get("yaw_deg"), (int, float)):
|
||||
origin["yaw_deg"] = mirror_yaw_deg(float(origin["yaw_deg"]), axis)
|
||||
origin["yaw_rad"] = math.radians(float(origin["yaw_deg"]))
|
||||
elif isinstance(origin.get("yaw_rad"), (int, float)):
|
||||
origin["yaw_rad"] = math.radians(mirror_yaw_deg(math.degrees(float(origin["yaw_rad"])), axis))
|
||||
origin["yaw_deg"] = math.degrees(float(origin["yaw_rad"]))
|
||||
|
||||
mirror_waypoint_rows(payload.get("waypoints"), axis, axis_value)
|
||||
segments = payload.get("segments")
|
||||
if isinstance(segments, list):
|
||||
for segment in segments:
|
||||
if isinstance(segment, dict):
|
||||
mirror_waypoint_rows(segment.get("waypoints"), axis, axis_value)
|
||||
|
||||
mirror_regions(payload, axis, axis_value)
|
||||
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
target.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
|
||||
def default_output_path(source: Path, suffix: str) -> Path:
|
||||
return source.with_name(f"{source.stem}{suffix}{source.suffix}")
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Mirror a nav_tools XML and matching points JSON.")
|
||||
parser.add_argument("--xml", type=Path, default=DEFAULT_XML, help="Input XML path.")
|
||||
parser.add_argument("--points", type=Path, default=DEFAULT_POINTS, help="Input points JSON path.")
|
||||
parser.add_argument("--mirror-axis", choices=("x", "y"), default="x", help="Coordinate axis to mirror.")
|
||||
parser.add_argument("--axis-x", type=float, default=0.0, help="Mirror axis x value. Default: 0.")
|
||||
parser.add_argument("--axis-y", type=float, default=0.0, help="Mirror axis y value.")
|
||||
parser.add_argument("--out-xml", type=Path, help="Output XML path. Default: <input>_mirror.xml.")
|
||||
parser.add_argument("--out-points", type=Path, help="Output JSON path. Default: <input>_mirror.json.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
xml_path = args.xml if args.xml.is_absolute() else TOOL_DIR / args.xml
|
||||
points_path = args.points if args.points.is_absolute() else TOOL_DIR / args.points
|
||||
out_xml = args.out_xml if args.out_xml else default_output_path(xml_path, "_mirror")
|
||||
out_points = args.out_points if args.out_points else default_output_path(points_path, "_mirror")
|
||||
if not out_xml.is_absolute():
|
||||
out_xml = TOOL_DIR / out_xml
|
||||
if not out_points.is_absolute():
|
||||
out_points = TOOL_DIR / out_points
|
||||
|
||||
axis_value = args.axis_x if args.mirror_axis == "x" else args.axis_y
|
||||
mirror_xml(xml_path, out_xml, args.mirror_axis, axis_value)
|
||||
mirror_points_json(points_path, out_points, args.mirror_axis, axis_value, out_xml)
|
||||
print(f"XML: {out_xml}")
|
||||
print(f"JSON: {out_points}")
|
||||
print(f"axis_{args.mirror_axis}: {axis_value}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 311 KiB |
@@ -0,0 +1 @@
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,108 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Transform ASCII PCD maps by XY translation and yaw rotation.
|
||||
|
||||
Examples:
|
||||
python pcd_transform_tool.py pcd/1hao.pcd --dx -9.34 --dy -0.88 --yaw-deg -90 --output pcd/1hao_tf.pcd
|
||||
python pcd_transform_tool.py pcd/1hao.pcd --yaw-deg 90 --in-place
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import shutil
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def tool_relative(path: Path) -> Path:
|
||||
if path.is_absolute():
|
||||
raise ValueError("Use a path relative to this nav_tools folder.")
|
||||
return TOOL_DIR / path
|
||||
|
||||
|
||||
def format_float(value: float) -> str:
|
||||
if abs(value) < 5e-10:
|
||||
value = 0.0
|
||||
return f"{value:.8f}".rstrip("0").rstrip(".")
|
||||
|
||||
|
||||
def transform_xy(x: float, y: float, dx: float, dy: float, yaw_rad: float) -> tuple[float, float]:
|
||||
tx = x + dx
|
||||
ty = y + dy
|
||||
c = math.cos(yaw_rad)
|
||||
s = math.sin(yaw_rad)
|
||||
return tx * c - ty * s, tx * s + ty * c
|
||||
|
||||
|
||||
def transform_ascii_pcd(source: Path, target: Path, dx: float, dy: float, yaw_deg: float) -> int:
|
||||
yaw_rad = math.radians(yaw_deg)
|
||||
data_started = False
|
||||
transformed = 0
|
||||
with source.open("r", encoding="utf-8", errors="ignore") as fin, target.open("w", encoding="utf-8", newline="\n") as fout:
|
||||
for raw in fin:
|
||||
line = raw.strip()
|
||||
if data_started and line:
|
||||
parts = line.split()
|
||||
if len(parts) >= 2:
|
||||
x, y = transform_xy(float(parts[0]), float(parts[1]), dx, dy, yaw_rad)
|
||||
parts[0] = format_float(x)
|
||||
parts[1] = format_float(y)
|
||||
fout.write(" ".join(parts) + "\n")
|
||||
transformed += 1
|
||||
else:
|
||||
fout.write(raw)
|
||||
else:
|
||||
fout.write(raw)
|
||||
if line.upper().startswith("DATA"):
|
||||
if "ascii" not in line.lower():
|
||||
raise RuntimeError(f"Only ASCII PCD is supported: {source}")
|
||||
data_started = True
|
||||
if transformed == 0:
|
||||
raise RuntimeError(f"No points transformed: {source}")
|
||||
return transformed
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Translate and yaw-rotate an ASCII PCD file.")
|
||||
parser.add_argument("pcd", type=Path, help="Input PCD path relative to this nav_tools folder.")
|
||||
parser.add_argument("--dx", type=float, default=0.0, help="X translation before rotation, meters.")
|
||||
parser.add_argument("--dy", type=float, default=0.0, help="Y translation before rotation, meters.")
|
||||
parser.add_argument("--yaw-deg", type=float, default=0.0, help="Yaw rotation after translation, degrees.")
|
||||
parser.add_argument("--output", type=Path, help="Output PCD path relative to this nav_tools folder.")
|
||||
parser.add_argument("--in-place", action="store_true", help="Overwrite input PCD after creating a .bak copy.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
source = tool_relative(args.pcd)
|
||||
if not source.exists():
|
||||
raise FileNotFoundError(source)
|
||||
|
||||
if args.in_place:
|
||||
target = source.with_suffix(source.suffix + ".tmp")
|
||||
elif args.output:
|
||||
target = tool_relative(args.output)
|
||||
else:
|
||||
target = source.with_name(f"{source.stem}_tf{source.suffix}")
|
||||
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
count = transform_ascii_pcd(source, target, args.dx, args.dy, args.yaw_deg)
|
||||
|
||||
if args.in_place:
|
||||
backup = source.with_suffix(source.suffix + f".bak_{time.strftime('%Y%m%d_%H%M%S')}")
|
||||
shutil.copy2(source, backup)
|
||||
target.replace(source)
|
||||
print(f"Transformed {count} points in-place: {source}")
|
||||
print(f"Backup: {backup}")
|
||||
else:
|
||||
print(f"Transformed {count} points: {target}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,24 @@
|
||||
# 航点路线演进
|
||||
|
||||
本目录按原始文件名保存导航打点阶段的路线快照。文件名中的 `new`、`real`、`reall` 等是开发期原名，仅表示当时的迭代语境，不统一解释为正式版本或最终验证状态。
|
||||
|
||||
| 源文件时间 | 文件 | 航点 | 避障区 | 绑定 XML |
|
||||
| --- | --- | ---: | ---: | --- |
|
||||
| 2026-07-10 23:15 | `1B_FF.json` | 48 | 26 | `B_C.xml` |
|
||||
| 2026-07-11 01:35 | `A_FFF.json` | 46 | 26 | `A_C.xml` |
|
||||
| 2026-07-12 23:55 | `1hao_wallspeed08_桥A适用于重定位.json` | 49 | 19 | `1hao.xml` |
|
||||
| 2026-07-13 00:10 | `1hao_not_raogan.json` | 32 | 19 | `1hao.xml` |
|
||||
| 2026-07-13 20:36 | `1hao_n.json` | 48 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 09:57 | `1hao_new.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 11:20 | `1hao_bridge_a_crawl.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 12:01 | `1hao_crawl2.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-14 12:16 | `1hao_new1.json` | 47 | 19 | `A_C.xml` |
|
||||
| 2026-07-14 12:19 | `1hao_jianyi.json` | 47 | 19 | `1hao.xml` |
|
||||
| 2026-07-15 11:42 | `1hao_real.json` | 30 | 19 | `1hao.xml` |
|
||||
| 2026-07-15 11:43 | `2hao_not_raogan.json` | 29 | 21 | `2hao.xml` |
|
||||
| 2026-07-15 12:01 | `1hao_reall.json` | 29 | 19 | `1hao.xml` |
|
||||
| 2026-07-15 12:01 | `points_20260715_120154.json` | 29 | 19 | `1hao.xml` |
|
||||
|
||||
这些文件各有参数和路线差异，全部保留以呈现打点演进。默认工具入口使用时间戳文件 `points_20260715_120154.json`；它不是对其他文件的替代，其他路线仍可通过 `--points` 或界面下拉框载入。
|
||||
|
||||
默认路线在当前保守圆形包络检查下存在 3 组净空不足记录，因此这里称为“默认归档路线”，不额外标记为几何安全最终版。
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
pygame>=2.6.1
|
||||
Pillow>=10.0.0
|
||||
@@ -0,0 +1,558 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate safer waypoint candidates for nav_tools route JSON files.
|
||||
|
||||
The optimizer keeps the original JSON intact, adjusts copied waypoints away
|
||||
from avoid polygons, recomputes yaw, and writes candidate JSON files for
|
||||
offline/sim2sim validation.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from route_safety_check import (
|
||||
AvoidRegion,
|
||||
Waypoint,
|
||||
analyze_route,
|
||||
default_lateral_footprint_radius,
|
||||
load_regions,
|
||||
load_waypoints,
|
||||
point_in_polygon,
|
||||
point_segment_distance,
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class MutablePoint:
|
||||
id: str
|
||||
x: float
|
||||
y: float
|
||||
yaw_deg: float
|
||||
speed: float | None
|
||||
policy: str
|
||||
tolerance: float | None
|
||||
row_refs: list[dict[str, Any]]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Generate safer route candidates.")
|
||||
parser.add_argument("--points", type=Path, default=Path("tools/nav_tools/points/points_20260715_120154.json"))
|
||||
parser.add_argument("--out-dir", type=Path, default=Path("tools/nav_tools/points/auto_candidates"))
|
||||
parser.add_argument("--avoid-margin", type=float, default=0.05)
|
||||
parser.add_argument("--extra-margin", type=float, default=0.08)
|
||||
parser.add_argument("--footprint-radius", type=float, default=None)
|
||||
parser.add_argument("--iterations", type=int, default=240)
|
||||
parser.add_argument("--step-scale", type=float, default=0.45)
|
||||
parser.add_argument("--max-move-per-iter", type=float, default=0.08)
|
||||
parser.add_argument("--max-total-move", type=float, default=0.55)
|
||||
parser.add_argument("--smooth-weight", type=float, default=0.12)
|
||||
parser.add_argument("--lock-ends", action="store_true", default=True)
|
||||
parser.add_argument("--no-lock-ends", dest="lock_ends", action="store_false")
|
||||
parser.add_argument("--slow-near-risk", action="store_true", default=True)
|
||||
parser.add_argument("--no-slow-near-risk", dest="slow_near_risk", action="store_false")
|
||||
parser.add_argument("--min-speed", type=float, default=0.22)
|
||||
parser.add_argument("--max-segment-length", type=float, default=0.65)
|
||||
parser.add_argument(
|
||||
"--focus-start-index",
|
||||
type=int,
|
||||
default=None,
|
||||
help="1-based first waypoint index allowed to move; points outside the focus range are locked.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--focus-end-index",
|
||||
type=int,
|
||||
default=None,
|
||||
help="1-based last waypoint index allowed to move; points outside the focus range are locked.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--policy-range",
|
||||
action="append",
|
||||
default=[],
|
||||
metavar="START:END:POLICY",
|
||||
help="Override waypoint policy by 1-based inclusive index range, e.g. 21:26:crawl. Can be repeated.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--speed-range",
|
||||
action="append",
|
||||
default=[],
|
||||
metavar="START:END:SPEED",
|
||||
help="Override waypoint speed by 1-based inclusive index range, e.g. 21:26:0.22. Can be repeated.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--shift-range",
|
||||
action="append",
|
||||
default=[],
|
||||
metavar="START:END:DX:DY",
|
||||
help="Shift waypoint positions by 1-based inclusive index range. Applied after optimization.",
|
||||
)
|
||||
parser.add_argument("--top", type=int, default=12)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def load_payload(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
payload = json.load(f)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"JSON root must be object: {path}")
|
||||
return payload
|
||||
|
||||
|
||||
def iter_waypoint_rows(payload: dict[str, Any]) -> list[dict[str, Any]]:
|
||||
rows: list[dict[str, Any]] = []
|
||||
if isinstance(payload.get("waypoints"), list):
|
||||
rows.extend(row for row in payload["waypoints"] if isinstance(row, dict))
|
||||
if isinstance(payload.get("segments"), list):
|
||||
for segment in payload["segments"]:
|
||||
if isinstance(segment, dict) and isinstance(segment.get("waypoints"), list):
|
||||
rows.extend(row for row in segment["waypoints"] if isinstance(row, dict))
|
||||
return rows
|
||||
|
||||
|
||||
def load_mutable_points(payload: dict[str, Any]) -> list[MutablePoint]:
|
||||
unique: list[MutablePoint] = []
|
||||
refs_by_key: dict[tuple[str, float, float], MutablePoint] = {}
|
||||
for index, row in enumerate(iter_waypoint_rows(payload), start=1):
|
||||
x = float(row.get("world_x", row.get("x", 0.0)))
|
||||
y = float(row.get("world_y", row.get("y", 0.0)))
|
||||
key = (str(row.get("id", index)), round(x, 9), round(y, 9))
|
||||
point = refs_by_key.get(key)
|
||||
if point is None:
|
||||
point = MutablePoint(
|
||||
id=str(row.get("id", index)),
|
||||
x=x,
|
||||
y=y,
|
||||
yaw_deg=float(row.get("yawDeg", row.get("yaw_deg", row.get("yaw", 0.0)))),
|
||||
speed=float(row["speed"]) if row.get("speed") is not None else None,
|
||||
policy=str(row.get("policy", "rough")),
|
||||
tolerance=float(row["tolerance"]) if row.get("tolerance") is not None else None,
|
||||
row_refs=[],
|
||||
)
|
||||
refs_by_key[key] = point
|
||||
unique.append(point)
|
||||
point.row_refs.append(row)
|
||||
if len(unique) < 2:
|
||||
raise ValueError("Route must contain at least two unique waypoints")
|
||||
return unique
|
||||
|
||||
|
||||
def mutable_to_waypoints(points: list[MutablePoint]) -> list[Waypoint]:
|
||||
return [
|
||||
Waypoint(
|
||||
index=i,
|
||||
id=p.id,
|
||||
x=p.x,
|
||||
y=p.y,
|
||||
yaw_deg=p.yaw_deg,
|
||||
speed=p.speed,
|
||||
policy=p.policy,
|
||||
tolerance=p.tolerance,
|
||||
)
|
||||
for i, p in enumerate(points, start=1)
|
||||
]
|
||||
|
||||
|
||||
def closest_point_on_segment(px: float, py: float, ax: float, ay: float, bx: float, by: float) -> tuple[float, float]:
|
||||
dx = bx - ax
|
||||
dy = by - ay
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq <= 1.0e-12:
|
||||
return ax, ay
|
||||
t = ((px - ax) * dx + (py - ay) * dy) / length_sq
|
||||
t = max(0.0, min(1.0, t))
|
||||
return ax + t * dx, ay + t * dy
|
||||
|
||||
|
||||
def closest_segment_polygon_pair(
|
||||
ax: float,
|
||||
ay: float,
|
||||
bx: float,
|
||||
by: float,
|
||||
polygon: tuple[tuple[float, float], ...],
|
||||
) -> tuple[float, float, float, float, float]:
|
||||
best = (float("inf"), ax, ay, polygon[0][0], polygon[0][1])
|
||||
samples = 17
|
||||
for i in range(samples):
|
||||
t = i / (samples - 1)
|
||||
sx = ax + (bx - ax) * t
|
||||
sy = ay + (by - ay) * t
|
||||
for px, py in polygon:
|
||||
dist = math.hypot(sx - px, sy - py)
|
||||
if dist < best[0]:
|
||||
best = (dist, sx, sy, px, py)
|
||||
for (ex0, ey0), (ex1, ey1) in zip(polygon, polygon[1:] + polygon[:1]):
|
||||
qx, qy = closest_point_on_segment(sx, sy, ex0, ey0, ex1, ey1)
|
||||
dist = math.hypot(sx - qx, sy - qy)
|
||||
if dist < best[0]:
|
||||
best = (dist, sx, sy, qx, qy)
|
||||
for sx, sy in ((ax, ay), (bx, by)):
|
||||
for (ex0, ey0), (ex1, ey1) in zip(polygon, polygon[1:] + polygon[:1]):
|
||||
qx, qy = closest_point_on_segment(sx, sy, ex0, ey0, ex1, ey1)
|
||||
dist = math.hypot(sx - qx, sy - qy)
|
||||
if dist < best[0]:
|
||||
best = (dist, sx, sy, qx, qy)
|
||||
return best
|
||||
|
||||
|
||||
def polygon_centroid(polygon: tuple[tuple[float, float], ...]) -> tuple[float, float]:
|
||||
return (
|
||||
sum(x for x, _ in polygon) / len(polygon),
|
||||
sum(y for _, y in polygon) / len(polygon),
|
||||
)
|
||||
|
||||
|
||||
def point_inside_any_region(x: float, y: float, regions: list[AvoidRegion]) -> bool:
|
||||
return any(point_in_polygon(x, y, region.polygon) for region in regions)
|
||||
|
||||
|
||||
def nearest_region_clearance(x: float, y: float, regions: list[AvoidRegion]) -> float:
|
||||
best = float("inf")
|
||||
for region in regions:
|
||||
if point_in_polygon(x, y, region.polygon):
|
||||
return 0.0
|
||||
polygon = region.polygon
|
||||
for (ax, ay), (bx, by) in zip(polygon, polygon[1:] + polygon[:1]):
|
||||
best = min(best, point_segment_distance(x, y, ax, ay, bx, by))
|
||||
return best
|
||||
|
||||
|
||||
def optimize_points(
|
||||
points: list[MutablePoint],
|
||||
regions: list[AvoidRegion],
|
||||
required_clearance: float,
|
||||
args: argparse.Namespace,
|
||||
) -> tuple[list[MutablePoint], dict[str, Any]]:
|
||||
original = [(p.x, p.y) for p in points]
|
||||
target_clearance = required_clearance + float(args.extra_margin)
|
||||
locked = {0, len(points) - 1} if args.lock_ends else set()
|
||||
if args.focus_start_index is not None or args.focus_end_index is not None:
|
||||
focus_start = max(1, int(args.focus_start_index or 1)) - 1
|
||||
focus_end = min(len(points), int(args.focus_end_index or len(points))) - 1
|
||||
if focus_start > focus_end:
|
||||
raise ValueError(
|
||||
f"Invalid focus range: start={args.focus_start_index} end={args.focus_end_index}"
|
||||
)
|
||||
locked.update(i for i in range(len(points)) if i < focus_start or i > focus_end)
|
||||
risk_hit_counts = [0 for _ in points]
|
||||
|
||||
for _ in range(int(args.iterations)):
|
||||
deltas = [[0.0, 0.0] for _ in points]
|
||||
weights = [0.0 for _ in points]
|
||||
waypoints = mutable_to_waypoints(points)
|
||||
risks = analyze_route(waypoints, regions, target_clearance)
|
||||
active_risks = [risk for risk in risks if risk.margin_m < 0.0]
|
||||
if not active_risks:
|
||||
break
|
||||
|
||||
id_to_index = {p.id: i for i, p in enumerate(points)}
|
||||
for risk in active_risks:
|
||||
ia = id_to_index.get(risk.start_id)
|
||||
ib = id_to_index.get(risk.end_id)
|
||||
region = next((item for item in regions if item.name == risk.region), None)
|
||||
if ia is None or ib is None or region is None:
|
||||
continue
|
||||
ax, ay = points[ia].x, points[ia].y
|
||||
bx, by = points[ib].x, points[ib].y
|
||||
dist, sx, sy, qx, qy = closest_segment_polygon_pair(ax, ay, bx, by, region.polygon)
|
||||
vx = sx - qx
|
||||
vy = sy - qy
|
||||
norm = math.hypot(vx, vy)
|
||||
if norm < 1.0e-6 or risk.centerline_intersects:
|
||||
cx, cy = polygon_centroid(region.polygon)
|
||||
mx = 0.5 * (ax + bx)
|
||||
my = 0.5 * (ay + by)
|
||||
vx = mx - cx
|
||||
vy = my - cy
|
||||
norm = math.hypot(vx, vy)
|
||||
if norm < 1.0e-6:
|
||||
vx, vy, norm = 1.0, 0.0, 1.0
|
||||
ux = vx / norm
|
||||
uy = vy / norm
|
||||
push = min(float(args.max_move_per_iter), max(0.0, target_clearance - dist) * float(args.step_scale))
|
||||
for idx in (ia, ib):
|
||||
if idx in locked:
|
||||
continue
|
||||
deltas[idx][0] += ux * push
|
||||
deltas[idx][1] += uy * push
|
||||
weights[idx] += 1.0
|
||||
risk_hit_counts[idx] += 1
|
||||
|
||||
if args.smooth_weight > 0.0 and len(points) > 2:
|
||||
for i in range(1, len(points) - 1):
|
||||
if i in locked:
|
||||
continue
|
||||
avg_x = 0.5 * (points[i - 1].x + points[i + 1].x)
|
||||
avg_y = 0.5 * (points[i - 1].y + points[i + 1].y)
|
||||
deltas[i][0] += (avg_x - points[i].x) * float(args.smooth_weight)
|
||||
deltas[i][1] += (avg_y - points[i].y) * float(args.smooth_weight)
|
||||
weights[i] += 1.0
|
||||
|
||||
for i, point in enumerate(points):
|
||||
if i in locked or weights[i] <= 0.0:
|
||||
continue
|
||||
dx = deltas[i][0] / weights[i]
|
||||
dy = deltas[i][1] / weights[i]
|
||||
ox, oy = original[i]
|
||||
next_x = point.x + dx
|
||||
next_y = point.y + dy
|
||||
total_dx = next_x - ox
|
||||
total_dy = next_y - oy
|
||||
total = math.hypot(total_dx, total_dy)
|
||||
if total > float(args.max_total_move):
|
||||
scale = float(args.max_total_move) / total
|
||||
next_x = ox + total_dx * scale
|
||||
next_y = oy + total_dy * scale
|
||||
point.x = next_x
|
||||
point.y = next_y
|
||||
|
||||
for i, point in enumerate(points[:-1]):
|
||||
nxt = points[i + 1]
|
||||
point.yaw_deg = math.degrees(math.atan2(nxt.y - point.y, nxt.x - point.x))
|
||||
points[-1].yaw_deg = points[-2].yaw_deg
|
||||
|
||||
if args.slow_near_risk:
|
||||
for i, point in enumerate(points):
|
||||
clearance = nearest_region_clearance(point.x, point.y, regions)
|
||||
if clearance < target_clearance + 0.12:
|
||||
current = point.speed if point.speed is not None else 0.35
|
||||
point.speed = max(float(args.min_speed), min(current, 0.28))
|
||||
if risk_hit_counts[i] > 0:
|
||||
current = point.speed if point.speed is not None else 0.35
|
||||
point.speed = max(float(args.min_speed), min(current, 0.25))
|
||||
|
||||
final_risks = analyze_route(mutable_to_waypoints(points), regions, required_clearance)
|
||||
violations = [risk for risk in final_risks if risk.margin_m < 0.0 or risk.centerline_intersects]
|
||||
moves = [
|
||||
{
|
||||
"id": point.id,
|
||||
"dx": round(point.x - ox, 4),
|
||||
"dy": round(point.y - oy, 4),
|
||||
"dist": round(math.hypot(point.x - ox, point.y - oy), 4),
|
||||
"risk_hits": risk_hit_counts[i],
|
||||
}
|
||||
for i, (point, (ox, oy)) in enumerate(zip(points, original))
|
||||
if math.hypot(point.x - ox, point.y - oy) > 1.0e-4 or risk_hit_counts[i] > 0
|
||||
]
|
||||
return points, {
|
||||
"violations": len(violations),
|
||||
"min_margin": round(final_risks[0].margin_m, 6) if final_risks else None,
|
||||
"focus_range": [
|
||||
args.focus_start_index,
|
||||
args.focus_end_index,
|
||||
],
|
||||
"moves": moves,
|
||||
"top_risks": [
|
||||
{
|
||||
"start_id": risk.start_id,
|
||||
"end_id": risk.end_id,
|
||||
"region": risk.region,
|
||||
"margin_m": round(risk.margin_m, 6),
|
||||
"clearance_m": round(risk.clearance_m, 6),
|
||||
"centerline_intersects": risk.centerline_intersects,
|
||||
}
|
||||
for risk in final_risks[: int(args.top)]
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def apply_policy_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
|
||||
applied: list[dict[str, Any]] = []
|
||||
for item in ranges:
|
||||
parts = [part.strip() for part in str(item).split(":")]
|
||||
if len(parts) != 3 or not parts[2]:
|
||||
raise ValueError(f"Invalid --policy-range {item!r}; expected START:END:POLICY")
|
||||
start = max(1, int(parts[0]))
|
||||
end = min(len(points), int(parts[1]))
|
||||
if start > end:
|
||||
raise ValueError(f"Invalid --policy-range {item!r}; start is after end")
|
||||
policy = parts[2]
|
||||
for index in range(start - 1, end):
|
||||
points[index].policy = policy
|
||||
applied.append({"start": start, "end": end, "policy": policy})
|
||||
return applied
|
||||
|
||||
|
||||
def apply_speed_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
|
||||
applied: list[dict[str, Any]] = []
|
||||
for item in ranges:
|
||||
parts = [part.strip() for part in str(item).split(":")]
|
||||
if len(parts) != 3:
|
||||
raise ValueError(f"Invalid --speed-range {item!r}; expected START:END:SPEED")
|
||||
start = max(1, int(parts[0]))
|
||||
end = min(len(points), int(parts[1]))
|
||||
if start > end:
|
||||
raise ValueError(f"Invalid --speed-range {item!r}; start is after end")
|
||||
speed = float(parts[2])
|
||||
for index in range(start - 1, end):
|
||||
points[index].speed = speed
|
||||
applied.append({"start": start, "end": end, "speed": speed})
|
||||
return applied
|
||||
|
||||
|
||||
def apply_shift_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
|
||||
applied: list[dict[str, Any]] = []
|
||||
for item in ranges:
|
||||
parts = [part.strip() for part in str(item).split(":")]
|
||||
if len(parts) != 4:
|
||||
raise ValueError(f"Invalid --shift-range {item!r}; expected START:END:DX:DY")
|
||||
start = max(1, int(parts[0]))
|
||||
end = min(len(points), int(parts[1]))
|
||||
if start > end:
|
||||
raise ValueError(f"Invalid --shift-range {item!r}; start is after end")
|
||||
dx = float(parts[2])
|
||||
dy = float(parts[3])
|
||||
for index in range(start - 1, end):
|
||||
points[index].x += dx
|
||||
points[index].y += dy
|
||||
applied.append({"start": start, "end": end, "dx": dx, "dy": dy})
|
||||
if applied:
|
||||
for i, point in enumerate(points[:-1]):
|
||||
nxt = points[i + 1]
|
||||
point.yaw_deg = math.degrees(math.atan2(nxt.y - point.y, nxt.x - point.x))
|
||||
points[-1].yaw_deg = points[-2].yaw_deg
|
||||
return applied
|
||||
|
||||
|
||||
def refresh_safety_summary(
|
||||
summary: dict[str, Any],
|
||||
points: list[MutablePoint],
|
||||
regions: list[AvoidRegion],
|
||||
required_clearance: float,
|
||||
args: argparse.Namespace,
|
||||
) -> None:
|
||||
final_risks = analyze_route(mutable_to_waypoints(points), regions, required_clearance)
|
||||
violations = [risk for risk in final_risks if risk.margin_m < 0.0 or risk.centerline_intersects]
|
||||
summary["violations"] = len(violations)
|
||||
summary["min_margin"] = round(final_risks[0].margin_m, 6) if final_risks else None
|
||||
summary["top_risks"] = [
|
||||
{
|
||||
"start_id": risk.start_id,
|
||||
"end_id": risk.end_id,
|
||||
"region": risk.region,
|
||||
"margin_m": round(risk.margin_m, 6),
|
||||
"clearance_m": round(risk.clearance_m, 6),
|
||||
"centerline_intersects": risk.centerline_intersects,
|
||||
}
|
||||
for risk in final_risks[: int(args.top)]
|
||||
]
|
||||
|
||||
|
||||
def apply_points_to_payload(payload: dict[str, Any], points: list[MutablePoint]) -> None:
|
||||
for point in points:
|
||||
for row in point.row_refs:
|
||||
if "world_x" in row:
|
||||
row["world_x"] = point.x
|
||||
row["x"] = point.x
|
||||
if "world_y" in row:
|
||||
row["world_y"] = point.y
|
||||
row["y"] = point.y
|
||||
row["yawDeg"] = point.yaw_deg
|
||||
if point.speed is not None:
|
||||
row["speed"] = point.speed
|
||||
if point.tolerance is not None:
|
||||
row["tolerance"] = point.tolerance
|
||||
if point.policy:
|
||||
row["policy"] = point.policy
|
||||
|
||||
|
||||
def densify_segments(payload: dict[str, Any], max_len: float) -> None:
|
||||
if max_len <= 0.0:
|
||||
return
|
||||
if not isinstance(payload.get("segments"), list):
|
||||
return
|
||||
for segment in payload["segments"]:
|
||||
rows = segment.get("waypoints") if isinstance(segment, dict) else None
|
||||
if not isinstance(rows, list) or len(rows) < 2:
|
||||
continue
|
||||
new_rows: list[dict[str, Any]] = []
|
||||
for a, b in zip(rows, rows[1:]):
|
||||
new_rows.append(a)
|
||||
ax, ay = float(a.get("x", 0.0)), float(a.get("y", 0.0))
|
||||
bx, by = float(b.get("x", 0.0)), float(b.get("y", 0.0))
|
||||
dist = math.hypot(bx - ax, by - ay)
|
||||
inserts = max(0, int(math.ceil(dist / max_len)) - 1)
|
||||
for j in range(inserts):
|
||||
t = (j + 1) / (inserts + 1)
|
||||
row = copy.deepcopy(a)
|
||||
row["id"] = f"{a.get('id')}_{j + 1}"
|
||||
row["x"] = ax + (bx - ax) * t
|
||||
row["y"] = ay + (by - ay) * t
|
||||
row["yawDeg"] = math.degrees(math.atan2(by - ay, bx - ax))
|
||||
if a.get("speed") is not None and b.get("speed") is not None:
|
||||
row["speed"] = min(float(a["speed"]), float(b["speed"]))
|
||||
new_rows.append(row)
|
||||
new_rows.append(rows[-1])
|
||||
segment["waypoints"] = new_rows
|
||||
if isinstance(payload.get("waypoints"), list):
|
||||
flat: list[dict[str, Any]] = []
|
||||
for segment in payload["segments"]:
|
||||
rows = segment.get("waypoints") if isinstance(segment, dict) else None
|
||||
if isinstance(rows, list):
|
||||
flat.extend(copy.deepcopy(row) for row in rows if isinstance(row, dict))
|
||||
if flat:
|
||||
payload["waypoints"] = flat
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
payload = load_payload(args.points)
|
||||
regions = load_regions(payload)
|
||||
points = load_mutable_points(payload)
|
||||
footprint = float(args.footprint_radius) if args.footprint_radius is not None else default_lateral_footprint_radius()
|
||||
required = footprint + float(args.avoid_margin)
|
||||
|
||||
candidate_payload = copy.deepcopy(payload)
|
||||
candidate_points = load_mutable_points(candidate_payload)
|
||||
optimized, summary = optimize_points(candidate_points, regions, required, args)
|
||||
shift_overrides = apply_shift_ranges(optimized, list(args.shift_range))
|
||||
policy_overrides = apply_policy_ranges(optimized, list(args.policy_range))
|
||||
speed_overrides = apply_speed_ranges(optimized, list(args.speed_range))
|
||||
if shift_overrides:
|
||||
summary["shift_overrides"] = shift_overrides
|
||||
if policy_overrides:
|
||||
summary["policy_overrides"] = policy_overrides
|
||||
if speed_overrides:
|
||||
summary["speed_overrides"] = speed_overrides
|
||||
if shift_overrides or policy_overrides or speed_overrides:
|
||||
refresh_safety_summary(summary, optimized, regions, required, args)
|
||||
apply_points_to_payload(candidate_payload, optimized)
|
||||
densify_segments(candidate_payload, float(args.max_segment_length))
|
||||
|
||||
stamp = time.strftime("%Y%m%d_%H%M%S")
|
||||
args.out_dir.mkdir(parents=True, exist_ok=True)
|
||||
out_path = args.out_dir / f"{args.points.stem}_auto_{stamp}.json"
|
||||
candidate_payload["name"] = f"{payload.get('name', args.points.stem)}_auto_{stamp}"
|
||||
candidate_payload["autoOptimize"] = {
|
||||
"source": str(args.points),
|
||||
"createdAt": time.strftime("%Y-%m-%dT%H:%M:%S"),
|
||||
"footprintRadius": footprint,
|
||||
"avoidMargin": args.avoid_margin,
|
||||
"extraMargin": args.extra_margin,
|
||||
"summary": summary,
|
||||
}
|
||||
out_path.write_text(json.dumps(candidate_payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
print("Route candidate optimizer")
|
||||
print(f" source: {args.points}")
|
||||
print(f" output: {out_path}")
|
||||
print(f" required_clearance: {required:.3f} m, target: {required + args.extra_margin:.3f} m")
|
||||
print(f" final_violations: {summary['violations']}")
|
||||
print(f" min_margin: {summary['min_margin']} m")
|
||||
print(f" moved_points: {len(summary['moves'])}")
|
||||
for item in summary["top_risks"][: int(args.top)]:
|
||||
print(
|
||||
" risk "
|
||||
f"{item['start_id']}->{item['end_id']} {item['region']} "
|
||||
f"margin={item['margin_m']:.3f} clearance={item['clearance_m']:.3f}"
|
||||
)
|
||||
return 0 if summary["violations"] == 0 else 2
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -90,13 +90,13 @@ def parse_args() -> argparse.Namespace:
|
||||
parser.add_argument(
|
||||
"--points",
|
||||
type=Path,
|
||||
default=Path("tools/nav_tools/points/points_20260705_174627.json"),
|
||||
default=Path("tools/nav_tools/points/points_20260715_120154.json"),
|
||||
help="Route JSON exported by nav_map_viewer.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--xml",
|
||||
type=Path,
|
||||
default=Path("tools/nav_tools/xml/A.xml"),
|
||||
default=Path("tools/nav_tools/xml/1hao.xml"),
|
||||
help="Optional MuJoCo terrain XML used for metadata checks.",
|
||||
)
|
||||
parser.add_argument(
|
||||
|
||||
@@ -0,0 +1,203 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run repeatable safety and sim2sim experiments for a route candidate."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
PROJECT_ROOT = Path(__file__).resolve().parents[2]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Run route safety + sim2sim experiment suite.")
|
||||
parser.add_argument("--points", type=Path, required=True)
|
||||
parser.add_argument("--terrain-xml", type=Path, default=PROJECT_ROOT / "tools/nav_tools/xml/1hao.xml")
|
||||
parser.add_argument("--onnx", type=Path, default=PROJECT_ROOT / "model_6800.onnx")
|
||||
parser.add_argument("--out-dir", type=Path, default=PROJECT_ROOT / "sim2sim/route_experiments")
|
||||
parser.add_argument("--start-yaw-offset-deg", type=float, default=-180.0)
|
||||
parser.add_argument("--heading-offset-deg", type=float, default=180.0)
|
||||
parser.add_argument("--start-z", type=float, default=0.75)
|
||||
parser.add_argument("--settle-steps", type=int, default=500)
|
||||
parser.add_argument("--follower", choices=("waypoint", "pure-pursuit"), default="pure-pursuit")
|
||||
parser.add_argument("--lookahead", type=float, default=0.45)
|
||||
parser.add_argument("--max-vx", type=float, default=0.22)
|
||||
parser.add_argument("--min-cmd-vx", type=float, default=0.04)
|
||||
parser.add_argument("--creep-cmd-vx", type=float, default=0.04)
|
||||
parser.add_argument("--yaw-stop-threshold-deg", type=float, default=45.0)
|
||||
parser.add_argument("--turn-in-place-enter-deg", type=float, default=70.0)
|
||||
parser.add_argument("--cmd-vx-scale", type=float, default=1.0)
|
||||
parser.add_argument("--no-local-safety", action="store_true")
|
||||
parser.add_argument("--include-reverse-vx", action="store_true")
|
||||
parser.add_argument("--quick", action="store_true", help="Use shorter timeouts for fast iteration.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def run_command(args: list[str], cwd: Path) -> tuple[int, str]:
|
||||
proc = subprocess.run(
|
||||
args,
|
||||
cwd=str(cwd),
|
||||
text=True,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
)
|
||||
return proc.returncode, proc.stdout
|
||||
|
||||
|
||||
def newest_report(out_dir: Path) -> Path | None:
|
||||
reports = sorted(out_dir.glob("route_check_*/report.json"), key=lambda p: p.stat().st_mtime, reverse=True)
|
||||
return reports[0] if reports else None
|
||||
|
||||
|
||||
def load_report(path: Path | None) -> dict[str, Any]:
|
||||
if path is None:
|
||||
return {}
|
||||
return json.loads(path.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def run_sim_case(
|
||||
name: str,
|
||||
args: argparse.Namespace,
|
||||
extra: list[str],
|
||||
max_time: int,
|
||||
waypoint_timeout: int,
|
||||
) -> dict[str, Any]:
|
||||
case_out = args.out_dir / name
|
||||
case_out.mkdir(parents=True, exist_ok=True)
|
||||
cmd = [
|
||||
sys.executable,
|
||||
"sim2sim/nav_route_sim2sim_check.py",
|
||||
"--terrain-xml",
|
||||
str(args.terrain_xml),
|
||||
"--points",
|
||||
str(args.points),
|
||||
"--onnx",
|
||||
str(args.onnx),
|
||||
"--policy-backend",
|
||||
"policy-runner",
|
||||
"--mission",
|
||||
"json",
|
||||
"--start-yaw-offset-deg",
|
||||
str(args.start_yaw_offset_deg),
|
||||
"--heading-offset-deg",
|
||||
str(args.heading_offset_deg),
|
||||
"--start-z",
|
||||
str(args.start_z),
|
||||
"--settle-steps",
|
||||
str(args.settle_steps),
|
||||
"--follower",
|
||||
str(args.follower),
|
||||
"--lookahead",
|
||||
str(args.lookahead),
|
||||
"--max-vx",
|
||||
str(args.max_vx),
|
||||
"--min-cmd-vx",
|
||||
str(args.min_cmd_vx),
|
||||
"--creep-cmd-vx",
|
||||
str(args.creep_cmd_vx),
|
||||
"--yaw-stop-threshold-deg",
|
||||
str(args.yaw_stop_threshold_deg),
|
||||
"--turn-in-place-enter-deg",
|
||||
str(args.turn_in_place_enter_deg),
|
||||
"--cmd-vx-scale",
|
||||
str(args.cmd_vx_scale),
|
||||
"--max-time",
|
||||
str(max_time),
|
||||
"--waypoint-timeout",
|
||||
str(waypoint_timeout),
|
||||
"--sample-every",
|
||||
"10",
|
||||
"--out-dir",
|
||||
str(case_out),
|
||||
*extra,
|
||||
]
|
||||
if args.no_local_safety:
|
||||
cmd.append("--no-local-safety")
|
||||
code, output = run_command(cmd, PROJECT_ROOT)
|
||||
report_path = newest_report(case_out)
|
||||
report = load_report(report_path)
|
||||
return {
|
||||
"name": name,
|
||||
"exit_code": code,
|
||||
"success": bool(report.get("success", False)),
|
||||
"reason": report.get("reason", "no report"),
|
||||
"reached": f"{report.get('reached_count', '?')}/{report.get('waypoint_count', '?')}",
|
||||
"min_margin": report.get("min_margin"),
|
||||
"report": str(report_path) if report_path else None,
|
||||
"output_tail": "\n".join(output.splitlines()[-12:]),
|
||||
}
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
args.out_dir.mkdir(parents=True, exist_ok=True)
|
||||
suite_dir = args.out_dir / f"suite_{time.strftime('%Y%m%d_%H%M%S')}"
|
||||
suite_dir.mkdir(parents=True, exist_ok=True)
|
||||
args.out_dir = suite_dir
|
||||
|
||||
safety_cmd = [
|
||||
sys.executable,
|
||||
"tools/nav_tools/route_safety_check.py",
|
||||
"--points",
|
||||
str(args.points),
|
||||
"--xml",
|
||||
str(args.terrain_xml),
|
||||
"--onnx",
|
||||
str(args.onnx),
|
||||
"--top",
|
||||
"12",
|
||||
]
|
||||
safety_code, safety_output = run_command(safety_cmd, PROJECT_ROOT)
|
||||
cases = []
|
||||
full_time = 60 if args.quick else 180
|
||||
slice_time = 45 if args.quick else 90
|
||||
cases.append(run_sim_case("full", args, [], full_time, 20))
|
||||
cases.append(run_sim_case("start_1_8", args, ["--start-id", "1", "--end-id", "8"], slice_time, 20))
|
||||
cases.append(run_sim_case("mid_13_22", args, ["--start-id", "13", "--end-id", "22"], slice_time, 20))
|
||||
cases.append(run_sim_case("slalom_30_45", args, ["--start-id", "30", "--end-id", "45"], slice_time, 20))
|
||||
if args.include_reverse_vx:
|
||||
old_scale = args.cmd_vx_scale
|
||||
args.cmd_vx_scale = -abs(old_scale)
|
||||
cases.append(run_sim_case("reverse_start_1_8", args, ["--start-id", "1", "--end-id", "8"], slice_time, 20))
|
||||
cases.append(run_sim_case("reverse_slalom_30_45", args, ["--start-id", "30", "--end-id", "45"], slice_time, 20))
|
||||
args.cmd_vx_scale = old_scale
|
||||
|
||||
summary = {
|
||||
"points": str(args.points),
|
||||
"terrain_xml": str(args.terrain_xml),
|
||||
"onnx": str(args.onnx),
|
||||
"follower": args.follower,
|
||||
"lookahead": args.lookahead,
|
||||
"max_vx": args.max_vx,
|
||||
"cmd_vx_scale": args.cmd_vx_scale,
|
||||
"no_local_safety": args.no_local_safety,
|
||||
"suite_dir": str(suite_dir),
|
||||
"safety_exit_code": safety_code,
|
||||
"safety_tail": "\n".join(safety_output.splitlines()[-18:]),
|
||||
"cases": cases,
|
||||
}
|
||||
summary_path = suite_dir / "summary.json"
|
||||
summary_path.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
print("Route experiment suite")
|
||||
print(f" points: {args.points}")
|
||||
print(f" suite: {suite_dir}")
|
||||
print(f" safety: {'PASS' if safety_code == 0 else 'FAIL'}")
|
||||
for case in cases:
|
||||
print(
|
||||
f" {case['name']}: success={case['success']} "
|
||||
f"reached={case['reached']} reason={case['reason']} "
|
||||
f"margin={case['min_margin']} report={case['report']}"
|
||||
)
|
||||
print(f" summary: {summary_path}")
|
||||
return 0 if safety_code == 0 and all(case["success"] for case in cases) else 2
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,117 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Translate and yaw-rotate an ASCII PCD in XY.
|
||||
|
||||
Example:
|
||||
python transform_pcd_xy.py pcd/1hao.pcd --in-place --origin-x 9.34 --origin-y 0.88 --yaw-deg -90
|
||||
|
||||
The transform is:
|
||||
1. subtract origin from x/y
|
||||
2. rotate around (0, 0) by yaw-deg
|
||||
|
||||
Only ASCII PCD files are supported. Non-x/y fields are preserved.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
TOOL_DIR = Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def tool_relative(path: Path) -> Path:
|
||||
if path.is_absolute():
|
||||
raise ValueError("Use a path relative to this nav_tools folder.")
|
||||
return TOOL_DIR / path
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description="Translate and yaw-rotate an ASCII PCD in XY.")
|
||||
parser.add_argument("input", type=Path, help="Input .pcd file.")
|
||||
parser.add_argument("--output", type=Path, help="Output .pcd file. Required unless --in-place is used.")
|
||||
parser.add_argument("--in-place", action="store_true", help="Replace the input file after a successful transform.")
|
||||
parser.add_argument("--origin-x", type=float, default=0.0, help="X value to subtract before rotation.")
|
||||
parser.add_argument("--origin-y", type=float, default=0.0, help="Y value to subtract before rotation.")
|
||||
parser.add_argument("--yaw-deg", type=float, default=0.0, help="Yaw rotation in degrees after origin subtraction.")
|
||||
parser.add_argument("--precision", type=int, default=8, help="Decimal precision for transformed x/y.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def fmt(value: float, precision: int) -> str:
|
||||
if abs(value) < 0.5 * 10 ** (-precision):
|
||||
value = 0.0
|
||||
return f"{value:.{precision}f}".rstrip("0").rstrip(".")
|
||||
|
||||
|
||||
def transform_file(
|
||||
source: Path,
|
||||
target: Path,
|
||||
origin_x: float,
|
||||
origin_y: float,
|
||||
yaw_deg: float,
|
||||
precision: int,
|
||||
) -> int:
|
||||
yaw = math.radians(yaw_deg)
|
||||
cos_yaw = math.cos(yaw)
|
||||
sin_yaw = math.sin(yaw)
|
||||
data_started = False
|
||||
fields: list[str] = []
|
||||
x_idx = 0
|
||||
y_idx = 1
|
||||
count = 0
|
||||
|
||||
with source.open("r", encoding="utf-8", errors="ignore") as fin, target.open("w", encoding="utf-8", newline="\n") as fout:
|
||||
for line in fin:
|
||||
stripped = line.strip()
|
||||
upper = stripped.upper()
|
||||
if data_started and stripped:
|
||||
parts = stripped.split()
|
||||
if len(parts) > max(x_idx, y_idx):
|
||||
x0 = float(parts[x_idx]) - origin_x
|
||||
y0 = float(parts[y_idx]) - origin_y
|
||||
x1 = x0 * cos_yaw - y0 * sin_yaw
|
||||
y1 = x0 * sin_yaw + y0 * cos_yaw
|
||||
parts[x_idx] = fmt(x1, precision)
|
||||
parts[y_idx] = fmt(y1, precision)
|
||||
fout.write(" ".join(parts) + "\n")
|
||||
count += 1
|
||||
else:
|
||||
fout.write(line)
|
||||
else:
|
||||
fout.write(line)
|
||||
if upper.startswith("FIELDS "):
|
||||
fields = stripped.split()[1:]
|
||||
if "x" in fields and "y" in fields:
|
||||
x_idx = fields.index("x")
|
||||
y_idx = fields.index("y")
|
||||
elif upper.startswith("DATA "):
|
||||
if "ASCII" not in upper:
|
||||
raise RuntimeError(f"Only ASCII PCD is supported: {source}")
|
||||
data_started = True
|
||||
return count
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
source = tool_relative(args.input)
|
||||
if args.in_place:
|
||||
target = source.with_suffix(source.suffix + ".tmp")
|
||||
elif args.output:
|
||||
target = tool_relative(args.output)
|
||||
else:
|
||||
raise SystemExit("--output is required unless --in-place is used")
|
||||
|
||||
count = transform_file(source, target, args.origin_x, args.origin_y, args.yaw_deg, args.precision)
|
||||
if count <= 0:
|
||||
target.unlink(missing_ok=True)
|
||||
raise SystemExit("No PCD points transformed")
|
||||
if args.in_place:
|
||||
target.replace(source)
|
||||
print(f"transformed {count} points: origin=({args.origin_x}, {args.origin_y}), yaw={args.yaw_deg} deg")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,72 @@
|
||||
<mujoco model="nav_tools terrain A_C">
|
||||
<statistic center="-5.5 1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 14m x 6m outer boundary transformed from A_bian: x_C=y_bian, y_C=-x_bian. -->
|
||||
<geom name="border_x_min" pos="-9.5 1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_x_max" pos="4.5 1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_min" pos="-2.5 -1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_max" pos="-2.5 4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.620583 -0.010049 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.470246 2.97224 0.075" type="box" size="0.5 0.5 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.970246 3.97224 0.075" type="box" size="0.5 1.0 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.219014 4.406386 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.219014 3.406386 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.219014 3.906386 0.325" type="cylinder" size="0.015 0.5" quat="0.5 -0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.325629 -0.037815 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.025629 -0.037815 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.725629 -0.037815 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 -0.037815 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.574371 -0.037815 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.874371 -0.037815 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.174371 -0.037815 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 0.612185 0.25" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 0.912185 0.15" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.075629 1.212185 0.05" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.540216 0.040352 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546088 0.121869866 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 0.040352 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 0.890352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 1.440352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 1.990352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 2.540352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.420216 3.090352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.920216 3.940352 0.1" type="box" size="0.5 1.0 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.420216 3.040552 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836142 0.086174413 0.086174413 -0.701836142" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.170216 3.590352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.170216 3.890352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.170216 4.190352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.079784 3.940352 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.579784 2.874152 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416012 0.061628579 0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2.079784 4.008452 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416012 -0.061628579 -0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.121128 0.005994 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.521128 1.405994 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.771128 -0.244006 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom name="slalom_pole_1_base" pos="-6.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_1_shaft" pos="-6.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_2_base" pos="-7.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_2_shaft" pos="-7.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_3_base" pos="-8.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_3_shaft" pos="-8.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_4_base" pos="-8.521128 1.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom name="slalom_pole_4_shaft" pos="-8.521128 1.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,72 @@
|
||||
<mujoco model="nav_tools terrain B_C">
|
||||
<statistic center="-5.5 -1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 14m x 6m outer boundary, mirrored from A_C across the x axis. -->
|
||||
<geom name="border_x_min" pos="-9.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_x_max" pos="4.401477 -1.937395 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_min" pos="-2.5 -4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_max" pos="-2.5 1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.589248 0.022509 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.430146 -2.855226 0.075" type="box" size="0.5 0.5 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.930146 -3.855226 0.075" type="box" size="0.5 1.0 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.231818 -4.372727 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.231818 -3.372727 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.231818 -3.872727 0.325" type="cylinder" size="0.015 0.5" quat="0.5 0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 -1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.521477 -0.037395 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546 -0.12187 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -0.037395 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -0.887395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -1.437395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -1.987395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -2.537395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.401477 -3.087395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.901477 -3.937395 0.1" type="box" size="0.5 1.0 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.401477 -3.037595 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836 -0.086174 0.086174 -0.701836" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.151477 -3.587395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.151477 -3.887395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.151477 -4.187395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.098523 -3.937395 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.598523 -2.871195 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416 -0.061629 0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2.098523 -4.005495 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416 0.061629 -0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.075243 0.034483 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.475243 -1.365517 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.725243 0.284483 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-6.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-6.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 -0.965517 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.475243 -0.965517 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,67 @@
|
||||
<mujoco model="nav_tools terrain A_C">
|
||||
<statistic center="-5.5 1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.5 0 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.4 3.0361 0.075" type="box" size="0.5 0.5 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.9 4.0361 0.075" type="box" size="0.5 1.0 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.5 4.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 3.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 3.9 0.325" type="cylinder" size="0.015 0.5" quat="0.5 -0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.62 0 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546088 0.121869866 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 0 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 0.85 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 1.4 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 1.95 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 2.5 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 3.05 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3 3.9 0.1" type="box" size="0.5 1.0 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.5 3.0002 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836142 0.086174413 0.086174413 -0.701836142" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 3.55 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 3.85 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 4.15 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 3.9 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.5 2.8338 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416012 0.061628579 0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2 3.9681 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416012 -0.061628579 -0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.025 0.0861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.425 1.4861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.675 -0.1639 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-6.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-6.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 1.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 1.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,72 @@
|
||||
<mujoco model="nav_tools terrain B_C">
|
||||
<statistic center="-5.5 -1.9 0.4" extent="5.0" />
|
||||
<visual>
|
||||
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
|
||||
<rgba haze="0.15 0.25 0.35 1" />
|
||||
<global azimuth="90" elevation="-20" />
|
||||
</visual>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
|
||||
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
|
||||
</asset>
|
||||
<worldbody>
|
||||
<!-- 比赛地图基准地面。 -->
|
||||
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
|
||||
<!-- 14m x 6m outer boundary, mirrored from A_C across the x axis. -->
|
||||
<geom name="border_x_min" pos="-9.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_x_max" pos="4.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_min" pos="-2.5 -4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<geom name="border_y_max" pos="-2.5 1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
|
||||
<!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
|
||||
<geom pos="-3.5 0 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
|
||||
<geom pos="-8.4 -3.0361 0.075" type="box" size="0.5 0.5 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.9 -4.0361 0.075" type="box" size="0.5 1.0 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
|
||||
<geom pos="-5.5 -4.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 -3.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
|
||||
<geom pos="-5.5 -3.9 0.325" type="cylinder" size="0.015 0.5" quat="0.5 0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
|
||||
<!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
|
||||
<geom pos="-5.5 -1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
|
||||
<geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
|
||||
<geom pos="2.62 0 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546 -0.12187 0 0" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 0 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -0.85 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -1.4 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -1.95 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -2.5 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3.5 -3.05 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="3 -3.9 0.1" type="box" size="0.5 1.0 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="2.5 -3.0002 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836 -0.086174 0.086174 -0.701836" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 -3.55 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 -3.85 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="1.25 -4.15 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="0 -3.9 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-1.5 -2.8338 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416 -0.061629 0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-2 -3.9681 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416 0.061629 -0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
|
||||
<!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
|
||||
<geom pos="-6.025 -0.0861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.425 -1.4861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-8.675 0.1639 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
|
||||
<geom pos="-6.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-6.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-7.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -1.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
<geom pos="-8.425 -1.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
RC_WheelLeg 是山东华宇工学院 16DOF 串联轮足机器人项目。
|
||||
|
||||
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构、后期 MuJoCo 工具集和后期 Sim2Sim 已经完成整理。
|
||||
当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构、后期 MuJoCo/Sim2Sim 和导航打点工具已经完成整理。
|
||||
|
||||
## 平台概览
|
||||
|
||||
@@ -38,6 +38,7 @@ RC_WheelLeg/
|
||||
- [x] 整理比赛最终训练代码架构
|
||||
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
||||
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
||||
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
|
||||
- [ ] 核对比赛机械与仿真模型参数
|
||||
- [ ] 整理 URDF/MJCF 机器人描述
|
||||
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
||||
|
||||
Reference in New Issue
Block a user