acRealman_xr/xr_rm_teleop/test/test_act_control_sample.py

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