File size: 2,903 Bytes
06c11b0 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 | import importlib.util
from pathlib import Path
from tests._shared.repo_paths import find_repo_root
def _load_module(module_name: str, relative_path: str):
repo_root = find_repo_root(__file__)
module_path = repo_root / relative_path
spec = importlib.util.spec_from_file_location(module_name, module_path)
module = importlib.util.module_from_spec(spec)
assert spec.loader is not None
spec.loader.exec_module(module)
return module
matcher_mod = _load_module(
"oracle_action_matcher_position_under_test",
"src/robomme/robomme_env/utils/oracle_action_matcher.py",
)
class _Pose:
def __init__(self, p):
self.p = p
class _Actor:
def __init__(self, name, pose_p=None, use_get_pose=False):
self.name = name
self._pose = _Pose(pose_p) if pose_p is not None else None
if not use_get_pose and self._pose is not None:
self.pose = self._pose
def get_pose(self):
if self._pose is None:
raise RuntimeError("missing pose")
return self._pose
def test_select_target_with_position_nearest_candidate():
actor_a = _Actor("A", [0.0, 0.0, 0.0])
actor_b = _Actor("B", [0.8, 0.0, 0.0])
actor_c = _Actor("C", [3.0, 0.0, 0.0])
result = matcher_mod.select_target_with_position(
available=[actor_a, actor_b, actor_c],
position_like=[1.0, 0.0, 0.0],
)
assert result is not None
assert result["name"] == "B"
assert result["selection_mode"] == "nearest_position"
assert abs(float(result["match_distance"]) - 0.2) < 1e-9
def test_select_target_with_position_skips_invalid_candidates():
actor_invalid = _Actor("invalid", None)
actor_valid = _Actor("valid", [1.0, 2.0, 3.0], use_get_pose=True)
result = matcher_mod.select_target_with_position(
available=[actor_invalid, actor_valid],
position_like=[1.2, 2.2, 3.1],
)
assert result is not None
assert result["name"] == "valid"
def test_select_target_with_position_returns_none_without_valid_input():
actor_invalid = _Actor("invalid", None)
res_invalid_position = matcher_mod.select_target_with_position(
available=[_Actor("a", [0.0, 0.0, 0.0])],
position_like=[0.0, 0.0],
)
assert res_invalid_position is None
res_invalid_candidates = matcher_mod.select_target_with_position(
available=[actor_invalid],
position_like=[0.0, 0.0, 0.0],
)
assert res_invalid_candidates is None
def test_select_target_with_position_tie_breaks_by_first_flattened_candidate():
actor_first = _Actor("first", [1.0, 0.0, 0.0])
actor_second = _Actor("second", [-1.0, 0.0, 0.0])
result = matcher_mod.select_target_with_position(
available={"left": [actor_first], "right": [actor_second]},
position_like=[0.0, 0.0, 0.0],
)
assert result is not None
assert result["name"] == "first"
|