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
import re
import time
from dataclasses import dataclass
from enum import Enum
from functools import lru_cache
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_CTRL_MODE_TEACH = 0x02
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:
@ -89,54 +79,27 @@ def wait_enable_piper(arm: Any, timeout_s: float, retry_interval_s: float = 0.2)
return False
def parse_piper_ctrl_mode(value: Any) -> PiperCtrlMode:
if value is None:
return PiperCtrlMode(code=None, text="None")
def _piper_mode_to_int(value: Any) -> int | None:
if isinstance(value, bool):
code = int(value)
return PiperCtrlMode(code=code, text=f"0x{code:02X}")
return int(value)
if isinstance(value, int):
return PiperCtrlMode(code=value, text=f"0x{value:02X}")
if isinstance(value, Enum):
enum_value = value.value
if isinstance(enum_value, int):
return PiperCtrlMode(code=enum_value, text=str(value))
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))
return value
if hasattr(value, "value") and isinstance(value.value, int):
return int(value.value)
if hasattr(value, "__int__"):
return int(value)
return None
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)
while time.monotonic() < deadline:
status_msg = arm.GetArmStatus()
if getattr(status_msg, "time_stamp", 0.0) > 0.0:
arm_status = getattr(status_msg, "arm_status", None)
if arm_status is not None:
mode = parse_piper_ctrl_mode(getattr(arm_status, "ctrl_mode", None))
if mode.code is not None:
mode = _piper_mode_to_int(getattr(arm_status, "ctrl_mode", None))
if mode is not None:
return mode
time.sleep(max(0.005, poll_s))
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. "
"Check CAN wiring/power and rerun."
)
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}:
if mode in {PIPER_CTRL_MODE_TEACH, PIPER_CTRL_MODE_LINKAGE_TEACH_INPUT}:
arm.MasterSlaveConfig(0xFC, 0x00, 0x00, 0x00)
if settle_s > 0:
time.sleep(settle_s)
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."
)