config: 同步双臂 TCP 与前方工作空间

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,13 +1,22 @@
 import math
 import sys
+from pathlib import Path
 from types import ModuleType, SimpleNamespace
 
 import pytest
+import yaml
 
 from xr_rm_teleop import realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
-from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame
+from xr_rm_teleop.fun_peripheral import (
+    PeripheralConfig,
+    _configure_tool_frame,
+    load_peripheral_config,
+)
+
+
+CONFIG_DIR = Path(__file__).resolve().parents[2] / "xr_rm_bringup" / "config"
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -60,6 +69,37 @@ def test_peripheral_config_exposes_selected_tool() -> None:
     assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
 
 
+def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
+    config_file = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(config_file), "left")
+    right = load_peripheral_config(str(config_file), "right")
+
+    assert left.scissorgripper == 2
+    assert left.tool_name == "minisci"
+    assert left.tool_pose == [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+    assert right.scissorgripper == 1
+    assert right.tool_name == "omnipic"
+    assert right.tool_pose == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+
+
+@pytest.mark.parametrize(
+    ("config_name", "node_names"),
+    [
+        ("left_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
+        ("right_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
+        ("dual_arm_rm75.yaml", ("left_arm_teleop", "right_arm_teleop")),
+    ],
+)
+def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> None:
+    with (CONFIG_DIR / config_name).open(encoding="utf-8") as stream:
+        config = yaml.safe_load(stream)
+
+    for node_name in node_names:
+        parameters = config[node_name]["ros__parameters"]
+        assert parameters["workspace_min"] == [-0.70, -0.70, 0.10]
+        assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
+
+
 @pytest.mark.parametrize(
     ("existing", "expected_operation"),
     [(False, "create"), (True, "update")],