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

xr_rm_teleop/test/test_joint_control.py

@@ -686,9 +686,10 @@ def test_qp_failure_returns_last_known_good_target() -> None:
     teleop._arm_name = "right_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(np.eye(4))
+    target, qp_success = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.1] * 7)
+    assert not qp_success
     assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
 
 
@@ -704,9 +705,10 @@ def test_qp_success_updates_last_known_good_target() -> None:
     teleop._arm_name = "left_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(np.eye(4))
+    target, qp_success = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.2] * 7)
+    assert qp_success
     assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
 
 

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -10,17 +10,19 @@ import math
 import queue
 import threading
 import time
+from dataclasses import dataclass
 from typing import Iterable
 
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from rclpy.time import Time
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Bool
 
-from xr_rm_interfaces.msg import XrController
+from xr_rm_interfaces.msg import ActControlSample, XrController
 
 from .fun_peripheral import load_peripheral_config
 from .placo_ik_solver import PlacoIkSolver
@@ -31,6 +33,30 @@ from .realman_adapter import (
 )
 
 
+@dataclass
+class _ActCycleContext:
+    control_seq: int
+    control_monotonic_ns: int
+    feedback_monotonic_ns: int = -1
+    action_monotonic_ns: int = -1
+    feedback_age_ms: float = math.inf
+    qp_duration_ms: float = 0.0
+    q_actual: list[float] | None = None
+    q_qp_raw: list[float] | None = None
+    q_target: list[float] | None = None
+    current_pose: np.ndarray | None = None
+    raw_target_pose: np.ndarray | None = None
+    target_pose: np.ndarray | None = None
+    command_velocity: list[float] | None = None
+    feedback_valid: bool = False
+    command_sent: bool = False
+    send_failed: bool = False
+    qp_attempted: bool = False
+    qp_success: bool = False
+    target_clamped: bool = False
+    control_fault: bool = False
+
+
 def _norm(values: Iterable[float]) -> float:
     return math.sqrt(sum(value * value for value in values))
 
@@ -291,6 +317,8 @@ class SingleArmVelocityTeleop(Node):
         self._latest_joint_positions: list[float] | None = None
         self._last_joint_command_target: list[float] | None = None
         self._last_joint_command_velocity: list[float] | None = None
+        self._last_successful_action_target: list[float] | None = None
+        self._act_control_seq = 0
         self._joint_feedback_ready = False
         self._grip_rearm_required = False
         self._feedback_resync_attempted = False
@@ -345,6 +373,15 @@ class SingleArmVelocityTeleop(Node):
             f"{debug_ns}/joint_target",
             10,
         )
+        self._act_sample_pub = self.create_publisher(
+            ActControlSample,
+            f"{debug_ns}/act_control_sample",
+            QoSProfile(
+                history=HistoryPolicy.KEEP_LAST,
+                depth=10,
+                reliability=ReliabilityPolicy.BEST_EFFORT,
+            ),
+        )
         self._adapter = self._make_adapter()
         self._adapter.connect()
         self._initialize_joint_state()
@@ -619,6 +656,18 @@ class SingleArmVelocityTeleop(Node):
         self._enqueue_tool_command(self._trigger_tool_open, "trigger")
 
     def _control_tick(self) -> None:
+        control_seq = getattr(self, "_act_control_seq", 0)
+        self._act_control_seq = control_seq + 1
+        cycle = _ActCycleContext(
+            control_seq=control_seq,
+            control_monotonic_ns=time.monotonic_ns(),
+        )
+        try:
+            self._control_tick_impl(cycle)
+        finally:
+            self._publish_act_control_sample(cycle)
+
+    def _control_tick_impl(self, cycle: _ActCycleContext) -> None:
         tick_started_ns = time.perf_counter_ns()
         last_tick_started_ns = getattr(
             self,
@@ -633,10 +682,12 @@ class SingleArmVelocityTeleop(Node):
         )
         now = self.get_clock().now()
         if self._control_fault_latched:
+            cycle.control_fault = True
             return
 
         snapshot = self._adapter.get_latest_joint_state()
         if not self._joint_snapshot_is_motion_ready(snapshot):
