def test_conversion_to_from_cartesian(self):
# Fixture
horiz = 0.123
vert = -0.987
v1 = LighthouseBsVector(horiz, vert)
# Test
actual = LighthouseBsVector.from_cart(v1.cart)
# Assert
self.assertAlmostEqual(horiz, actual.lh_v1_horiz_angle)
self.assertAlmostEqual(vert, actual.lh_v1_vert_angle)
def test_conversion_to_from_projection(self):
# Fixture
horiz = 0.123
vert = 0.456
v1 = LighthouseBsVector(horiz, vert)
# Test
actual = LighthouseBsVector.from_projection(v1.projection)
# Assert
self.assertAlmostEqual(horiz, actual.lh_v1_horiz_angle)
self.assertAlmostEqual(vert, actual.lh_v1_vert_angle)
def test_that_initial_yaw_guess_is_correct_bs_left_behind(self):
# Fixture
bs_vectors = [
LighthouseBsVector(1.0, 0),
LighthouseBsVector(-0.5, 0),
LighthouseBsVector(0.5, 0),
LighthouseBsVector(-1.0, 0),
]
# Test
actual = self.sut._find_initial_yaw_guess(bs_vectors)
# Assert
self.assertEqual(math.radians(155), actual)
def test_that_initial_yaw_guess_is_correct_ba_is_front(self):
# Fixture 1, 3, 2, 0
bs_vectors = [
LighthouseBsVector(3, 0),
LighthouseBsVector(0, 0),
LighthouseBsVector(2, 0),
LighthouseBsVector(1, 0),
]
# Test
actual = self.sut._find_initial_yaw_guess(bs_vectors)
# Assert
self.assertEqual(math.radians(180), actual)
def test_that_initial_yaw_guess_is_correct_ba_is_behind(self):
# Fixture
bs_vectors = [
LighthouseBsVector(1, 0),
LighthouseBsVector(2, 0),
LighthouseBsVector(0, 0),
LighthouseBsVector(3, 0),
]
# Test
actual = self.sut._find_initial_yaw_guess(bs_vectors)
# Assert
self.assertEqual(0.0, actual)
def test_conversion_to_angle_list(self):
# Fixture
vectors = LighthouseBsVectors((
LighthouseBsVector(0.0, 0.1),
LighthouseBsVector(0.2, 0.3),
LighthouseBsVector(0.4, 0.5),
LighthouseBsVector(0.6, 0.7),
))
# Test
actual = vectors.angle_list()
# Assert
self.assertVectorsAlmostEqual((0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7),
actual)
def test_conversion_to_projection_pair_list(self):
# Fixture
vectors = LighthouseBsVectors((
LighthouseBsVector(0.0, 0.0),
LighthouseBsVector(0.1, 0.1),
LighthouseBsVector(0.2, 0.2),
LighthouseBsVector(0.3, 0.3),
))
# Test
actual = vectors.projection_pair_list()
# Assert
self.assertEqual(len(vectors), len(actual))
self.assertListEqual(vectors[0].projection.tolist(),
actual[0].tolist())
self.assertListEqual(vectors[3].projection.tolist(),
actual[3].tolist())
def test_conversion_to_lh2_angles_are_equal_with_vert_zero(self):
# Fixture
horiz = 1.0
vert = 0.0
# Test
actual = LighthouseBsVector(horiz, vert)
# Assert
self.assertEqual(actual.lh_v2_angle_1, actual.lh_v2_angle_2)
def _average_sample_list(self, sample_list):
sum_horiz = 0.0
sum_vert = 0.0
for bs_vector in sample_list:
sum_horiz += bs_vector.lh_v1_horiz_angle
sum_vert += bs_vector.lh_v1_vert_angle
count = len(sample_list)
return LighthouseBsVector(sum_horiz / count, sum_vert / count)
def test_conversion_to_lh2_angles_are_zero_straight_forward(self):
# Fixture
horiz = 0
vert = 0
# Test
actual = LighthouseBsVector(horiz, vert)
# Assert
self.assertEqual(0.0, actual.lh_v2_angle_1)
self.assertEqual(0.0, actual.lh_v2_angle_2)
def test_init_from_lh1_angles(self):
# Fixture
horiz = 0.123
vert = -1.23
# Test
actual = LighthouseBsVector(horiz, vert)
# Assert
self.assertEqual(horiz, actual.lh_v1_horiz_angle)
self.assertEqual(vert, actual.lh_v1_vert_angle)
def test_cartesian_is_normalized(self):
# Fixture
horiz = 0.123
vert = 0.456
vector = LighthouseBsVector(horiz, vert)
# Test
actual = np.linalg.norm(vector.cart)
# Assert
self.assertAlmostEqual(1.0, actual)
def test_conversion_to_cartesian_straight_forward(self):
# Fixture
horiz = 0.0
vert = 0.0
vector = LighthouseBsVector(horiz, vert)
# Test
actual = vector.cart
# Assert
self.assertAlmostEqual(1.0, actual[0])
self.assertAlmostEqual(0.0, actual[1])
self.assertAlmostEqual(0.0, actual[2])
def _packet_received_cb(self, packet):
if packet.type != self._cf.loc.LH_ANGLE_STREAM:
return
if self._cb:
base_station_id = packet.data['basestation']
horiz_angles = packet.data['x']
vert_angles = packet.data['y']
result = []
for i in range(self.NR_OF_SENSORS):
result.append(
LighthouseBsVector(horiz_angles[i], vert_angles[i]))
self._cb(base_station_id, result)