feat: 添加双臂 MuJoCo 显示节点

xr_rm_mujoco/setup.py

@@ -20,4 +20,9 @@ setup(
     description="MuJoCo kinematic visualization for the dual RM75 platform.",
     license="Apache-2.0",
     tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "dual_arm_simulator = xr_rm_mujoco.dual_arm_simulator:main",
+        ],
+    },
 )

xr_rm_mujoco/test/test_dual_arm_simulator.py

@@ -15,6 +15,9 @@ URDF_PATH = (
     SRC_DIR / "xr_rm_teleop" / "models" / "dual_rm75" / "Dual_arm.urdf"
 )
 DUAL_CONFIG_PATH = SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_rm75.yaml"
+MUJOCO_CONFIG_PATH = (
+    SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_mujoco.yaml"
+)
 
 
 def test_dual_urdf_loads_with_expected_joint_mapping() -> None:
@@ -60,6 +63,13 @@ def test_yaml_initial_poses_populate_both_arms() -> None:
     assert simulation.ready
 
 
+def test_mujoco_config_contains_only_render_parameters() -> None:
+    with MUJOCO_CONFIG_PATH.open(encoding="utf-8") as stream:
+        parameters = yaml.safe_load(stream)["dual_arm_simulator"]["ros__parameters"]
+
+    assert parameters == {"render_rate_hz": 60.0}
+
+
 @pytest.mark.parametrize(
     ("names", "positions", "match"),
     [

xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py

@@ -4,14 +4,23 @@ from __future__ import annotations
 
 import math
 from pathlib import Path
+from typing import Callable
 
 import mujoco
+import mujoco.viewer
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
 
 
 ARM_JOINT_NAMES = {
     "left": tuple(f"scissor_joint_{index}" for index in range(1, 8)),
     "right": tuple(f"omnipic_joint_{index}" for index in range(1, 8)),
 }
+STATE_TOPICS = {
+    "left": "/xr_rm/left_rm75/joint_states",
+    "right": "/xr_rm/right_rm75/joint_states",
+}
 
 
 class DualArmKinematicModel:
@@ -84,3 +93,108 @@ class DualArmKinematicModel:
             float(self.data.qpos[self._qpos_addresses[arm][name]])
             for name in ARM_JOINT_NAMES[arm]
         ]
+
+
+class DualArmSimulator(Node):
+    """订阅左右关节反馈并刷新一个 MuJoCo 双臂 viewer。"""
+
+    def __init__(
+        self,
+        viewer_factory: Callable = mujoco.viewer.launch_passive,
+    ) -> None:
+        super().__init__("dual_arm_simulator")
+        self.declare_parameter("robot_urdf_path", "")
+        self.declare_parameter("render_rate_hz", 60.0)
+
+        render_rate_hz = float(self.get_parameter("render_rate_hz").value)
+        if not math.isfinite(render_rate_hz) or render_rate_hz <= 0.0:
+            raise ValueError("render_rate_hz must be finite and > 0")
+
+        self._kinematics = DualArmKinematicModel(
+            str(self.get_parameter("robot_urdf_path").value)
+        )
+        self._viewer_factory = viewer_factory
+        self._viewer = None
+        self._subscriptions = [
+            self.create_subscription(
+                JointState,
+                topic,
+                lambda message, selected_arm=arm: self._on_joint_state(
+                    selected_arm,
+                    message,
+                ),
+                10,
+            )
+            for arm, topic in STATE_TOPICS.items()
+        ]
+        self.create_timer(1.0 / render_rate_hz, self._render)
+        self.get_logger().info(
+            "MuJoCo 双臂节点已启动，等待左右关节状态，"
+            f"render_rate_hz={render_rate_hz:.1f}"
+        )
+
+    def _on_joint_state(self, arm: str, message: JointState) -> None:
+        try:
+            if self._viewer is None:
+                self._kinematics.apply_arm_state(
+                    arm,
+                    list(message.name),
+                    list(message.position),
+                )
+            else:
+                with self._viewer.lock():
+                    self._kinematics.apply_arm_state(
+                        arm,
+                        list(message.name),
+                        list(message.position),
+                    )
+        except (RuntimeError, ValueError) as exc:
+            self.get_logger().warn(
+                f"拒绝 {arm} 关节状态：{exc}",
+                throttle_duration_sec=1.0,
+            )
+
+    def _render(self) -> None:
+        for topic in STATE_TOPICS.values():
+            publisher_count = self.count_publishers(topic)
+            if publisher_count > 1:
+                self.get_logger().warn(
+                    f"关节状态话题存在多个发布者：{topic}, count={publisher_count}",
+                    throttle_duration_sec=5.0,
+                )
+
+        if not self._kinematics.ready:
+            return
+        if self._viewer is None:
+            self._viewer = self._viewer_factory(
+                self._kinematics.model,
+                self._kinematics.data,
+            )
+        if not self._viewer.is_running():
+            self.get_logger().info("MuJoCo viewer 已关闭。")
+            rclpy.shutdown()
+            return
+        self._viewer.sync()
+
+    def close_viewer(self) -> None:
+        if self._viewer is not None:
+            self._viewer.close()
+            self._viewer = None
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = DualArmSimulator()
+        rclpy.spin(node)
+    finally:
+        if node is not None:
+            node.close_viewer()
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()