feat: 发布同周期ACT控制样本

xr_rm_teleop/test/test_act_control_sample.py

@@ -0,0 +1,194 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+from builtin_interfaces.msg import Time as TimeMsg
+
+from xr_rm_interfaces.msg import XrController
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _ActCycleContext,
+)
+
+
+class FakePublisher:
+    def __init__(self, error=None) -> None:
+        self.error = error
+        self.messages = []
+
+    def publish(self, message) -> None:
+        if self.error is not None:
+            raise self.error
+        self.messages.append(message)
+
+
+class FakeLogger:
+    def __init__(self) -> None:
+        self.warnings = []
+
+    def warn(self, message, **kwargs) -> None:
+        del kwargs
+        self.warnings.append(message)
+
+
+def _controller(*, grip=True) -> XrController:
+    message = XrController()
+    message.hand = "right"
+    message.grip = grip
+    message.trigger = 0.25
+    message.primary = False
+    message.secondary = False
+    message.axis = [0.1, -0.2]
+    message.pose.orientation.w = 1.0
+    return message
+
+
+def _teleop(*, last_target=None, tool_state=True):
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = _controller()
+    teleop._active = True
+    teleop._control_fault_latched = False
+    teleop._last_current_pose = np.eye(4)
+    teleop._robot_start_transform = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_successful_action_target = last_target
+    teleop._ik_solver = SimpleNamespace(
+        joint_position_limits=np.asarray([[-1.0, 1.0]] * 7)
+    )
+    teleop._tool_state_snapshot = lambda: (
+        True,
+        tool_state,
+        False,
+        False,
+    )
+    teleop.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: TimeMsg())
+    )
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+
+
+def _cycle(**overrides) -> _ActCycleContext:
+    values = {
+        "control_seq": 100,
+        "control_monotonic_ns": 1_000_000_000,
+        "feedback_monotonic_ns": 990_000_000,
+        "action_monotonic_ns": 1_005_000_000,
+        "feedback_age_ms": 10.0,
+        "q_actual": [0.1] * 7,
+        "q_qp_raw": [0.3] * 7,
+        "q_target": [0.2] * 7,
+        "current_pose": np.eye(4),
+        "raw_target_pose": np.eye(4),
+        "target_pose": np.eye(4),
+        "command_velocity": [0.0] * 6,
+        "feedback_valid": True,
+        "command_sent": True,
+        "qp_attempted": True,
+        "qp_success": True,
+    }
+    values.update(overrides)
+    return _ActCycleContext(**values)
+
+
+def test_act_sample_uses_feedback_and_limited_target_from_one_cycle() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+
+    message = teleop._build_act_control_sample(_cycle())
+
+    assert message.control_seq == 100
+    assert message.q_actual == pytest.approx([0.1] * 7)
+    assert message.q_qp_raw == pytest.approx([0.3] * 7)
+    assert message.q_target == pytest.approx([0.2] * 7)
+    assert message.joint_lower_limits == pytest.approx([-1.0] * 7)
+    assert message.joint_upper_limits == pytest.approx([1.0] * 7)
+    assert message.command_sent
+    assert message.action_valid
+    assert message.qp_attempted
+    assert message.qp_success
+
+
+def test_act_sample_marks_qp_fallback_as_valid_held_action() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+    cycle = _cycle(
+        q_qp_raw=[0.2] * 7,
+        q_target=[0.2] * 7,
+        qp_success=False,
+    )
+
+    message = teleop._build_act_control_sample(cycle)
+
+    assert message.q_target == pytest.approx([0.2] * 7)
+    assert message.action_valid
+    assert message.qp_attempted
+    assert not message.qp_success
+
+
+def test_act_sample_holds_last_action_while_grip_is_released() -> None:
+    teleop = _teleop(last_target=[0.4] * 7)
+    teleop._last_msg = _controller(grip=False)
+    teleop._active = False
+    cycle = _cycle(
+        q_qp_raw=None,
+        q_target=None,
+        command_sent=False,
+        qp_attempted=False,
+        qp_success=False,
+        action_monotonic_ns=-1,
+    )
+
+    message = teleop._build_act_control_sample(cycle)
+
+    assert message.q_target == pytest.approx([0.4] * 7)
+    assert message.action_valid
+    assert not message.command_sent
+    assert not message.teleop_active
+
+
+def test_act_sample_marks_send_failure_invalid() -> None:
+    teleop = _teleop(last_target=[0.4] * 7)
+
+    message = teleop._build_act_control_sample(
+        _cycle(send_failed=True, command_sent=False)
+    )
+
+    assert not message.action_valid
+
+
+def test_act_sample_marks_unknown_gripper_state() -> None:
+    teleop = _teleop(last_target=[0.2] * 7, tool_state=None)
+
+    message = teleop._build_act_control_sample(_cycle())
+
+    assert not message.gripper_state_known
+
+
+def test_act_sample_publish_failure_does_not_escape_control_path() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+    logger = FakeLogger()
+    teleop._act_sample_pub = FakePublisher(RuntimeError("dds failed"))
+    teleop.get_logger = lambda: logger
+
+    teleop._publish_act_control_sample(_cycle())
+
+    assert logger.warnings == [
+        "right_rm75 ACT原子样本发布失败：dds failed"
+    ]
+
+
+def test_control_tick_wraps_one_impl_call_in_one_atomic_sample() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    cycles = []
+    published = []
+    teleop._act_control_seq = 7
+    teleop._control_tick_impl = lambda cycle: cycles.append(cycle)
+    teleop._publish_act_control_sample = lambda cycle: published.append(cycle)
+
+    teleop._control_tick()
+
+    assert len(cycles) == 1
+    assert published == cycles
+    assert cycles[0].control_seq == 7
+    assert teleop._act_control_seq == 8