Revert "fix(piper): robust ctrl_mode parsing for enum/string values"

This reverts commit 2190f5c237.
这个提交包含在:
Elvin
2026-03-07 11:44:14 +08:00
父节点 2190f5c237
当前提交 5e1d1cc999

查看文件

@ -16,10 +16,7 @@
from __future__ import annotations from __future__ import annotations
import re
import time import time
from dataclasses import dataclass
from enum import Enum
from functools import lru_cache from functools import lru_cache
from typing import Any from typing import Any
@ -35,13 +32,6 @@ PIPER_JOINT_ACTION_KEYS = tuple(f"{joint}.pos" for joint in PIPER_JOINT_NAMES)
PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",) PIPER_ACTION_KEYS = PIPER_JOINT_ACTION_KEYS + ("gripper.pos",)
PIPER_CTRL_MODE_TEACH = 0x02 PIPER_CTRL_MODE_TEACH = 0x02
PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06 PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT = 0x06
_CTRL_MODE_HEX_RE = re.compile(r"0x([0-9a-fA-F]+)")
@dataclass(frozen=True)
class PiperCtrlMode:
code: int | None
text: str
def milli_to_unit(value: float | int) -> float: def milli_to_unit(value: float | int) -> float:
@ -89,54 +79,27 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2)
return False return False
def parse_piper_ctrl_mode(value: Any) -> PiperCtrlMode: def _piper_mode_to_int(value: Any) -> int | None:
if value is None:
return PiperCtrlMode(code=None, text="None")
if isinstance(value, bool): if isinstance(value, bool):
code = int(value) return int(value)
return PiperCtrlMode(code=code, text=f"0x{code:02X}")
if isinstance(value, int): if isinstance(value, int):
return PiperCtrlMode(code=value, text=f"0x{value:02X}") return value
if hasattr(value, "value") and isinstance(value.value, int):
if isinstance(value, Enum): return int(value.value)
enum_value = value.value if hasattr(value, "__int__"):
if isinstance(enum_value, int): return int(value)
return PiperCtrlMode(code=enum_value, text=str(value)) return None
if isinstance(enum_value, str):
m = _CTRL_MODE_HEX_RE.search(enum_value)
if m:
return PiperCtrlMode(code=int(m.group(1), 16), text=str(value))
if enum_value.isdigit():
return PiperCtrlMode(code=int(enum_value), text=str(value))
return PiperCtrlMode(code=None, text=str(value))
raw_value = getattr(value, "value", None)
if isinstance(raw_value, int):
return PiperCtrlMode(code=raw_value, text=str(value))
if isinstance(value, str):
m = _CTRL_MODE_HEX_RE.search(value)
if m:
return PiperCtrlMode(code=int(m.group(1), 16), text=value)
if value.isdigit():
code = int(value)
return PiperCtrlMode(code=code, text=f"0x{code:02X}")
return PiperCtrlMode(code=None, text=value)
return PiperCtrlMode(code=None, text=repr(value))
def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> PiperCtrlMode | None: def read_piper_ctrl_mode(arm: Any, timeout_s: float = 1.0, poll_s: float = 0.02) -> int | None:
deadline = time.monotonic() + max(0.0, timeout_s) deadline = time.monotonic() + max(0.0, timeout_s)
while time.monotonic() < deadline: while time.monotonic() < deadline:
status_msg = arm.GetArmStatus() status_msg = arm.GetArmStatus()
if getattr(status_msg, "time_stamp", 0.0) > 0.0: if getattr(status_msg, "time_stamp", 0.0) > 0.0:
arm_status = getattr(status_msg, "arm_status", None) arm_status = getattr(status_msg, "arm_status", None)
if arm_status is not None: if arm_status is not None:
mode = parse_piper_ctrl_mode(getattr(arm_status, "ctrl_mode", None)) mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None))
if mode.code is not None: if mode is not None:
return mode return mode
time.sleep(max(0.005, poll_s)) time.sleep(max(0.005, poll_s))
return None return None
@ -156,18 +119,11 @@ def guard_piper_ctrl_mode_on_connect(
f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. " f"[{interface_name}] could not read arm ctrl_mode within {timeout_s:.2f}s. "
"Check CAN wiring/power and rerun." "Check CAN wiring/power and rerun."
) )
if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
if mode.code is None:
raise RuntimeError(
f"[{interface_name}] got unreadable arm ctrl_mode value: {mode.text}. "
"Check SDK version/firmware and rerun."
)
if mode.code in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00) arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
if settle_s > 0: if settle_s > 0:
time.sleep(settle_s) time.sleep(settle_s)
raise RuntimeError( raise RuntimeError(
f"[{interface_name}] arm is in master/teaching role (ctrl_mode={mode.text}). " f"[{interface_name}] arm is in master/teaching role (ctrl_mode=0x{mode:02X}). "
"Follower role command (0xFC) has been sent. Power-cycle this arm, then retry." "Follower role command (0xFC) has been sent. Power-cycle this arm, then retry."
) )