docs: 添加手柄主键回位实施计划

2026-07-31 15:24:59 +08:00
parent b7eab7cc76
commit 80e823c097
1 changed files with 486 additions and 0 deletions
@@ -0,0 +1,486 @@
 # 手柄主键回初始位姿实施计划
 > **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
 **Goal:** 左手 X 键和右手 A 键分别让对应机械臂安全回到配置的初始关节位姿，并同步三份机械臂配置中的新关节角。
 **Architecture:** 继续使用现有左右手柄独立话题和单臂遥操作节点，不增加协调节点。遥操作节点检测自身 `XrController.primary` 的上升沿，先停止当前遥操作，再调用真机或 mock 适配器的同名回位方法并重新同步关节状态。
 **Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、pytest、ament/colcon、RealMan Python API2（仅真机运行时）。
 ## 全局约束
 - 构建、测试和运行命令在 `/home/robot/WS_xr` 执行，并先运行 `source /opt/ros/humble/setup.bash`。
 - 所有自动验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。
 - 保留工作空间与圆柱限位、线速度与角速度限制、指令超时和安全停止逻辑。
 - `configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 默认值保持 `false`。
 - mock 模式不得导入或依赖睿尔曼厂商 SDK，不新增 RealMan 连接。
 - 只修改完成本功能所需文件，不新增依赖、节点、话题、服务或配置项。
 - 左臂初始关节角（度）：`[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]`。
 - 右臂初始关节角（度）：`[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]`。
 ---
 ### Task 1: 复用适配器初始位姿运动
 **Files:**
 - Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:42-82`
 - Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:178-182`
 - Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:454-459`
 - Test: `src/xr_rm_teleop/test/test_initial_joint_pose.py:13-35`
 **Interfaces:**
 - Consumes: 现有 `initial_joint_pose: list[float]`（度）和 `init_move_speed: int`。
 - Produces: `MockRealManAdapter.move_to_initial_pose() -> None`。
 - Produces: `RealManAdapter.move_to_initial_pose() -> None`。
 - [ ] **Step 1: 先写失败测试**
 将真机测试改为调用公开方法，并增加 mock 恢复初始关节角的测试：
 ```python
 def test_initial_pose_uses_joint_move_only() -> None:
    class FakeArm:
        def __init__(self) -> None:
            self.calls = []
        def rm_movej(self, *args):
            self.calls.append(args)
            return 0
    joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
    adapter = RealManAdapter(
        "127.0.0.1",
        8080,
        0,
        "127.0.0.1",
        8090,
        initial_joint_pose=joints,
    )
    adapter._arm = FakeArm()
    adapter.move_to_initial_pose()
    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
 def test_mock_initial_pose_restores_configured_joints() -> None:
    initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
    adapter = MockRealManAdapter(initial_degrees)
    adapter.send_joint_target([0.0] * 7, follow=False)
    adapter.move_to_initial_pose()
    assert adapter.read_joint_state().positions == pytest.approx(
        [math.radians(value) for value in initial_degrees]
    )
 ```
 - [ ] **Step 2: 运行测试并确认按预期失败**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
  -k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
 ```
 Expected: FAIL，两个适配器都还没有公开的 `move_to_initial_pose` 方法。
 - [ ] **Step 3: 写最小实现**
 在 mock 中保存初始弧度值并实现恢复：
 ```python
 self._initial_joint_positions = [
    math.radians(value) for value in initial_joint_degrees
 ]
 self._joint_positions = list(self._initial_joint_positions)
 ```
 ```python
 def move_to_initial_pose(self) -> None:
    self._joint_positions = list(self._initial_joint_positions)
    self.last_joint_target = list(self._joint_positions)
 ```
 将真机 `_move_to_initial_pose` 改为公开方法，并保留原有阻塞式关节运动：
 ```python
 def move_to_initial_pose(self) -> None:
    self._require_arm()
    if self._initial_joint_pose is None:
        raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
    ret = self._arm.rm_movej(
        self._initial_joint_pose,
        self._init_move_speed,
        0,
        0,
        1,
    )
    self._check_return(ret, "rm_movej(initial_joint_pose)")
 ```
 同时把 `connect()` 中的启动回位调用改为：
 ```python
 if self._move_to_initial_pose_on_connect:
    self.move_to_initial_pose()
 ```
 - [ ] **Step 4: 运行测试并确认通过**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
  -k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
 ```
 Expected: PASS。
 - [ ] **Step 5: 创建本地提交**
 ```bash
 cd /home/robot/WS_xr/src
 git add xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
  xr_rm_teleop/test/test_initial_joint_pose.py
 git commit -m "feat: 复用适配器初始位姿运动"
 ```
 ### Task 2: 在遥操作节点处理主键上升沿
 **Files:**
 - Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:276-299`
 - Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:514-534`
 - Test: `src/xr_rm_teleop/test/test_joint_control.py:16-114`
 **Interfaces:**
 - Consumes: `XrController.primary: bool`。
 - Consumes: Task 1 的 `adapter.move_to_initial_pose() -> None`。
 - Produces: `SingleArmVelocityTeleop._handle_initial_pose_button(msg: XrController) -> None`。
 - [ ] **Step 1: 先写主键边沿失败测试**
 在 `test_joint_control.py` 增加测试辅助函数和成功路径测试：
 ```python
 def _primary_button_teleop(*, move_error=None):
    events = []
    errors = []
    snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
    class Adapter:
        def move_to_initial_pose(self):
            events.append("move")
            if move_error is not None:
                raise move_error
        def read_joint_state(self):
            events.append("read")
            return snapshot
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._adapter = Adapter()
    teleop._last_primary_pressed = None
    teleop._grip_rearm_required = False
    teleop._safe_stop = lambda reset_active: events.append(
        ("stop", reset_active)
    )
    teleop._reset_joint_state = lambda value: events.append(("sync", value))
    teleop._handle_trigger_gripper = lambda msg: None
    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
    teleop.get_logger = lambda: SimpleNamespace(
        info=lambda message: None,
        error=lambda message: errors.append(message),
    )
    return teleop, events, errors, snapshot
 def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
    teleop, events, _, snapshot = _primary_button_teleop()
    released = SimpleNamespace(primary=False)
    pressed = SimpleNamespace(primary=True)
    teleop._on_controller(released)
    teleop._on_controller(pressed)
    teleop._on_controller(pressed)
    teleop._on_controller(released)
    teleop._on_controller(pressed)
    expected_once = [
        ("stop", True),
        "move",
        "read",
        ("sync", snapshot),
    ]
    assert events == expected_once * 2
    assert teleop._grip_rearm_required
 ```
 - [ ] **Step 2: 运行测试并确认按预期失败**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k primary_button_rising_edge_moves_once_and_resyncs -v
 ```
 Expected: FAIL，因为 `_on_controller` 尚未处理 `primary`。
 - [ ] **Step 3: 写最小成功实现**
 在节点状态中增加与现有 trigger 相同的首次采样保护：
 ```python
 self._last_primary_pressed: bool | None = None
 ```
 在现有回调中接入主键处理：
 ```python
 def _on_controller(self, msg: XrController) -> None:
    self._last_msg = msg
    self._last_msg_time = self.get_clock().now()
    self._handle_initial_pose_button(msg)
    self._handle_trigger_gripper(msg)
 ```
 增加主键上升沿处理；首次采样只建立状态，避免节点启动时按键已经按住而意外运动：
 ```python
 def _handle_initial_pose_button(self, msg: XrController) -> None:
    if self._last_primary_pressed is None:
        self._last_primary_pressed = msg.primary
        return
    rising_edge = msg.primary and not self._last_primary_pressed
    self._last_primary_pressed = msg.primary
    if not rising_edge:
        return
    self._grip_rearm_required = True
    self._safe_stop(reset_active=True)
    self._adapter.move_to_initial_pose()
    self._reset_joint_state(self._adapter.read_joint_state())
    self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
 ```
 - [ ] **Step 4: 运行测试并确认通过**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k primary_button_rising_edge_moves_once_and_resyncs -v
 ```
 Expected: PASS。
 - [ ] **Step 5: 先写失败路径测试**
 ```python
 def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
    failure = RuntimeError("rm_movej failed")
    teleop, events, errors, _ = _primary_button_teleop(
        move_error=failure
    )
    teleop._on_controller(SimpleNamespace(primary=False))
    teleop._on_controller(SimpleNamespace(primary=True))
    assert events == [("stop", True), "move"]
    assert teleop._grip_rearm_required
    assert errors == [
        "right_rm75 回初始位姿失败：rm_movej failed"
    ]
 ```
 - [ ] **Step 6: 运行失败路径测试并确认按预期失败**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_joint_control.py \
  -k primary_button_move_failure_logs_and_stays_stopped -v
 ```
 Expected: FAIL，并抛出 `RuntimeError: rm_movej failed`。
 - [ ] **Step 7: 增加最小异常处理**
 用 `try/except` 包住回位和状态同步，失败时记录错误并保持已经设置的停止与 Grip
 重新使能状态：
 ```python
 try:
    self._adapter.move_to_initial_pose()
    self._reset_joint_state(self._adapter.read_joint_state())
 except Exception as exc:
    self.get_logger().error(
        f"{self._arm_name} 回初始位姿失败：{exc}"
    )
    return
 self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
 ```
 - [ ] **Step 8: 运行两条主键测试并确认通过**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_joint_control.py -k primary_button -v
 ```
 Expected: PASS。
 - [ ] **Step 9: 创建本地提交**
 ```bash
 cd /home/robot/WS_xr/src
 git add xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
  xr_rm_teleop/test/test_joint_control.py
 git commit -m "feat: 添加手柄主键回初始位姿"
 ```
 ### Task 3: 同步三份初始位姿配置
 **Files:**
 - Modify: `src/xr_rm_bringup/config/left_arm_rm75.yaml:57`
 - Modify: `src/xr_rm_bringup/config/right_arm_rm75.yaml:57`
 - Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:64`
 - Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:121`
 **Interfaces:**
 - Consumes: 用户确认的左右臂 7 个关节角，单位为度。
 - Produces: 单臂和双臂模式一致的对应臂 `initial_joint_pose`。
 - [ ] **Step 1: 只替换四处初始位姿**
 ```yaml
 # left_arm_rm75.yaml
 initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
 # right_arm_rm75.yaml
 initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
 # dual_arm_rm75.yaml / left_arm_teleop
 initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
 # dual_arm_rm75.yaml / right_arm_teleop
 initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
 ```
 - [ ] **Step 2: 解析 YAML 并验证四处值**
 Run:
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 python3 - <<'PY'
 from pathlib import Path
 import yaml
 config_dir = Path("src/xr_rm_bringup/config")
 left = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
 right = [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
 left_single = yaml.safe_load((config_dir / "left_arm_rm75.yaml").read_text())
 right_single = yaml.safe_load((config_dir / "right_arm_rm75.yaml").read_text())
 dual = yaml.safe_load((config_dir / "dual_arm_rm75.yaml").read_text())
 assert left_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == left
 assert right_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == right
 assert dual["left_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == left
 assert dual["right_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == right
 PY
 ```
 Expected: exit code 0，无输出。
 - [ ] **Step 3: 确认没有改动其他 YAML 参数**
 Run:
 ```bash
 cd /home/robot/WS_xr/src
 git diff --word-diff=plain -- \
  xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml \
  xr_rm_bringup/config/dual_arm_rm75.yaml
 ```
 Expected: 只有四个 `initial_joint_pose` 列表发生变化。
 - [ ] **Step 4: 创建本地提交**
 ```bash
 cd /home/robot/WS_xr/src
 git add xr_rm_bringup/config/left_arm_rm75.yaml \
  xr_rm_bringup/config/right_arm_rm75.yaml \
  xr_rm_bringup/config/dual_arm_rm75.yaml
 git commit -m "config: 更新左右臂初始位姿"
 ```
 ### Task 4: 完整验证
 **Files:**
 - Verify: `src/xr_rm_teleop/test/test_initial_joint_pose.py`
 - Verify: `src/xr_rm_teleop/test/test_joint_control.py`
 - Verify: `src/xr_rm_teleop/test/test_orientation_control.py`
 - Verify: 全部四个 ROS2 包
 **Interfaces:**
 - Consumes: Tasks 1–3 的本地提交。
 - Produces: mock 测试与 ROS2 构建通过的可验证结果。
 - [ ] **Step 1: 运行遥操作相关测试**
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
  src/xr_rm_teleop/test/test_joint_control.py \
  src/xr_rm_teleop/test/test_orientation_control.py
 ```
 Expected: PASS，无 error 或 warning。
 - [ ] **Step 2: 构建工作空间**
 ```bash
 cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 colcon build --symlink-install
 ```
 Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和 `xr_rm_bringup`
 构建完成，无失败包。
 - [ ] **Step 3: 检查最终范围**
 ```bash
 cd /home/robot/WS_xr/src
 git status --short
 git log -6 --oneline
 ```
 Expected: 工作区干净；只有设计、计划、适配器、遥操作节点、两份测试和三份 YAML
 配置的相关本地提交，不存在远程写操作。