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

xr_rm_teleop/package.xml

@@ -11,6 +11,7 @@
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>xr_rm_interfaces</exec_depend>

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())

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -149,6 +149,10 @@ class PlacoIkSolver:
         self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
+    @property
+    def joint_names(self) -> list[str]:
+        return list(self._joint_names)
+
     @property
     def base_configuration(self) -> list[float]:
         return self._robot.state.q[:7].tolist()

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -17,6 +17,7 @@ import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
 from rclpy.time import Time
+from sensor_msgs.msg import JointState
 from std_msgs.msg import Bool
 
 from xr_rm_interfaces.msg import XrController
@@ -327,12 +328,22 @@ class SingleArmVelocityTeleop(Node):
             self._dt,
             peripheral_arm,
         )
+        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
+        self._joint_state_pub = self.create_publisher(
+            JointState,
+            f"{debug_ns}/joint_states",
+            10,
+        )
+        self._joint_target_pub = self.create_publisher(
+            JointState,
+            f"{debug_ns}/joint_target",
+            10,
+        )
         self._adapter = self._make_adapter()
         self._adapter.connect()
         self._initialize_joint_state()
         self._setup_tool_control()
 
-        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
         self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
         self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
@@ -393,6 +404,13 @@ class SingleArmVelocityTeleop(Node):
             self._adapter.close()
             raise
 
+    def _publish_joint_positions(self, publisher, positions: list[float]) -> None:
+        message = JointState()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.name = self._ik_solver.joint_names
+        message.position = [float(value) for value in positions]
+        publisher.publish(message)
+
     def _reset_joint_state(
         self,
         snapshot: JointStateSnapshot,
@@ -406,6 +424,7 @@ class SingleArmVelocityTeleop(Node):
         self._last_valid_joint_target = list(positions)
         self._last_joint_command_target = list(positions)
         self._last_joint_command_velocity = [0.0] * 7
+        self._publish_joint_positions(self._joint_state_pub, positions)
         return current_pose
 
     def _setup_tool_control(self) -> None:
@@ -1166,6 +1185,10 @@ class SingleArmVelocityTeleop(Node):
         )
         self._latest_joint_positions = list(snapshot.positions)
         self._last_current_pose = current_pose
+        self._publish_joint_positions(
+            self._joint_state_pub,
+            list(snapshot.positions),
+        )
         if not self._active or self._last_valid_joint_target is None:
             self._last_valid_joint_target = list(snapshot.positions)
         return current_pose
@@ -1254,6 +1277,10 @@ class SingleArmVelocityTeleop(Node):
             return False
         self._last_joint_command_target = limited_target
         self._last_joint_command_velocity = limited_velocity
+        self._publish_joint_positions(
+            self._joint_target_pub,
+            limited_target,
+        )
         return True
 
     @staticmethod