""" P1 关节数据 & 资源跟随桥接测试 — 全面覆盖 HostNode 关节回调 + resource_pose 回调的边缘 case。 不依赖 ROS2 运行时，通过 mock 模拟 msg 和 bridge。 测试分组: E1: JointRepublisher JSON 输出格式 (已修复 str→json.dumps) E2: 关节状态回调 — 从 /joint_states (JointState msg) 直接读取 name/position E3: 资源跟随 (resource_pose) — 夹爪抓取/释放/多资源 E4: 联合流程 — 关节 + 资源一并通过 bridge 发送 E5: Bridge 调用验证 E6: 同类型设备多实例 — 重复关节名场景 E7: 吞吐优化 — 死区过滤、抑频、增量 resource_poses """ import json import time import pytest from unittest.mock import MagicMock from types import SimpleNamespace from typing import Dict, Optional # ==================== 辅助: 模拟 JointState msg ==================== def _make_joint_state_msg(names: list, positions: list, velocities=None, efforts=None): """构造模拟的 sensor_msgs/JointState 消息（不依赖 ROS2）""" msg = SimpleNamespace() msg.name = names msg.position = positions msg.velocity = velocities or [0.0] * len(names) msg.effort = efforts or [0.0] * len(names) return msg def _make_string_msg(data: str): """构造模拟的 std_msgs/String 消息""" msg = SimpleNamespace() msg.data = data return msg # ==================== 辅助: 提取 HostNode 核心逻辑用于隔离测试 ==================== class JointBridgeSimulator: """ 模拟 HostNode 的关节桥接核心逻辑（提取自 host_node.py）， 不依赖 ROS2 Node、subscription 等基础设施。 包含吞吐优化逻辑: - 死区过滤 (dead band): 关节变化 < 阈值时不发送 - 抑频 (throttle): 限制最大发送频率 - 增量 resource_poses: 仅在变化时附带 """ JOINT_DEAD_BAND: float = 1e-4 JOINT_MIN_INTERVAL: float = 0.05 # 秒 def __init__(self, device_uuid_map: Dict[str, str], dead_band: Optional[float] = None, min_interval: Optional[float] = None): self.device_uuid_map = device_uuid_map self._device_ids_sorted = sorted(device_uuid_map.keys(), key=len, reverse=True) self._resource_poses: Dict[str, str] = {} self._resource_poses_dirty: bool = False self._last_joint_values: Dict[str, float] = {} self._last_send_time: float = -float("inf") # 确保首条消息总是通过 # 允许测试覆盖优化参数 if dead_band is not None: self.JOINT_DEAD_BAND = dead_band if min_interval is not None: self.JOINT_MIN_INTERVAL = min_interval def resource_pose_callback(self, msg) -> None: """模拟 HostNode._resource_pose_callback（含变化检测）""" try: data = json.loads(msg.data) except (json.JSONDecodeError, ValueError): return if not isinstance(data, dict) or not data: return has_change = False for k, v in data.items(): if self._resource_poses.get(k) != v: has_change = True break if has_change: self._resource_poses.update(data) self._resource_poses_dirty = True def joint_state_callback(self, msg, now: Optional[float] = None) -> dict: """ 模拟 HostNode._joint_state_callback 核心逻辑（含优化）。 now 参数允许测试控制时间。 返回 {device_id: {"node_uuid": ..., "joint_states": {...}, "resource_poses": {...}}}。 返回 {} 表示被优化过滤。 """ names = list(msg.name) positions = list(msg.position) if not names or len(names) != len(positions): return {} if now is None: now = time.time() resource_dirty = self._resource_poses_dirty # 抑频检查 if not resource_dirty and (now - self._last_send_time) < self.JOINT_MIN_INTERVAL: return {} # 死区过滤 has_significant_change = False for name, pos in zip(names, positions): last_val = self._last_joint_values.get(name) if last_val is None or abs(float(pos) - last_val) >= self.JOINT_DEAD_BAND: has_significant_change = True break if not has_significant_change and not resource_dirty: return {} # 更新状态 for name, pos in zip(names, positions): self._last_joint_values[name] = float(pos) self._last_send_time = now # 按设备 ID 分组关节数据 device_joints: Dict[str, Dict[str, float]] = {} for name, pos in zip(names, positions): matched_device = None for device_id in self._device_ids_sorted: if name.startswith(device_id + "_"): matched_device = device_id break if matched_device: if matched_device not in device_joints: device_joints[matched_device] = {} device_joints[matched_device][name] = float(pos) elif len(self.device_uuid_map) == 1: fallback_id = self._device_ids_sorted[0] if fallback_id not in device_joints: device_joints[fallback_id] = {} device_joints[fallback_id][name] = float(pos) # 构建设备级 resource_poses（仅 dirty 时附带） device_resource_poses: Dict[str, Dict[str, str]] = {} if resource_dirty: for resource_id, link_name in self._resource_poses.items(): matched_device = None for device_id in self._device_ids_sorted: if link_name.startswith(device_id + "_"): matched_device = device_id break if matched_device: if matched_device not in device_resource_poses: device_resource_poses[matched_device] = {} device_resource_poses[matched_device][resource_id] = link_name elif len(self.device_uuid_map) == 1: fallback_id = self._device_ids_sorted[0] if fallback_id not in device_resource_poses: device_resource_poses[fallback_id] = {} device_resource_poses[fallback_id][resource_id] = link_name self._resource_poses_dirty = False result = {} for device_id, joint_states in device_joints.items(): node_uuid = self.device_uuid_map.get(device_id) if not node_uuid: continue result[device_id] = { "node_uuid": node_uuid, "joint_states": joint_states, "resource_poses": device_resource_poses.get(device_id, {}), } return result # 功能测试中禁用优化（dead_band=0, min_interval=0），确保逻辑正确性 def _make_sim(device_uuid_map: Dict[str, str]) -> JointBridgeSimulator: """创建禁用吞吐优化的模拟器（用于功能正确性测试）""" return JointBridgeSimulator(device_uuid_map, dead_band=0.0, min_interval=0.0) # ==================== E1: JointRepublisher JSON 输出 ==================== class TestJointRepublisherFormat: """验证 JointRepublisher 输出标准 JSON（双引号）而非 Python repr（单引号）""" def test_output_is_valid_json(self): """str() 产生单引号，json.dumps() 产生双引号""" joint_dict = { "name": ["joint1", "joint2"], "position": [0.1, 0.2], "velocity": [0.0, 0.0], "effort": [0.0, 0.0], } result = json.dumps(joint_dict) parsed = json.loads(result) assert parsed["name"] == ["joint1", "joint2"] assert parsed["position"] == [0.1, 0.2] assert "'" not in result def test_str_produces_invalid_json(self): """对比: str() 不是合法 JSON""" joint_dict = {"name": ["joint1"], "position": [0.1]} result = str(joint_dict) with pytest.raises(json.JSONDecodeError): json.loads(result) # ==================== E2: 关节状态回调（JointState msg 直接读取）==================== class TestJointStateCallback: """测试从 JointState msg 直接读取 name/position 的分组逻辑""" def test_single_device_simple(self): """单设备，关节名有设备前缀""" sim = _make_sim({"panda": "uuid-panda"}) msg = _make_joint_state_msg( ["panda_joint1", "panda_joint2"], [0.5, 1.0] ) result = sim.joint_state_callback(msg) assert "panda" in result assert result["panda"]["joint_states"]["panda_joint1"] == 0.5 assert result["panda"]["joint_states"]["panda_joint2"] == 1.0 def test_single_device_no_prefix_fallback(self): """单设备，关节名无设备前缀 → 应归入唯一设备""" sim = _make_sim({"robot1": "uuid-r1"}) msg = _make_joint_state_msg(["joint_a", "joint_b"], [1.0, 2.0]) result = sim.joint_state_callback(msg) assert "robot1" in result assert result["robot1"]["joint_states"]["joint_a"] == 1.0 assert result["robot1"]["joint_states"]["joint_b"] == 2.0 def test_multi_device_distinct_prefixes(self): """多设备，不同前缀，正确分组""" sim = _make_sim({"arm1": "uuid-arm1", "arm2": "uuid-arm2"}) msg = _make_joint_state_msg( ["arm1_j1", "arm1_j2", "arm2_j1", "arm2_j2"], [0.1, 0.2, 0.3, 0.4], ) result = sim.joint_state_callback(msg) assert result["arm1"]["joint_states"]["arm1_j1"] == 0.1 assert result["arm1"]["joint_states"]["arm1_j2"] == 0.2 assert result["arm2"]["joint_states"]["arm2_j1"] == 0.3 assert result["arm2"]["joint_states"]["arm2_j2"] == 0.4 def test_ambiguous_prefix_longest_wins(self): """前缀歧义: arm 和 arm_left — 最长前缀优先""" sim = _make_sim({"arm": "uuid-arm", "arm_left": "uuid-arm-left"}) msg = _make_joint_state_msg( ["arm_j1", "arm_left_j1", "arm_left_j2"], [0.1, 0.2, 0.3], ) result = sim.joint_state_callback(msg) assert result["arm"]["joint_states"]["arm_j1"] == 0.1 assert result["arm_left"]["joint_states"]["arm_left_j1"] == 0.2 assert result["arm_left"]["joint_states"]["arm_left_j2"] == 0.3 def test_multi_device_unmatched_joints_dropped(self): """多设备时，无法匹配前缀的关节应被丢弃（不 fallback）""" sim = _make_sim({"arm1": "uuid-arm1", "arm2": "uuid-arm2"}) msg = _make_joint_state_msg( ["arm1_j1", "unknown_j1"], [0.1, 0.9], ) result = sim.joint_state_callback(msg) assert result["arm1"]["joint_states"]["arm1_j1"] == 0.1 for device_id, data in result.items(): assert "unknown_j1" not in data["joint_states"] def test_empty_names(self): """空 name 列表""" sim = _make_sim({"dev": "uuid-dev"}) msg = _make_joint_state_msg([], []) result = sim.joint_state_callback(msg) assert result == {} def test_mismatched_lengths(self): """name 和 position 长度不一致""" sim = _make_sim({"dev": "uuid-dev"}) msg = _make_joint_state_msg(["j1", "j2"], [0.1]) result = sim.joint_state_callback(msg) assert result == {} def test_no_devices(self): """无设备 UUID 映射""" sim = _make_sim({}) msg = _make_joint_state_msg(["j1"], [0.1]) result = sim.joint_state_callback(msg) assert result == {} def test_numeric_prefix_device_ids(self): """数字化设备 ID (如 deck1, deck12) — deck12_slot1 不应匹配 deck1""" sim = _make_sim({"deck1": "uuid-d1", "deck12": "uuid-d12"}) msg = _make_joint_state_msg( ["deck1_slot1", "deck12_slot1"], [1.0, 2.0], ) result = sim.joint_state_callback(msg) assert result["deck1"]["joint_states"]["deck1_slot1"] == 1.0 assert result["deck12"]["joint_states"]["deck12_slot1"] == 2.0 def test_position_float_conversion(self): """position 值应强制转为 float（即使输入为 int）""" sim = _make_sim({"arm": "uuid-arm"}) msg = _make_joint_state_msg(["arm_j1"], [1]) result = sim.joint_state_callback(msg) assert result["arm"]["joint_states"]["arm_j1"] == 1.0 assert isinstance(result["arm"]["joint_states"]["arm_j1"], float) def test_node_uuid_in_result(self): """结果中应携带正确的 node_uuid""" sim = _make_sim({"panda": "uuid-panda-123"}) msg = _make_joint_state_msg(["panda_j1"], [0.5]) result = sim.joint_state_callback(msg) assert result["panda"]["node_uuid"] == "uuid-panda-123" def test_device_with_no_uuid_skipped(self): """device_uuid_map 中存在映射但值为空 → 跳过""" sim = _make_sim({"arm": ""}) msg = _make_joint_state_msg(["arm_j1"], [0.5]) result = sim.joint_state_callback(msg) assert result == {} def test_many_joints_single_device(self): """单设备大量关节（如 7-DOF arm）""" sim = _make_sim({"panda": "uuid-panda"}) names = [f"panda_joint{i}" for i in range(1, 8)] positions = [float(i) * 0.1 for i in range(1, 8)] msg = _make_joint_state_msg(names, positions) result = sim.joint_state_callback(msg) assert len(result["panda"]["joint_states"]) == 7 assert result["panda"]["joint_states"]["panda_joint7"] == pytest.approx(0.7) def test_duplicate_joint_names_last_wins(self): """同类型设备多个实例时，如果关节名完全重复（bug 场景），后出现的值覆盖前者""" sim = _make_sim({"dev": "uuid-dev"}) msg = _make_joint_state_msg(["dev_j1", "dev_j1"], [1.0, 2.0]) result = sim.joint_state_callback(msg) assert result["dev"]["joint_states"]["dev_j1"] == 2.0 def test_negative_positions(self): """关节角度为负数""" sim = _make_sim({"arm": "uuid-arm"}) msg = _make_joint_state_msg(["arm_j1", "arm_j2"], [-1.57, -3.14]) result = sim.joint_state_callback(msg) assert result["arm"]["joint_states"]["arm_j1"] == pytest.approx(-1.57) assert result["arm"]["joint_states"]["arm_j2"] == pytest.approx(-3.14) # ==================== E3: 资源跟随 (resource_pose) ==================== class TestResourcePoseCallback: """测试 resource_pose 回调 — 夹爪抓取/释放/多资源""" def test_single_resource_attach(self): """单个资源挂载到夹爪 link""" sim = _make_sim({"panda": "uuid-panda"}) msg = _make_string_msg(json.dumps({"plate_1": "panda_gripper_link"})) sim.resource_pose_callback(msg) assert sim._resource_poses == {"plate_1": "panda_gripper_link"} assert sim._resource_poses_dirty is True def test_multiple_resource_attach(self): """多个资源同时挂载到不同 link""" sim = _make_sim({"panda": "uuid-panda"}) msg = _make_string_msg(json.dumps({ "plate_1": "panda_gripper_link", "tip_rack": "panda_deck_link", })) sim.resource_pose_callback(msg) assert sim._resource_poses["plate_1"] == "panda_gripper_link" assert sim._resource_poses["tip_rack"] == "panda_deck_link" def test_incremental_update(self): """增量更新：新消息合并到已有状态""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_deck_link"}))) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_2": "panda_gripper_link"}))) assert len(sim._resource_poses) == 2 assert sim._resource_poses["plate_1"] == "panda_deck_link" assert sim._resource_poses["plate_2"] == "panda_gripper_link" def test_resource_reattach(self): """资源从 deck 移动到 gripper（抓取操作）""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_deck_link"}))) assert sim._resource_poses["plate_1"] == "panda_deck_link" sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_gripper_link"}))) assert sim._resource_poses["plate_1"] == "panda_gripper_link" def test_resource_release_back_to_world(self): """释放资源回到 world""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_gripper_link"}))) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "world"}))) assert sim._resource_poses["plate_1"] == "world" def test_empty_dict_heartbeat_no_dirty(self): """空 dict（心跳包）不标记 dirty""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_link"}))) sim._resource_poses_dirty = False # 重置 sim.resource_pose_callback(_make_string_msg(json.dumps({}))) assert sim._resource_poses_dirty is False # 空 dict 不应标记 dirty def test_same_value_no_dirty(self): """重复发送相同值不应标记 dirty""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_link"}))) sim._resource_poses_dirty = False sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_link"}))) assert sim._resource_poses_dirty is False def test_invalid_json_ignored(self): """非法 JSON 消息不影响状态""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate_1": "panda_link"}))) sim.resource_pose_callback(_make_string_msg("not valid json {{{")) assert sim._resource_poses["plate_1"] == "panda_link" def test_non_dict_json_ignored(self): """JSON 但不是 dict（如 list）应被忽略""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps(["not", "a", "dict"]))) assert sim._resource_poses == {} def test_python_repr_ignored(self): """Python repr 格式（单引号）应被忽略""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg("{'plate_1': 'panda_link'}")) assert sim._resource_poses == {} def test_multi_device_resource_attach(self): """多设备场景：不同设备的 link 挂载不同资源""" sim = _make_sim({"arm1": "uuid-arm1", "arm2": "uuid-arm2"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_A": "arm1_gripper_link", "plate_B": "arm2_gripper_link", }))) assert sim._resource_poses["plate_A"] == "arm1_gripper_link" assert sim._resource_poses["plate_B"] == "arm2_gripper_link" # ==================== E4: 联合流程 — 关节 + 资源一并通过 bridge ==================== class TestJointWithResourcePoses: """测试关节状态回调时，resource_poses 被正确按设备分组并包含在结果中""" def test_single_device_joint_with_resource(self): """单设备：关节更新时携带已挂载的资源""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_1": "panda_gripper_link", }))) msg = _make_joint_state_msg(["panda_j1", "panda_j2"], [0.5, 1.0]) result = sim.joint_state_callback(msg) assert result["panda"]["resource_poses"] == {"plate_1": "panda_gripper_link"} def test_single_device_no_resource(self): """单设备：无资源挂载时 resource_poses 为空 dict""" sim = _make_sim({"panda": "uuid-panda"}) msg = _make_joint_state_msg(["panda_j1"], [0.5]) result = sim.joint_state_callback(msg) assert result["panda"]["resource_poses"] == {} def test_multi_device_resource_routing(self): """多设备：资源按 link 前缀路由到正确设备""" sim = _make_sim({"arm1": "uuid-arm1", "arm2": "uuid-arm2"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_A": "arm1_gripper_link", "plate_B": "arm2_gripper_link", "tube_1": "arm1_tool_link", }))) msg = _make_joint_state_msg( ["arm1_j1", "arm2_j1"], [0.1, 0.2], ) result = sim.joint_state_callback(msg) assert result["arm1"]["resource_poses"] == { "plate_A": "arm1_gripper_link", "tube_1": "arm1_tool_link", } assert result["arm2"]["resource_poses"] == {"plate_B": "arm2_gripper_link"} def test_resource_on_world_frame_not_routed(self): """资源挂在 world frame（已释放）— 多设备时无法匹配任何设备前缀""" sim = _make_sim({"arm1": "uuid-arm1", "arm2": "uuid-arm2"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_A": "world", }))) msg = _make_joint_state_msg(["arm1_j1"], [0.1]) result = sim.joint_state_callback(msg) assert result["arm1"]["resource_poses"] == {} def test_resource_world_frame_single_device_fallback(self): """单设备时 world frame 的资源走 fallback""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_A": "world", }))) msg = _make_joint_state_msg(["panda_j1"], [0.1]) result = sim.joint_state_callback(msg) assert result["panda"]["resource_poses"] == {"plate_A": "world"} def test_grab_and_move_sequence(self): """完整夹取序列: 资源在 deck → gripper 抓取 → arm 移动 → 放下""" sim = _make_sim({"panda": "uuid-panda"}) # 初始: plate 在 deck sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_1": "panda_deck_third_link", }))) msg = _make_joint_state_msg( ["panda_j1", "panda_j2", "panda_j3"], [0.0, -0.5, 1.0], ) result = sim.joint_state_callback(msg) assert result["panda"]["resource_poses"]["plate_1"] == "panda_deck_third_link" # 抓取: plate 从 deck → gripper sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_1": "panda_gripper_link", }))) msg = _make_joint_state_msg( ["panda_j1", "panda_j2", "panda_j3"], [1.57, 0.0, -0.5], ) result = sim.joint_state_callback(msg) assert result["panda"]["resource_poses"]["plate_1"] == "panda_gripper_link" assert result["panda"]["joint_states"]["panda_j1"] == pytest.approx(1.57) # 放下: plate 从 gripper → 目标 deck sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_1": "panda_deck_first_link", }))) msg = _make_joint_state_msg( ["panda_j1", "panda_j2", "panda_j3"], [0.0, 0.0, 0.0], ) result = sim.joint_state_callback(msg) assert result["panda"]["resource_poses"]["plate_1"] == "panda_deck_first_link" def test_simultaneous_grab_multiple_resources(self): """同时持有多个资源（如双夹爪）""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_1": "panda_left_gripper", "plate_2": "panda_right_gripper", "tip_rack": "panda_deck_link", }))) msg = _make_joint_state_msg(["panda_j1"], [0.5]) result = sim.joint_state_callback(msg) assert len(result["panda"]["resource_poses"]) == 3 def test_resource_with_ambiguous_link_prefix(self): """link 前缀歧义: arm_left_gripper 应匹配 arm_left 而非 arm""" sim = _make_sim({"arm": "uuid-arm", "arm_left": "uuid-arm-left"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_A": "arm_gripper_link", "plate_B": "arm_left_gripper_link", }))) msg = _make_joint_state_msg( ["arm_j1", "arm_left_j1"], [0.1, 0.2], ) result = sim.joint_state_callback(msg) assert result["arm"]["resource_poses"] == {"plate_A": "arm_gripper_link"} assert result["arm_left"]["resource_poses"] == {"plate_B": "arm_left_gripper_link"} # ==================== E5: Bridge 调用验证 ==================== class TestBridgeCalls: """验证完整桥接流: callback → bridge.publish_joint_state 调用""" def test_bridge_called_per_device(self): """每个设备调用一次 publish_joint_state""" device_uuid_map = {"arm1": "uuid-111", "arm2": "uuid-222"} sim = _make_sim(device_uuid_map) bridge = MagicMock() bridge.publish_joint_state = MagicMock() msg = _make_joint_state_msg( ["arm1_j1", "arm2_j1"], [1.0, 2.0], ) result = sim.joint_state_callback(msg) for device_id, data in result.items(): bridge.publish_joint_state( data["node_uuid"], data["joint_states"], data["resource_poses"] ) assert bridge.publish_joint_state.call_count == 2 call_uuids = {c[0][0] for c in bridge.publish_joint_state.call_args_list} assert call_uuids == {"uuid-111", "uuid-222"} def test_bridge_called_with_resource_poses(self): """bridge 调用时携带 resource_poses""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_1": "panda_gripper_link", }))) bridge = MagicMock() msg = _make_joint_state_msg(["panda_j1"], [0.5]) result = sim.joint_state_callback(msg) for device_id, data in result.items(): bridge.publish_joint_state( data["node_uuid"], data["joint_states"], data["resource_poses"] ) bridge.publish_joint_state.assert_called_once_with( "uuid-panda", {"panda_j1": 0.5}, {"plate_1": "panda_gripper_link"}, ) def test_bridge_no_call_for_empty_joints(self): """无关节数据时不调用 bridge""" sim = _make_sim({"panda": "uuid-panda"}) bridge = MagicMock() msg = _make_joint_state_msg([], []) result = sim.joint_state_callback(msg) for device_id, data in result.items(): bridge.publish_joint_state( data["node_uuid"], data["joint_states"], data["resource_poses"] ) bridge.publish_joint_state.assert_not_called() def test_bridge_resource_poses_empty_when_no_resources(self): """无资源挂载时，resource_poses 参数为空 dict""" sim = _make_sim({"panda": "uuid-panda"}) bridge = MagicMock() msg = _make_joint_state_msg(["panda_j1"], [0.5]) result = sim.joint_state_callback(msg) for device_id, data in result.items(): bridge.publish_joint_state( data["node_uuid"], data["joint_states"], data["resource_poses"] ) bridge.publish_joint_state.assert_called_once_with( "uuid-panda", {"panda_j1": 0.5}, {}, ) def test_multi_bridge_all_called(self): """多个 bridge 都应被调用""" sim = _make_sim({"arm": "uuid-arm"}) bridges = [MagicMock(), MagicMock()] msg = _make_joint_state_msg(["arm_j1"], [0.5]) result = sim.joint_state_callback(msg) for device_id, data in result.items(): for bridge in bridges: bridge.publish_joint_state( data["node_uuid"], data["joint_states"], data["resource_poses"] ) for bridge in bridges: bridge.publish_joint_state.assert_called_once() # ==================== E6: 同类型设备多个实例 — 重复关节名场景 ==================== class TestDuplicateDeviceTypes: """ 多个同类型设备（如 2 个 OT-2 移液器），关节名格式为 {device_id}_{joint_name}。 设备 ID 不同（如 ot2_left, ot2_right），但底层关节名相同（如 pipette_j1）。 """ def test_same_type_different_id(self): """同类型设备不同 ID""" sim = _make_sim({ "ot2_left": "uuid-ot2-left", "ot2_right": "uuid-ot2-right", }) msg = _make_joint_state_msg( ["ot2_left_pipette_j1", "ot2_left_pipette_j2", "ot2_right_pipette_j1", "ot2_right_pipette_j2"], [0.1, 0.2, 0.3, 0.4], ) result = sim.joint_state_callback(msg) assert result["ot2_left"]["joint_states"]["ot2_left_pipette_j1"] == 0.1 assert result["ot2_left"]["joint_states"]["ot2_left_pipette_j2"] == 0.2 assert result["ot2_right"]["joint_states"]["ot2_right_pipette_j1"] == 0.3 assert result["ot2_right"]["joint_states"]["ot2_right_pipette_j2"] == 0.4 def test_same_type_with_resources_routed_correctly(self): """同类型设备各自抓取资源，按 link 前缀正确路由""" sim = _make_sim({ "ot2_left": "uuid-ot2-left", "ot2_right": "uuid-ot2-right", }) sim.resource_pose_callback(_make_string_msg(json.dumps({ "plate_A": "ot2_left_gripper", "plate_B": "ot2_right_gripper", }))) msg = _make_joint_state_msg( ["ot2_left_j1", "ot2_right_j1"], [0.5, 0.6], ) result = sim.joint_state_callback(msg) assert result["ot2_left"]["resource_poses"] == {"plate_A": "ot2_left_gripper"} assert result["ot2_right"]["resource_poses"] == {"plate_B": "ot2_right_gripper"} def test_numbered_devices_no_confusion(self): """编号设备: robot1 不应匹配 robot10 的关节""" sim = _make_sim({ "robot1": "uuid-r1", "robot10": "uuid-r10", }) msg = _make_joint_state_msg( ["robot1_j1", "robot10_j1"], [1.0, 10.0], ) result = sim.joint_state_callback(msg) assert result["robot1"]["joint_states"]["robot1_j1"] == 1.0 assert result["robot10"]["joint_states"]["robot10_j1"] == 10.0 def test_three_same_type_devices(self): """三个同类型设备""" sim = _make_sim({ "pump_a": "uuid-pa", "pump_b": "uuid-pb", "pump_c": "uuid-pc", }) msg = _make_joint_state_msg( ["pump_a_flow", "pump_b_flow", "pump_c_flow", "pump_a_pressure", "pump_b_pressure"], [1.0, 2.0, 3.0, 0.1, 0.2], ) result = sim.joint_state_callback(msg) assert len(result["pump_a"]["joint_states"]) == 2 assert len(result["pump_b"]["joint_states"]) == 2 assert len(result["pump_c"]["joint_states"]) == 1 # ==================== E7: 吞吐优化测试 ==================== class TestThroughputOptimizations: """测试死区过滤、抑频、增量 resource_poses 等优化行为""" # --- 死区过滤 (Dead Band) --- def test_dead_band_filters_tiny_change(self): """关节变化小于死区阈值 → 被过滤""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.01, min_interval=0.0) msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) result1 = sim.joint_state_callback(msg1, now=0.0) assert "arm" in result1 # 微小变化 (0.001 < 0.01 死区) msg2 = _make_joint_state_msg(["arm_j1"], [1.001]) result2 = sim.joint_state_callback(msg2, now=1.0) assert result2 == {} def test_dead_band_passes_significant_change(self): """关节变化大于死区阈值 → 通过""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.01, min_interval=0.0) msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) sim.joint_state_callback(msg1, now=0.0) msg2 = _make_joint_state_msg(["arm_j1"], [1.05]) result2 = sim.joint_state_callback(msg2, now=1.0) assert "arm" in result2 assert result2["arm"]["joint_states"]["arm_j1"] == pytest.approx(1.05) def test_dead_band_first_message_always_passes(self): """首次消息总是通过（无历史值）""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=1000.0, min_interval=0.0) msg = _make_joint_state_msg(["arm_j1"], [0.001]) result = sim.joint_state_callback(msg, now=0.0) assert "arm" in result def test_dead_band_any_joint_change_triggers(self): """多关节中只要有一个超过死区就全部发送""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.01, min_interval=0.0) msg1 = _make_joint_state_msg(["arm_j1", "arm_j2"], [1.0, 2.0]) sim.joint_state_callback(msg1, now=0.0) # j1 微变化，j2 大变化 msg2 = _make_joint_state_msg(["arm_j1", "arm_j2"], [1.001, 2.5]) result2 = sim.joint_state_callback(msg2, now=1.0) assert "arm" in result2 # 两个关节的值都应包含在结果中 assert result2["arm"]["joint_states"]["arm_j1"] == pytest.approx(1.001) assert result2["arm"]["joint_states"]["arm_j2"] == pytest.approx(2.5) # --- 抑频 (Throttle) --- def test_throttle_filters_rapid_messages(self): """发送间隔内的消息被过滤""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.0, min_interval=0.1) msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) result1 = sim.joint_state_callback(msg1, now=0.0) assert "arm" in result1 # 0.05s < 0.1s 间隔 msg2 = _make_joint_state_msg(["arm_j1"], [2.0]) result2 = sim.joint_state_callback(msg2, now=0.05) assert result2 == {} def test_throttle_passes_after_interval(self): """超过发送间隔后消息通过""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.0, min_interval=0.1) msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) sim.joint_state_callback(msg1, now=0.0) msg2 = _make_joint_state_msg(["arm_j1"], [2.0]) result2 = sim.joint_state_callback(msg2, now=0.15) assert "arm" in result2 def test_throttle_bypassed_by_resource_change(self): """resource_pose 变化时忽略抑频限制""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.0, min_interval=1.0) msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) sim.joint_state_callback(msg1, now=0.0) # 资源变化 → 强制发送 sim.resource_pose_callback(_make_string_msg(json.dumps({"plate": "arm_gripper"}))) msg2 = _make_joint_state_msg(["arm_j1"], [1.0]) result2 = sim.joint_state_callback(msg2, now=0.01) # 远小于 1.0 间隔 assert "arm" in result2 assert result2["arm"]["resource_poses"] == {"plate": "arm_gripper"} # --- 增量 resource_poses --- def test_resource_poses_only_sent_when_dirty(self): """resource_poses 仅在 dirty 时附带，否则为空""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate": "panda_gripper"}))) # 第一次发送：dirty → 携带 resource_poses msg1 = _make_joint_state_msg(["panda_j1"], [0.5]) result1 = sim.joint_state_callback(msg1) assert result1["panda"]["resource_poses"] == {"plate": "panda_gripper"} # dirty 已清除 assert sim._resource_poses_dirty is False # 第二次发送：not dirty → resource_poses 为空 msg2 = _make_joint_state_msg(["panda_j1"], [1.0]) result2 = sim.joint_state_callback(msg2) assert result2["panda"]["resource_poses"] == {} def test_resource_change_resets_dirty_after_send(self): """dirty 在发送后被重置，再次 resource_pose 变化后重新标记""" sim = _make_sim({"panda": "uuid-panda"}) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate": "panda_deck"}))) msg = _make_joint_state_msg(["panda_j1"], [0.5]) sim.joint_state_callback(msg) assert sim._resource_poses_dirty is False # 再次资源变化 sim.resource_pose_callback(_make_string_msg(json.dumps({"plate": "panda_gripper"}))) assert sim._resource_poses_dirty is True msg2 = _make_joint_state_msg(["panda_j1"], [1.0]) result2 = sim.joint_state_callback(msg2) assert result2["panda"]["resource_poses"] == {"plate": "panda_gripper"} # --- 组合场景 --- def test_dead_band_bypassed_by_resource_dirty(self): """关节无变化但 resource_pose 有变化 → 仍然发送""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.01, min_interval=0.0) msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) sim.joint_state_callback(msg1, now=0.0) sim.resource_pose_callback(_make_string_msg(json.dumps({"plate": "arm_gripper"}))) # 关节值完全不变 msg2 = _make_joint_state_msg(["arm_j1"], [1.0]) result2 = sim.joint_state_callback(msg2, now=1.0) assert "arm" in result2 assert result2["arm"]["resource_poses"] == {"plate": "arm_gripper"} def test_high_frequency_stream_only_significant_pass(self): """模拟高频流: 只有显著变化的消息通过""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.01, min_interval=0.0) t = 0.0 passed_count = 0 # 100 条消息，每条微小递增 0.001 for i in range(100): t += 0.1 val = 1.0 + i * 0.001 msg = _make_joint_state_msg(["arm_j1"], [val]) result = sim.joint_state_callback(msg, now=t) if result: passed_count += 1 # 首次总通过 + 每 10 条左右（累计 0.01 变化）通过一次 assert passed_count < 20 # 远少于 100 assert passed_count >= 5 # 但不应为 0 def test_throttle_and_dead_band_combined(self): """同时受抑频和死区影响""" sim = JointBridgeSimulator({"arm": "uuid-arm"}, dead_band=0.01, min_interval=0.5) # 首条通过 msg1 = _make_joint_state_msg(["arm_j1"], [1.0]) assert sim.joint_state_callback(msg1, now=0.0) != {} # 时间不够 + 变化不够 → 过滤 msg2 = _make_joint_state_msg(["arm_j1"], [1.001]) assert sim.joint_state_callback(msg2, now=0.1) == {} # 时间够但变化不够 → 过滤 msg3 = _make_joint_state_msg(["arm_j1"], [1.002]) assert sim.joint_state_callback(msg3, now=1.0) == {} # 时间够且变化够 → 通过 msg4 = _make_joint_state_msg(["arm_j1"], [1.05]) assert sim.joint_state_callback(msg4, now=1.5) != {}