42 lines
941 B
Python
42 lines
941 B
Python
#!/usr/bin/env python3
|
|
"""Test UGV control logic."""
|
|
|
|
import pytest
|
|
import numpy as np
|
|
|
|
|
|
def test_differential_drive():
|
|
target_angle = np.pi / 4
|
|
current_yaw = 0
|
|
|
|
angle_error = target_angle - current_yaw
|
|
kp_angular = 1.5
|
|
max_angular = 1.0
|
|
|
|
angular_cmd = np.clip(kp_angular * angle_error, -max_angular, max_angular)
|
|
assert np.isclose(angular_cmd, 1.0)
|
|
|
|
|
|
def test_angle_normalization():
|
|
def normalize(angle):
|
|
while angle > np.pi:
|
|
angle -= 2 * np.pi
|
|
while angle < -np.pi:
|
|
angle += 2 * np.pi
|
|
return angle
|
|
|
|
assert np.isclose(normalize(3 * np.pi), np.pi)
|
|
assert np.isclose(normalize(-3 * np.pi), np.pi)
|
|
|
|
|
|
def test_goal_distance_2d():
|
|
current = np.array([0, 0])
|
|
target = np.array([3, 4])
|
|
|
|
distance = np.linalg.norm(target - current)
|
|
assert np.isclose(distance, 5.0)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
pytest.main([__file__, '-v'])
|