393 lines
14 KiB
Python
393 lines
14 KiB
Python
|
import threading
|
||
|
|
import time
|
||
|
|
|
||
|
|
import pytest
|
||
|
|
|
||
|
|
from lingkong_motor_service.config import Settings
|
||
|
|
from lingkong_motor_service.controller import (
|
||
|
|
MotorCommunicationError,
|
||
|
|
MotorController,
|
||
|
|
MotorNotEnabledError,
|
||
|
|
MotorState,
|
||
|
|
)
|
||
|
|
from lingkong_motor_service.protocol import CONTROL_FRAME_ID, MIXED_COMMAND_FRAME_ID, CanFrame
|
||
|
|
from lingkong_motor_service.transport import CanTransportError
|
||
|
|
|
||
|
|
from .fakes import FakeTransport
|
||
|
|
|
||
|
|
|
||
|
|
class BlockingSecondControlSendTransport(FakeTransport):
|
||
|
|
"""Hold the sender's first refresh while allowing the synchronous send."""
|
||
|
|
|
||
|
|
def __init__(self) -> None:
|
||
|
|
super().__init__()
|
||
|
|
self.background_send_entered = threading.Event()
|
||
|
|
self.release_background_send = threading.Event()
|
||
|
|
self._nonzero_sends = 0
|
||
|
|
self._count_lock = threading.Lock()
|
||
|
|
|
||
|
|
def send(self, frame: CanFrame, *, tx_queue_timeout_s: float | None = None) -> None:
|
||
|
|
if frame.arbitration_id == CONTROL_FRAME_ID and frame.data != bytes(8):
|
||
|
|
with self._count_lock:
|
||
|
|
self._nonzero_sends += 1
|
||
|
|
should_block = self._nonzero_sends == 2
|
||
|
|
if should_block:
|
||
|
|
self.background_send_entered.set()
|
||
|
|
self.release_background_send.wait(timeout=1.0)
|
||
|
|
super().send(frame, tx_queue_timeout_s=tx_queue_timeout_s)
|
||
|
|
|
||
|
|
|
||
|
|
class FailOnThirdZeroTransport(FakeTransport):
|
||
|
|
"""Fail the strict zero frame during an open-to-close transition."""
|
||
|
|
|
||
|
|
def __init__(self) -> None:
|
||
|
|
super().__init__()
|
||
|
|
self._zero_count = 0
|
||
|
|
|
||
|
|
def send(self, frame: CanFrame, *, tx_queue_timeout_s: float | None = None) -> None:
|
||
|
|
if frame.arbitration_id == CONTROL_FRAME_ID and frame.data == bytes(8):
|
||
|
|
self._zero_count += 1
|
||
|
|
if self._zero_count == 3:
|
||
|
|
raise CanTransportError("injected transition stop failure")
|
||
|
|
super().send(frame, tx_queue_timeout_s=tx_queue_timeout_s)
|
||
|
|
|
||
|
|
|
||
|
|
def enabled_settings(**overrides: object) -> Settings:
|
||
|
|
values = {
|
||
|
|
"hardware_enabled": True,
|
||
|
|
"motor_ids": (1, 2),
|
||
|
|
"broadcast_frequency_hz": 500,
|
||
|
|
}
|
||
|
|
values.update(overrides)
|
||
|
|
return Settings(**values)
|
||
|
|
|
||
|
|
|
||
|
|
def frame_dump(transport: FakeTransport) -> list[tuple[int, str]]:
|
||
|
|
with transport.lock:
|
||
|
|
return [(frame.arbitration_id, frame.data.hex()) for frame in transport.frames]
|
||
|
|
|
||
|
|
|
||
|
|
def wait_for_payload_count(transport: FakeTransport, payload: bytes, count: int) -> None:
|
||
|
|
deadline = time.monotonic() + 0.2
|
||
|
|
while time.monotonic() < deadline:
|
||
|
|
with transport.lock:
|
||
|
|
found = sum(frame.data == payload for frame in transport.frames)
|
||
|
|
if found >= count:
|
||
|
|
return
|
||
|
|
time.sleep(0.002)
|
||
|
|
raise AssertionError(f"did not observe {count} copies of {payload.hex()}")
|
||
|
|
|
||
|
|
|
||
|
|
def test_requires_enable_before_motion() -> None:
|
||
|
|
controller = MotorController(enabled_settings(), FakeTransport())
|
||
|
|
|
||
|
|
with pytest.raises(MotorNotEnabledError):
|
||
|
|
controller.open()
|
||
|
|
|
||
|
|
|
||
|
|
def test_open_and_close_can_switch_directions_without_manual_stop() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
|
||
|
|
enabled = controller.enable()
|
||
|
|
assert enabled.state is MotorState.READY
|
||
|
|
assert enabled.streaming is False
|
||
|
|
assert frame_dump(transport) == [
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8800880000000000"),
|
||
|
|
]
|
||
|
|
|
||
|
|
opening = controller.open()
|
||
|
|
assert opening.control_raw == 400
|
||
|
|
assert opening.control_raw_by_motor == {1: 400, 2: -400}
|
||
|
|
assert opening.state is MotorState.OPENING
|
||
|
|
assert opening.streaming is True
|
||
|
|
wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3)
|
||
|
|
|
||
|
|
transition_start = len(frame_dump(transport))
|
||
|
|
closing = controller.close()
|
||
|
|
assert closing.control_raw == -400
|
||
|
|
assert closing.control_raw_by_motor == {1: -400, 2: 400}
|
||
|
|
assert closing.state is MotorState.CLOSING
|
||
|
|
assert closing.streaming is True
|
||
|
|
wait_for_payload_count(transport, bytes.fromhex("70 FE 90 01 00 00 00 00"), 3)
|
||
|
|
|
||
|
|
transition_frames = frame_dump(transport)[transition_start:]
|
||
|
|
first_close = transition_frames.index((CONTROL_FRAME_ID, "70fe900100000000"))
|
||
|
|
assert (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_close]
|
||
|
|
assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_close]
|
||
|
|
assert not any(
|
||
|
|
frame == (CONTROL_FRAME_ID, "900170fe00000000") for frame in transition_frames[first_close:]
|
||
|
|
)
|
||
|
|
|
||
|
|
open_payload = bytes.fromhex("90 01 70 FE 00 00 00 00")
|
||
|
|
open_count_before = sum(frame.data == open_payload for frame in transport.frames)
|
||
|
|
transition_start = len(frame_dump(transport))
|
||
|
|
reopened = controller.open()
|
||
|
|
assert reopened.control_raw == 400
|
||
|
|
assert reopened.control_raw_by_motor == {1: 400, 2: -400}
|
||
|
|
assert reopened.state is MotorState.OPENING
|
||
|
|
assert reopened.streaming is True
|
||
|
|
wait_for_payload_count(transport, open_payload, open_count_before + 3)
|
||
|
|
|
||
|
|
transition_frames = frame_dump(transport)[transition_start:]
|
||
|
|
first_open = transition_frames.index((CONTROL_FRAME_ID, "900170fe00000000"))
|
||
|
|
assert (CONTROL_FRAME_ID, "0000000000000000") in transition_frames[:first_open]
|
||
|
|
assert (MIXED_COMMAND_FRAME_ID, "8100810000000000") in transition_frames[:first_open]
|
||
|
|
assert not any(
|
||
|
|
frame == (CONTROL_FRAME_ID, "70fe900100000000") for frame in transition_frames[first_open:]
|
||
|
|
)
|
||
|
|
|
||
|
|
stopped = controller.stop()
|
||
|
|
assert stopped.state is MotorState.STOPPED
|
||
|
|
assert stopped.streaming is False
|
||
|
|
count_after_stop = len(frame_dump(transport))
|
||
|
|
time.sleep(0.01)
|
||
|
|
assert len(frame_dump(transport)) == count_after_stop
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_open_to_close_stop_failure_never_sends_close_control() -> None:
|
||
|
|
transport = FailOnThirdZeroTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
controller.enable()
|
||
|
|
controller.open()
|
||
|
|
wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3)
|
||
|
|
|
||
|
|
with pytest.raises(MotorCommunicationError, match="transition stop failure"):
|
||
|
|
controller.close()
|
||
|
|
|
||
|
|
status = controller.status()
|
||
|
|
assert status.state is MotorState.FAULT
|
||
|
|
assert status.last_error == "injected transition stop failure"
|
||
|
|
assert not any(
|
||
|
|
frame.data == bytes.fromhex("70 FE 90 01 00 00 00 00") for frame in transport.frames
|
||
|
|
)
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
@pytest.mark.parametrize(
|
||
|
|
("motor_ids", "opening_values", "closing_values", "opening_payload", "closing_payload"),
|
||
|
|
[
|
||
|
|
((1,), {1: 400}, {1: -400}, "9001000000000000", "70fe000000000000"),
|
||
|
|
((2,), {2: -400}, {2: 400}, "000070fe00000000", "0000900100000000"),
|
||
|
|
],
|
||
|
|
)
|
||
|
|
def test_single_motor_keeps_its_configured_open_and_close_direction(
|
||
|
|
motor_ids: tuple[int, ...],
|
||
|
|
opening_values: dict[int, int],
|
||
|
|
closing_values: dict[int, int],
|
||
|
|
opening_payload: str,
|
||
|
|
closing_payload: str,
|
||
|
|
) -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(motor_ids=motor_ids), transport)
|
||
|
|
controller.enable()
|
||
|
|
|
||
|
|
assert controller.open().control_raw_by_motor == opening_values
|
||
|
|
wait_for_payload_count(transport, bytes.fromhex(opening_payload), 2)
|
||
|
|
controller.stop()
|
||
|
|
assert controller.close().control_raw_by_motor == closing_values
|
||
|
|
wait_for_payload_count(transport, bytes.fromhex(closing_payload), 2)
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_open_keeps_broadcasting_until_explicit_stop() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
controller.enable()
|
||
|
|
controller.open()
|
||
|
|
payload = bytes.fromhex("90 01 70 FE 00 00 00 00")
|
||
|
|
wait_for_payload_count(transport, payload, 3)
|
||
|
|
count_before_wait = sum(frame.data == payload for frame in transport.frames)
|
||
|
|
|
||
|
|
time.sleep(0.06)
|
||
|
|
|
||
|
|
status = controller.status()
|
||
|
|
count_after_wait = sum(frame.data == payload for frame in transport.frames)
|
||
|
|
assert status.state is MotorState.OPENING
|
||
|
|
assert status.streaming is True
|
||
|
|
assert count_after_wait > count_before_wait
|
||
|
|
assert not any(
|
||
|
|
frame.arbitration_id == MIXED_COMMAND_FRAME_ID
|
||
|
|
and frame.data == bytes.fromhex("81 00 81 00 00 00 00 00")
|
||
|
|
for frame in transport.frames
|
||
|
|
)
|
||
|
|
|
||
|
|
assert controller.stop().state is MotorState.STOPPED
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_enable_sends_zero_and_enable() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
|
||
|
|
status = controller.enable()
|
||
|
|
|
||
|
|
assert status.state is MotorState.READY
|
||
|
|
assert frame_dump(transport) == [
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8800880000000000"),
|
||
|
|
]
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_one_stop_request_sends_two_complete_rounds_with_tx_drain_wait() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
controller.enable()
|
||
|
|
start = len(transport.frames)
|
||
|
|
|
||
|
|
stopped = controller.stop()
|
||
|
|
|
||
|
|
with transport.lock:
|
||
|
|
frames = [(frame.arbitration_id, frame.data.hex()) for frame in transport.frames[start:]]
|
||
|
|
times_ns = transport.frame_times_ns[start:]
|
||
|
|
timeouts_s = transport.tx_queue_timeouts_s[start:]
|
||
|
|
assert stopped.state is MotorState.STOPPED
|
||
|
|
assert stopped.streaming is False
|
||
|
|
assert stopped.control_raw_by_motor == {1: 0, 2: 0}
|
||
|
|
assert frames == [
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8100810000000000"),
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8100810000000000"),
|
||
|
|
]
|
||
|
|
assert timeouts_s == [None, 0.1, None, 0.1]
|
||
|
|
assert times_ns[2] - times_ns[1] >= 15_000_000
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_background_send_failure_latches_fault_and_attempts_zero_stop() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
controller.enable()
|
||
|
|
controller.open()
|
||
|
|
transport.fail_next = True
|
||
|
|
|
||
|
|
deadline = time.monotonic() + 0.2
|
||
|
|
while controller.status().state is not MotorState.FAULT and time.monotonic() < deadline:
|
||
|
|
time.sleep(0.002)
|
||
|
|
|
||
|
|
assert controller.status().state is MotorState.FAULT
|
||
|
|
assert controller.status().streaming is False
|
||
|
|
while (
|
||
|
|
frame_dump(transport)[-1] != (MIXED_COMMAND_FRAME_ID, "8100810000000000")
|
||
|
|
and time.monotonic() < deadline
|
||
|
|
):
|
||
|
|
time.sleep(0.002)
|
||
|
|
assert frame_dump(transport)[-2:] == [
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8100810000000000"),
|
||
|
|
]
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_shutdown_joins_sender_before_zero_stop_disable_and_transport_shutdown() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
controller.enable()
|
||
|
|
controller.open()
|
||
|
|
wait_for_payload_count(transport, bytes.fromhex("90 01 70 FE 00 00 00 00"), 3)
|
||
|
|
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
assert frame_dump(transport)[-5:] == [
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8100810000000000"),
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8100810000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8000800000000000"),
|
||
|
|
]
|
||
|
|
assert transport.tx_queue_timeouts_s[-5:] == [None, 0.1, None, 0.1, 0.1]
|
||
|
|
assert transport.is_open is False
|
||
|
|
count_after_close = len(frame_dump(transport))
|
||
|
|
time.sleep(0.01)
|
||
|
|
assert len(frame_dump(transport)) == count_after_close
|
||
|
|
|
||
|
|
|
||
|
|
def test_send_failure_latches_local_fault() -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
transport.fail_next = True
|
||
|
|
|
||
|
|
with pytest.raises(MotorCommunicationError):
|
||
|
|
controller.enable()
|
||
|
|
|
||
|
|
assert controller.status().state is MotorState.FAULT
|
||
|
|
assert controller.status().last_error == "injected CAN failure"
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_thread_start_failure_never_sends_nonzero_control(
|
||
|
|
monkeypatch: pytest.MonkeyPatch,
|
||
|
|
) -> None:
|
||
|
|
transport = FakeTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
controller.enable()
|
||
|
|
|
||
|
|
def fail_start(thread: threading.Thread) -> None:
|
||
|
|
del thread
|
||
|
|
raise RuntimeError("injected thread start failure")
|
||
|
|
|
||
|
|
monkeypatch.setattr(threading.Thread, "start", fail_start)
|
||
|
|
|
||
|
|
with pytest.raises(MotorCommunicationError, match="could not start"):
|
||
|
|
controller.open()
|
||
|
|
|
||
|
|
assert controller.status().state is MotorState.FAULT
|
||
|
|
assert not any(
|
||
|
|
frame.arbitration_id == CONTROL_FRAME_ID and frame.data != bytes(8)
|
||
|
|
for frame in transport.frames
|
||
|
|
)
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_join_timeout_still_ends_with_sender_zero_and_stop(
|
||
|
|
monkeypatch: pytest.MonkeyPatch,
|
||
|
|
) -> None:
|
||
|
|
transport = BlockingSecondControlSendTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
monkeypatch.setattr(controller, "_BROADCAST_JOIN_TIMEOUT_S", 0.01)
|
||
|
|
controller.enable()
|
||
|
|
controller.open()
|
||
|
|
assert transport.background_send_entered.wait(timeout=0.2)
|
||
|
|
|
||
|
|
with pytest.raises(MotorCommunicationError, match="did not stop"):
|
||
|
|
controller.stop()
|
||
|
|
|
||
|
|
transport.release_background_send.set()
|
||
|
|
deadline = time.monotonic() + 0.2
|
||
|
|
while frame_dump(transport)[-1] != (MIXED_COMMAND_FRAME_ID, "8100810000000000"):
|
||
|
|
if time.monotonic() >= deadline:
|
||
|
|
raise AssertionError("sender did not finish with zero and Stop")
|
||
|
|
time.sleep(0.002)
|
||
|
|
|
||
|
|
assert frame_dump(transport)[-2:] == [
|
||
|
|
(CONTROL_FRAME_ID, "0000000000000000"),
|
||
|
|
(MIXED_COMMAND_FRAME_ID, "8100810000000000"),
|
||
|
|
]
|
||
|
|
controller.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
def test_shutdown_does_not_close_transport_while_sender_is_still_alive(
|
||
|
|
monkeypatch: pytest.MonkeyPatch,
|
||
|
|
) -> None:
|
||
|
|
transport = BlockingSecondControlSendTransport()
|
||
|
|
controller = MotorController(enabled_settings(), transport)
|
||
|
|
monkeypatch.setattr(controller, "_BROADCAST_JOIN_TIMEOUT_S", 0.01)
|
||
|
|
controller.enable()
|
||
|
|
controller.open()
|
||
|
|
assert transport.background_send_entered.wait(timeout=0.2)
|
||
|
|
|
||
|
|
with pytest.raises(MotorCommunicationError, match="did not stop"):
|
||
|
|
controller.shutdown()
|
||
|
|
assert transport.is_open is True
|
||
|
|
|
||
|
|
transport.release_background_send.set()
|
||
|
|
deadline = time.monotonic() + 0.2
|
||
|
|
while controller._broadcast_thread is not None and time.monotonic() < deadline:
|
||
|
|
time.sleep(0.002)
|
||
|
|
controller.shutdown()
|
||
|
|
assert transport.is_open is False
|