chore: 框架科学优化前的完整基线

在开始'设备契约层 + 目录重构 + 通止规装配'改造之前,先钉住当前可运行状态,
使后续任何删除/重构都可回退。

本基线包含:
- 现有 ROS 2 工作区全部源码(arm_application / arm_control / arm_bringup 等)
- 官方 flexiv_ros2 栈(含 flexiv_hardware / flexiv_controllers 等符号链接)
- 设备契约层 src/arm_interfaces(新增,46 项单测通过)
- devices/ 设备选型与品牌清单(新增)
- scripts/gen_site_example.py 与由它生成的 config/site.example.yaml

按 .gitignore 排除:build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
This commit is contained in:
arm_dev
2026-09-23 10:53:06 +08:00
commit 6edb8d39ab
635 changed files with 1165230 additions and 0 deletions
+143
View File
@@ -0,0 +1,143 @@
#!/usr/bin/env python3
"""Merge taught joint lines into handeye waypoints YAML.
Input (from handeye_teach / handeye_waypoints append file):
# pose 0
0.123456, -0.700000, ...
Target can be either:
- config/handeye/waypoints.yaml (/**: ros__parameters)
- handeye_calibrate.yaml (handeye_calibrate: ros__parameters)
Usage:
python3 scripts/merge_handeye_poses.py \\
~/ros2/config/handeye/taught_poses.txt \\
~/ros2/config/handeye/waypoints.yaml
# or via wrapper:
bash ~/ros2/scripts/run_handeye.sh merge
"""
from __future__ import annotations
import re
import sys
from pathlib import Path
def parse_poses(path: Path) -> list[list[float]]:
lines = path.read_text(encoding="utf-8").splitlines()
poses: list[list[float]] = []
buf: list[float] = []
num_re = re.compile(r"[-+]?(?:\d+\.\d*|\.\d+|\d+)(?:[eE][-+]?\d+)?")
for raw in lines:
line = raw.split("#", 1)[0].strip()
if not line:
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints, got {len(buf)}: {buf}")
poses.append(buf)
buf = []
continue
vals = [float(x) for x in num_re.findall(line)]
if not vals:
continue
if len(vals) == 7:
poses.append(vals)
elif len(vals) < 7:
buf.extend(vals)
if len(buf) > 7:
raise ValueError(f"Too many joint values in line: {line!r}")
else:
poses.append(vals[:7])
if buf:
if len(buf) != 7:
raise ValueError(f"Expected 7 joints at EOF, got {len(buf)}")
poses.append(buf)
return poses
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
rows = [f"{indent}waypoint_positions: ["]
for i, q in enumerate(poses):
rows.append(f"{indent} # pose {i}")
s = ", ".join(f"{v:.6f}" for v in q) + ","
rows.append(f"{indent} {s}")
rows.append(f"{indent}]")
return "\n".join(rows)
def merge(yaml_path: Path, poses: list[list[float]]) -> None:
text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n")
# Detect indent of waypoint_positions (2 or 4 spaces typical)
m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE)
indent = m_ind.group(1) if m_ind else " "
block = format_positions_block(poses, indent=indent)
pattern = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]",
re.MULTILINE,
)
# Also match a one-line empty list: waypoint_positions: []
one_line = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]",
re.MULTILINE,
)
if pattern.search(text):
new_text = pattern.sub(block, text, count=1)
elif one_line.search(text):
new_text = one_line.sub(block, text, count=1)
else:
raise RuntimeError("waypoint_positions block not found in YAML")
# Keep num_poses in sync when present
n = len(poses)
if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE):
new_text = re.sub(
r"^(\s*num_poses:\s*)\d+",
rf"\g<1>{n}",
new_text,
count=1,
flags=re.MULTILINE,
)
else:
# Insert num_poses just before waypoint_positions
new_text = new_text.replace(
block,
f"{indent}num_poses: {n}\n{block}",
1,
)
yaml_path.write_text(new_text, encoding="utf-8")
def main(argv: list[str]) -> int:
if len(argv) != 3:
print(
"Usage: merge_handeye_poses.py <taught_poses.txt> <waypoints.yaml>",
file=sys.stderr,
)
return 1
src = Path(argv[1]).expanduser()
dst = Path(argv[2]).expanduser()
if not src.is_file():
print(f"Not found: {src}", file=sys.stderr)
return 2
if not dst.is_file():
print(f"Not found: {dst}", file=sys.stderr)
return 3
poses = parse_poses(src)
if len(poses) < 5:
print(f"Need >=5 poses, got {len(poses)}", file=sys.stderr)
return 4
merge(dst, poses)
print(f"Merged {len(poses)} poses -> {dst}")
return 0
if __name__ == "__main__":
raise SystemExit(main(sys.argv))