49 lines
1.9 KiB
Python
49 lines
1.9 KiB
Python
from line_laser_modbus.client import LineLaserClient
|
|
from line_laser_modbus.config import SerialConfig
|
|
from line_laser_modbus.models import DeviceStatus, ModeCommand, Pose6D
|
|
from line_laser_modbus.runner import PollingRunner, pose_delta
|
|
from line_laser_modbus.simulator import SimulatedModbusBackend
|
|
|
|
|
|
def test_polling_runner_writes_tracking_correction() -> None:
|
|
backend = SimulatedModbusBackend(
|
|
mode=ModeCommand.ONLINE_TRACKING,
|
|
status=DeviceStatus.TRACKING_OK,
|
|
current_pose=Pose6D(1.0, 2.0, 3.0, 0.0, 1.0, 2.0),
|
|
)
|
|
target = Pose6D(2.0, 4.0, 6.0, 0.0, 0.0, 5.0)
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
snapshot = PollingRunner(client, correction_provider=pose_delta(target)).run_once()
|
|
|
|
assert snapshot.status is DeviceStatus.TRACKING_OK
|
|
assert snapshot.timestamp == 0
|
|
assert backend.correction() == Pose6D(1.0, 2.0, 3.0, 0.0, -1.0, 3.0)
|
|
|
|
|
|
def test_polling_runner_writes_replay_correction() -> None:
|
|
backend = SimulatedModbusBackend(
|
|
mode=ModeCommand.TRAJECTORY_REPLAY,
|
|
status=DeviceStatus.RUNNING,
|
|
current_pose=Pose6D(1.0, 2.0, 3.0, 0.0, 1.0, 2.0),
|
|
)
|
|
target = Pose6D(2.0, 4.0, 6.0, 0.0, 0.0, 5.0)
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
PollingRunner(client, correction_provider=pose_delta(target)).run_once()
|
|
|
|
assert backend.correction() == Pose6D(1.0, 2.0, 3.0, 0.0, -1.0, 3.0)
|
|
|
|
|
|
def test_polling_runner_does_not_write_correction_outside_correction_modes() -> None:
|
|
backend = SimulatedModbusBackend(current_pose=Pose6D(1.0, 2.0, 3.0, 0.0, 1.0, 2.0))
|
|
|
|
with LineLaserClient(SerialConfig(port="SIM"), backend=backend) as client:
|
|
runner = PollingRunner(
|
|
client,
|
|
correction_provider=pose_delta(Pose6D(2.0, 2.0, 2.0, 2.0, 2.0, 2.0)),
|
|
)
|
|
runner.run_once()
|
|
|
|
assert backend.correction() == Pose6D.zeros()
|