feat: 发布双臂关节状态与目标

xr_rm_teleop/test/test_joint_control.py

@@ -4,6 +4,7 @@ from types import SimpleNamespace
 
 import numpy as np
 import pytest
+from builtin_interfaces.msg import Time as TimeMsg
 
 from xr_rm_teleop.realman_adapter import JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
@@ -29,6 +30,80 @@ class FakeTime:
         del other
         return SimpleNamespace(nanoseconds=0)
 
+    def to_msg(self):
+        return TimeMsg()
+
+
+class FakePublisher:
+    def __init__(self) -> None:
+        self.messages = []
+
+    def publish(self, message) -> None:
+        self.messages.append(message)
+
+
+def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
+    names = [f"omnipic_joint_{index}" for index in range(1, 8)]
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = SimpleNamespace(
+        joint_names=names,
+        update_joint_state=lambda joints: np.eye(4),
+    )
+    teleop._joint_state_pub = FakePublisher()
+    teleop._joint_target_pub = FakePublisher()
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    return teleop
+
+
+def test_reset_joint_state_publishes_named_feedback() -> None:
+    teleop = _joint_publishing_teleop()
+    positions = [0.1 * index for index in range(7)]
+    snapshot = JointStateSnapshot(positions, time.monotonic())
+
+    teleop._reset_joint_state(snapshot)
+
+    message = teleop._joint_state_pub.messages[-1]
+    assert message.name == teleop._ik_solver.joint_names
+    assert message.position == pytest.approx(positions)
+
+
+def test_sync_joint_feedback_publishes_each_sample() -> None:
+    teleop = _joint_publishing_teleop()
+    positions = [0.2] * 7
+
+    teleop._sync_joint_feedback(
+        JointStateSnapshot(positions, time.monotonic())
+    )
+
+    assert len(teleop._joint_state_pub.messages) == 1
+    assert teleop._joint_state_pub.messages[0].position == pytest.approx(positions)
+
+
+def test_send_joint_target_publishes_limited_command() -> None:
+    sent = []
+    teleop = _joint_publishing_teleop()
+    teleop._adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sent.append((list(joints), follow))
+    )
+    teleop._follow = False
+    teleop._latest_joint_positions = [0.0] * 7
+    teleop._last_joint_command_target = [0.0] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._joint_command_max_speed = 1.0
+    teleop._joint_command_max_acceleration = 100.0
+    teleop._dt = 0.1
+
+    assert teleop._send_joint_target([0.5] * 7)
+
+    assert len(sent) == 1
+    assert sent[0][0] == pytest.approx([0.1] * 7)
+    assert sent[0][1] is False
+    message = teleop._joint_target_pub.messages[-1]
+    assert message.name == teleop._ik_solver.joint_names
+    assert message.position == pytest.approx(teleop._last_joint_command_target)
+
 
 def _primary_button_teleop(*, move_error=None):
     events = []
@@ -112,8 +187,11 @@ def test_startup_joint_query_initializes_qp_and_command_history() -> None:
         )
     )
     teleop._ik_solver = SimpleNamespace(
-        update_joint_state=lambda joints: pose
+        joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)],
+        update_joint_state=lambda joints: pose,
     )
+    teleop._joint_state_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
     teleop.get_logger = lambda: FakeLogger()
 
     teleop._initialize_joint_state()
@@ -178,6 +256,12 @@ def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
     teleop._ik_solver = SimpleNamespace(
         update_joint_state=lambda joints: np.eye(4)
     )
+    teleop._ik_solver.joint_names = [
+        f"omnipic_joint_{index}" for index in range(1, 8)
+    ]
+    teleop._joint_state_pub = FakePublisher()
+    teleop._joint_target_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
     teleop._stop_sent = False
     teleop._feedback_resync_timeout_sec = 0.5
     teleop._publish_stop_debug = lambda: None
@@ -433,6 +517,10 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
     teleop._ik_solver = SimpleNamespace(
         update_joint_state=lambda joints: np.eye(4)
     )
+    teleop._ik_solver.joint_names = [
+        f"omnipic_joint_{index}" for index in range(1, 8)
+    ]
+    teleop._joint_state_pub = FakePublisher()
     teleop._grip_rearm_required = True
     teleop._control_fault_latched = False
     teleop._feedback_resync_attempted = False
@@ -459,6 +547,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     class FakeSolver:
         def __init__(self) -> None:
             self.solve_calls = 0
+            self.joint_names = [
+                f"omnipic_joint_{index}" for index in range(1, 8)
+            ]
 
         def update_joint_state(self, joints):
             assert joints == [0.1] * 7
@@ -476,6 +567,8 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     teleop._active = False
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None
+    teleop._joint_state_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
 
     pose = teleop._sync_joint_feedback(
         JointStateSnapshot([0.1] * 7, time.monotonic())