131 lines
4.7 KiB
Python
131 lines
4.7 KiB
Python
import pytest
|
|
|
|
from line_laser_modbus.client import LineLaserClient
|
|
from line_laser_modbus.config import SerialConfig
|
|
from line_laser_modbus.constants import (
|
|
ADDR_AVAILABLE_CACHE_COUNT,
|
|
ADDR_CORRECTION,
|
|
ADDR_CURRENT_POSE,
|
|
ADDR_MODE_COMMAND,
|
|
ADDR_TARGET_POSE,
|
|
)
|
|
from line_laser_modbus.models import DeviceStatus, ModeCommand, Pose6D, TimedPose6D
|
|
from line_laser_modbus.simulator import SimulatedModbusBackend
|
|
|
|
|
|
class CountingBackend(SimulatedModbusBackend):
|
|
def __init__(self, **kwargs) -> None:
|
|
super().__init__(**kwargs)
|
|
self.read_addresses: list[int] = []
|
|
|
|
def read_holding_registers(self, address: int, *, count: int, device_id: int):
|
|
self.read_addresses.append(address)
|
|
return super().read_holding_registers(address, count=count, device_id=device_id)
|
|
|
|
|
|
def test_client_reads_seeded_status_and_pose_from_simulator() -> None:
|
|
pose = Pose6D(10.0, 20.0, 30.0, 1.0, 2.0, 3.0)
|
|
backend = SimulatedModbusBackend(
|
|
status=DeviceStatus.TRACKING_OK,
|
|
current_pose=pose,
|
|
)
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
assert client.read_status() is DeviceStatus.TRACKING_OK
|
|
assert client.read_available_cache_count() == 1
|
|
assert client.read_current_pose() == pose
|
|
timed_pose = client.read_current_timed_pose()
|
|
assert timed_pose.timestamp == 0
|
|
assert timed_pose.pose == pose
|
|
|
|
|
|
def test_client_writes_mode_and_correction_to_simulator() -> None:
|
|
backend = SimulatedModbusBackend()
|
|
correction = Pose6D(1.0, 2.0, 3.0, 0.0, 1.0, 2.0)
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
client.write_mode(ModeCommand.ONLINE_TRACKING)
|
|
client.write_correction(correction)
|
|
|
|
assert client.read_mode() is ModeCommand.ONLINE_TRACKING
|
|
assert backend.correction() == correction
|
|
|
|
|
|
def test_client_rejects_emergency_stop_as_normal_mode_write() -> None:
|
|
backend = SimulatedModbusBackend()
|
|
|
|
with (
|
|
pytest.raises(ValueError, match="reserved for emergency stop"),
|
|
LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client,
|
|
):
|
|
client.write_mode(ModeCommand.EMERGENCY_STOP)
|
|
|
|
|
|
def test_client_triggers_emergency_stop_as_special_command() -> None:
|
|
backend = SimulatedModbusBackend()
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
client.trigger_emergency_stop()
|
|
|
|
assert backend.registers[ADDR_MODE_COMMAND] == ModeCommand.EMERGENCY_STOP.value
|
|
|
|
|
|
def test_snapshot_skips_pose_read_in_manual_teaching_mode() -> None:
|
|
backend = CountingBackend(mode=ModeCommand.MANUAL_TEACHING, status=DeviceStatus.IDLE)
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
snapshot = client.read_snapshot()
|
|
|
|
assert snapshot.mode is ModeCommand.MANUAL_TEACHING
|
|
assert snapshot.status is DeviceStatus.IDLE
|
|
assert snapshot.pose == Pose6D.zeros()
|
|
assert ADDR_CURRENT_POSE not in backend.read_addresses
|
|
|
|
|
|
def test_client_writes_timed_target_pose_to_simulator() -> None:
|
|
backend = SimulatedModbusBackend()
|
|
target = TimedPose6D(1234, Pose6D(1.0, 2.0, 3.0, 4.0, 5.0, 6.0))
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
client.write_target_timed_pose(target)
|
|
|
|
assert backend.target_pose() == target.pose
|
|
assert backend.registers[ADDR_TARGET_POSE] == 0x0000
|
|
assert backend.registers[ADDR_TARGET_POSE + 1] == 0x04D2
|
|
|
|
|
|
def test_client_rejects_target_pose_when_cache_is_full() -> None:
|
|
backend = SimulatedModbusBackend(available_cache_count=0)
|
|
target = TimedPose6D(1234, Pose6D(1.0, 2.0, 3.0, 4.0, 5.0, 6.0))
|
|
|
|
with (
|
|
pytest.raises(RuntimeError, match="Target pose cache is full"),
|
|
LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client,
|
|
):
|
|
client.write_target_timed_pose(target)
|
|
|
|
assert ADDR_TARGET_POSE not in backend.registers
|
|
assert backend.registers[ADDR_AVAILABLE_CACHE_COUNT] == 0
|
|
|
|
|
|
def test_client_writes_timed_correction_to_simulator() -> None:
|
|
backend = SimulatedModbusBackend()
|
|
correction = TimedPose6D(1000, Pose6D(1.0, 2.0, 3.0, 0.0, 1.0, 2.0))
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
client.write_timed_correction(correction)
|
|
|
|
assert backend.correction() == correction.pose
|
|
assert backend.registers[ADDR_CORRECTION] == 0x0000
|
|
assert backend.registers[ADDR_CORRECTION + 1] == 0x03E8
|
|
|
|
|
|
def test_simulator_rejects_wrong_slave_id() -> None:
|
|
backend = SimulatedModbusBackend()
|
|
|
|
with (
|
|
pytest.raises(ConnectionError),
|
|
LineLaserClient(SerialConfig(port="SIM", slave_id=0x09), backend=backend) as client,
|
|
):
|
|
client.read_status()
|