diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index 6a49d9e..3e47bf4 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/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"), diff --git a/xr_rm_bringup/package.xml b/xr_rm_bringup/package.xml index 2a895da..680016b 100755 --- a/xr_rm_bringup/package.xml +++ b/xr_rm_bringup/package.xml @@ -10,6 +10,7 @@ ament_cmake xr_rm_input + xr_rm_mujoco xr_rm_teleop python3-tk diff --git a/xr_rm_bringup/test/test_arm_debug_launch.py b/xr_rm_bringup/test/test_arm_debug_launch.py new file mode 100644 index 0000000..cf2046b --- /dev/null +++ b/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)