+            cycle.control_fault = True
             self._grip_rearm_required = True
             if self._joint_feedback_ready:
                 self.get_logger().warn(
@@ -647,13 +698,18 @@ class SingleArmVelocityTeleop(Node):
             self._safe_stop(reset_active=True)
             return
         assert snapshot is not None
+        cycle.q_actual = list(snapshot.positions)
+        cycle.feedback_monotonic_ns = int(snapshot.received_at * 1e9)
         feedback_age = time.monotonic() - snapshot.received_at
+        cycle.feedback_age_ms = feedback_age * 1000.0
         if feedback_age < 0.0:
+            cycle.control_fault = True
             self._grip_rearm_required = True
             self._joint_feedback_ready = False
             self._safe_stop(reset_active=True)
             return
         if feedback_age > self._command_timeout_sec:
+            cycle.control_fault = True
             self._handle_stale_joint_feedback(feedback_age)
             return
 
@@ -661,6 +717,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             current_pose = self._sync_joint_feedback(snapshot)
         except Exception as exc:
+            cycle.control_fault = True
             self.get_logger().error(
                 f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
                 throttle_duration_sec=1.0,
@@ -669,6 +726,8 @@ class SingleArmVelocityTeleop(Node):
             self._grip_rearm_required = True
             self._safe_stop(reset_active=True)
             return
+        cycle.current_pose = current_pose
+        cycle.feedback_valid = True
         if not self._joint_feedback_ready:
             if self._grip_rearm_required:
                 message = (
@@ -717,6 +776,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             controller_quat = self._controller_quaternion(self._last_msg)
         except ValueError as exc:
+            cycle.control_fault = True
             self.get_logger().warn(
                 f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
                 throttle_duration_sec=1.0,
@@ -761,19 +821,35 @@ class SingleArmVelocityTeleop(Node):
             sent_target,
             sent_orientation,
         )
+        cycle.raw_target_pose = raw_target_pose
+        cycle.target_pose = target_pose
+        cycle.command_velocity = list(velocity)
+        cycle.target_clamped = target_clamped
 
         self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
+        cycle.qp_attempted = True
         qp_started_ns = time.perf_counter_ns()
-        joint_target = self._solve_joint_target(target_pose)
+        joint_target, qp_success = self._solve_joint_target(target_pose)
         qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
+        cycle.qp_duration_ms = qp_ms
+        cycle.q_qp_raw = list(joint_target)
+        cycle.qp_success = qp_success
         send_started_ns = time.perf_counter_ns()
         sent = self._send_joint_target(joint_target)
         send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
         if sent:
+            assert self._last_joint_command_target is not None
+            final_target = list(self._last_joint_command_target)
+            self._last_successful_action_target = final_target
+            cycle.q_target = final_target
+            cycle.action_monotonic_ns = time.monotonic_ns()
+            cycle.command_sent = True
             self._last_sent_target = sent_target
             self._last_sent_orientation = sent_orientation.copy()
             self._last_command_time = now
             self._stop_sent = False
+        else:
+            cycle.send_failed = True
         total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
         try:
             self._record_timing_sample(
@@ -1233,7 +1309,10 @@ class SingleArmVelocityTeleop(Node):
             self._last_valid_joint_target = list(snapshot.positions)
         return current_pose
 
-    def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
+    def _solve_joint_target(
+        self,
+        target_pose: np.ndarray,
+    ) -> tuple[list[float], bool]:
         if self._last_valid_joint_target is None:
             raise RuntimeError("valid joint feedback has not been initialized")
         try:
@@ -1243,9 +1322,9 @@ class SingleArmVelocityTeleop(Node):
                 f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
                 throttle_duration_sec=1.0,
             )
-            return list(self._last_valid_joint_target)
+            return list(self._last_valid_joint_target), False
         self._last_valid_joint_target = list(result)
-        return list(result)
+        return list(result), True
 
     def _safe_stop(self, reset_active: bool) -> None:
         if not self._stop_sent:
@@ -1417,6 +1496,122 @@ class SingleArmVelocityTeleop(Node):
         self._cmd_vel_pub.publish(velocity_msg)
         self._target_clamped_pub.publish(clamped_msg)
 
+    def _build_act_control_sample(
+        self,
+        cycle: _ActCycleContext,
+    ) -> ActControlSample:
+        message = ActControlSample()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.header.frame_id = "rm_base"
+        message.control_seq = cycle.control_seq
+        message.control_monotonic_ns = cycle.control_monotonic_ns
+        message.feedback_monotonic_ns = cycle.feedback_monotonic_ns
+        message.action_monotonic_ns = cycle.action_monotonic_ns
+        message.feedback_age_ms = float(cycle.feedback_age_ms)
+        message.qp_duration_ms = float(cycle.qp_duration_ms)
+
+        q_actual = cycle.q_actual or [0.0] * 7
+        held_target = (
+            cycle.q_target
+            or self._last_successful_action_target
+            or q_actual
+        )
+        qp_target = cycle.q_qp_raw or held_target
+        limits = np.asarray(
+            self._ik_solver.joint_position_limits,
+            dtype=float,
+        )
+        if limits.shape != (7, 2) or not np.isfinite(limits).all():
+            raise ValueError("joint limits must have finite shape (7, 2)")
+        message.q_actual = [float(value) for value in q_actual]
+        message.q_qp_raw = [float(value) for value in qp_target]
+        message.q_target = [float(value) for value in held_target]
+        message.joint_lower_limits = limits[:, 0].tolist()
+        message.joint_upper_limits = limits[:, 1].tolist()
+
+        current_pose = cycle.current_pose
+        if current_pose is None:
+            current_pose = self._debug_pose_fallback()
+        if current_pose is None:
+            current_pose = np.eye(4)
+        raw_target_pose = cycle.raw_target_pose
+        if raw_target_pose is None:
+            raw_target_pose = current_pose
+        target_pose = cycle.target_pose
+        if target_pose is None:
+            target_pose = current_pose
+        message.tcp_current = self._pose_msg(
+            message.header.stamp,
+            current_pose,
+        ).pose
+        message.tcp_raw_target = self._pose_msg(
+            message.header.stamp,
+            raw_target_pose,
+        ).pose
+        message.tcp_target = self._pose_msg(
+            message.header.stamp,
+            target_pose,
+        ).pose
+        velocity = cycle.command_velocity or [0.0] * 6
+        if len(velocity) != 6:
+            raise ValueError("ACT command velocity must contain 6 values")
+        message.tcp_command_velocity.linear.x = float(velocity[0])
+        message.tcp_command_velocity.linear.y = float(velocity[1])
+        message.tcp_command_velocity.linear.z = float(velocity[2])
+        message.tcp_command_velocity.angular.x = float(velocity[3])
+        message.tcp_command_velocity.angular.y = float(velocity[4])
+        message.tcp_command_velocity.angular.z = float(velocity[5])
+
+        controller = self._last_msg
+        if controller is not None:
+            message.pico_pose = controller.pose
+            message.pico_grip = bool(controller.grip)
+            message.pico_trigger = float(controller.trigger)
+            message.pico_primary = bool(controller.primary)
+            message.pico_secondary = bool(controller.secondary)
+            message.pico_axis = [float(value) for value in controller.axis]
+
+        tool_target, tool_state, tool_pending, tool_failed = (
+            self._tool_state_snapshot()
+        )
+        message.gripper_target_open = tool_target
+        message.gripper_state_known = tool_state is not None
+        message.gripper_state_open = bool(tool_state)
+        message.gripper_command_pending = tool_pending
+        message.gripper_command_failed = tool_failed
+
+        message.teleop_active = bool(self._active)
+        message.feedback_valid = cycle.feedback_valid
+        message.action_valid = bool(
+            self._last_successful_action_target is not None
+            and cycle.feedback_valid
+            and not cycle.send_failed
+            and not cycle.control_fault
+        )
+        message.command_sent = cycle.command_sent
+        message.qp_attempted = cycle.qp_attempted
+        message.qp_success = cycle.qp_success
+        message.target_clamped = cycle.target_clamped
+        message.control_fault = bool(
+            cycle.control_fault or self._control_fault_latched
+        )
+        return message
+
+    def _publish_act_control_sample(
+        self,
+        cycle: _ActCycleContext,
+    ) -> None:
+        publisher = getattr(self, "_act_sample_pub", None)
+        if publisher is None:
+            return
+        try:
+            publisher.publish(self._build_act_control_sample(cycle))
+        except Exception as exc:
+            self.get_logger().warn(
+                f"{self._arm_name} ACT原子样本发布失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+
     @staticmethod
     def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
         transform = _make_transform(pose[:3, 3], pose[:3, :3])