diff --git a/xr_rm_teleop/package.xml b/xr_rm_teleop/package.xml
index a2a9fc9..5994fb8 100755
--- a/xr_rm_teleop/package.xml
+++ b/xr_rm_teleop/package.xml
@@ -11,6 +11,7 @@
geometry_msgs
rclpy
+ sensor_msgs
python3-yaml
std_msgs
xr_rm_interfaces
diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py
index 25c68f1..0332452 100644
--- a/xr_rm_teleop/test/test_joint_control.py
+++ b/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())
diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
index 69f6842..098f9bf 100644
--- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
+++ b/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()
diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
index f5731ae..597ab88 100755
--- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+++ b/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