feat: 接入双臂 MuJoCo 启动模式

xr_rm_bringup/launch/arm_debug.launch.py

@@ -57,6 +57,26 @@ def _udp_receiver_node() -> Node:
     )
 
 
+def _mujoco_node() -> Node:
+    """启动只读双臂 MuJoCo 运动学显示节点。"""
+    return Node(
+        package="xr_rm_mujoco",
+        executable="dual_arm_simulator",
+        name="dual_arm_simulator",
+        output="screen",
+        prefix=[XR_PYTHON],
+        parameters=[
+            _config_file("dual_arm_mujoco.yaml"),
+            {"robot_urdf_path": _dual_rm75_urdf()},
+        ],
+    )
+
+
+def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None:
+    if use_mujoco and arm != "both":
+        raise ValueError("use_mujoco:=true requires arm:=both")
+
+
 def _single_arm_node(
     arm: str,
     use_mock: bool,
@@ -138,15 +158,21 @@ def _launch_setup(context, *args, **kwargs):
         )
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
+    use_mujoco = _as_bool(
+        LaunchConfiguration("use_mujoco").perform(context)
+    )
 
     if arm not in ("left", "right", "both"):
         raise ValueError("arm must be one of: left, right, both")
+    _validate_mujoco_mode(arm, use_mujoco)
 
     nodes = [_udp_receiver_node()]
     if arm == "both":
         nodes.extend(_dual_arm_nodes(use_mock))
     else:
         nodes.append(_single_arm_node(arm, use_mock))
+    if use_mujoco:
+        nodes.append(_mujoco_node())
     return nodes
 
 
@@ -156,6 +182,8 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("arm", default_value="right"),
         # true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。
         DeclareLaunchArgument("use_mock", default_value="true"),
+        # true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
+        DeclareLaunchArgument("use_mujoco", default_value="false"),
         # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),

xr_rm_bringup/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <exec_depend>xr_rm_input</exec_depend>
+  <exec_depend>xr_rm_mujoco</exec_depend>
   <exec_depend>xr_rm_teleop</exec_depend>
   <exec_depend>python3-tk</exec_depend>
 

xr_rm_bringup/test/test_arm_debug_launch.py

@@ -0,0 +1,37 @@
+import importlib.util
+from pathlib import Path
+
+import pytest
+from launch import LaunchContext
+from launch.actions import DeclareLaunchArgument
+from launch.utilities import perform_substitutions
+
+
+MODULE_PATH = Path(__file__).parents[1] / "launch" / "arm_debug.launch.py"
+SPEC = importlib.util.spec_from_file_location("arm_debug_launch", MODULE_PATH)
+arm_debug_launch = importlib.util.module_from_spec(SPEC)
+assert SPEC.loader is not None
+SPEC.loader.exec_module(arm_debug_launch)
+
+
+def test_launch_declares_mujoco_disabled_by_default() -> None:
+    description = arm_debug_launch.generate_launch_description()
+    arguments = {
+        entity.name: entity
+        for entity in description.entities
+        if isinstance(entity, DeclareLaunchArgument)
+    }
+
+    assert "use_mujoco" in arguments
+    assert perform_substitutions(
+        LaunchContext(),
+        arguments["use_mujoco"].default_value,
+    ) == "false"
+
+
+def test_mujoco_mode_requires_both_arms() -> None:
+    arm_debug_launch._validate_mujoco_mode("both", True)
+    arm_debug_launch._validate_mujoco_mode("left", False)
+
+    with pytest.raises(ValueError, match="arm:=both"):
+        arm_debug_launch._validate_mujoco_mode("left", True)