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