123 lines
3.2 KiB
Python
123 lines
3.2 KiB
Python
import math
|
|
from types import SimpleNamespace
|
|
|
|
from builtin_interfaces.msg import Time
|
|
from xr_rm_input.udp_controller_receiver import UdpControllerReceiver
|
|
from xr_rm_input.xrobotoolkit_to_udp_bridge import (
|
|
_buttons_payload,
|
|
_controller_payload,
|
|
_stop_controller_payload,
|
|
)
|
|
|
|
|
|
def _receiver_without_socket() -> UdpControllerReceiver:
|
|
receiver = object.__new__(UdpControllerReceiver)
|
|
receiver._quat_order = "xyzw"
|
|
receiver.get_clock = lambda: SimpleNamespace(
|
|
now=lambda: SimpleNamespace(to_msg=lambda: Time())
|
|
)
|
|
return receiver
|
|
|
|
|
|
def test_bridge_payload_contains_only_selected_controller_inputs() -> None:
|
|
buttons = _buttons_payload(
|
|
primary=lambda: True,
|
|
secondary=lambda: False,
|
|
)
|
|
payload = _controller_payload(
|
|
hand="left",
|
|
pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0],
|
|
axis=[2.0, -2.0],
|
|
buttons=buttons,
|
|
grip_pressed=True,
|
|
trigger_pressed=False,
|
|
)
|
|
|
|
assert payload == {
|
|
"hand": "left",
|
|
"grip": True,
|
|
"trigger": 0.0,
|
|
"pos": [1.0, 2.0, 3.0],
|
|
"quat": [0.0, 0.0, 0.0, 1.0],
|
|
"pose_valid": True,
|
|
"pose_source": "xrobotoolkit",
|
|
"axis": [1.0, -1.0],
|
|
"buttons": {
|
|
"primary": True,
|
|
"secondary": False,
|
|
},
|
|
}
|
|
|
|
|
|
def test_stop_payload_uses_neutral_selected_inputs() -> None:
|
|
payload = _stop_controller_payload("right")
|
|
|
|
assert payload["axis"] == [0.0, 0.0]
|
|
assert payload["buttons"] == {
|
|
"primary": False,
|
|
"secondary": False,
|
|
}
|
|
assert "grip_value" not in payload
|
|
assert "trigger_value" not in payload
|
|
|
|
|
|
def test_receiver_publishes_selected_controller_inputs() -> None:
|
|
msg = _receiver_without_socket()._payload_to_msg(
|
|
{
|
|
"grip": True,
|
|
"trigger": 1.0,
|
|
"axis": [2.0, -2.0],
|
|
"buttons": {
|
|
"primary": True,
|
|
"secondary": False,
|
|
},
|
|
"pos": [1.0, 2.0, 3.0],
|
|
"quat": [0.0, 0.0, 0.0, 1.0],
|
|
},
|
|
"left",
|
|
)
|
|
|
|
assert msg.primary is True
|
|
assert msg.secondary is False
|
|
assert list(msg.axis) == [1.0, -1.0]
|
|
|
|
|
|
def test_receiver_defaults_invalid_optional_inputs() -> None:
|
|
msg = _receiver_without_socket()._payload_to_msg(
|
|
{
|
|
"grip": True,
|
|
"trigger": 0.4,
|
|
"axis": [math.nan, 0.0],
|
|
"buttons": [],
|
|
"pos": [1.0, 2.0, 3.0],
|
|
"quat": [0.0, 0.0, 0.0, 1.0],
|
|
},
|
|
"right",
|
|
)
|
|
|
|
assert msg.primary is False
|
|
assert msg.secondary is False
|
|
assert list(msg.axis) == [0.0, 0.0]
|
|
assert msg.grip is True
|
|
assert abs(msg.trigger - 0.4) < 1e-6
|
|
assert msg.pose.position.x == 1.0
|
|
assert msg.pose.position.y == 2.0
|
|
assert msg.pose.position.z == 3.0
|
|
|
|
|
|
def test_receiver_defaults_missing_legacy_optional_inputs() -> None:
|
|
msg = _receiver_without_socket()._payload_to_msg(
|
|
{
|
|
"grip": True,
|
|
"trigger": 0.0,
|
|
"pos": [0.0, 1.0, 0.0],
|
|
"quat": [0.0, 0.0, 0.0, 1.0],
|
|
},
|
|
"left",
|
|
)
|
|
|
|
assert msg.primary is False
|
|
assert msg.secondary is False
|
|
assert list(msg.axis) == [0.0, 0.0]
|
|
assert msg.grip is True
|