class ModuleTest(unittest.TestCase):
def setUp(self):
n_section = 1
n_points = 100
lane_width = 0.4
# Create points
lines = fake_msgs.create_points(
section_count=n_section,
point_count=n_points,
offset_right=lane_width,
direction=0,
deviation=0,
)
self.right_border = Line([[0, -lane_width], [1, -2 * lane_width],
[2, -2 * lane_width], [3, -lane_width]])
self.left_border = Line([[0, lane_width], [1, 2 * lane_width],
[2, 2 * lane_width], [3, lane_width]])
# Fake section and lane msg proxies / usually in the groundtruth package
section_msg_proxy = functools.partial(fake_msgs.section_msgs,
section_count=n_section)
lane_msg_proxy = functools.partial(fake_msgs.lane_msgs, lines)
parking_msg_proxy = functools.partial(fake_msgs.parking_msgs, [], [],
self.right_border,
self.left_border)
self.speaker = AreaSpeaker(
section_proxy=section_msg_proxy,
lane_proxy=lane_msg_proxy,
parking_proxy=parking_msg_proxy,
min_wheel_count=3,
area_buffer=0.001,
)
def test_speaker_properties(self):
"""Test properties and speak function of event speaker."""
self.assertTrue(
utils.polygon_list_almost_equal(
[self.speaker.right_corridor],
[Polygon(self.speaker.middle_line, self.speaker.right_line)],
))
self.assertTrue(
utils.polygon_list_almost_equal(
[self.speaker.left_corridor],
[Polygon(self.speaker.left_line, self.speaker.middle_line)],
))
self.assertTrue(
utils.polygon_list_almost_equal(
self.speaker.parking_lots,
[
Polygon(self.right_border.get_points()),
Polygon(self.left_border.get_points()),
],
))
def test_speak_function(self):
"""Test speaker msg."""
# Car msg
pose = Pose(Point(1, 3), 0)
# Frames
frames = []
frames.append((
Polygon([
Point(0.1, 0.1),
Point(0.1, 0.3),
Point(0.5, 0.3),
Point(0.5, 0.1)
]),
SpeakerMsg.LEFT_LANE,
)) # On left side
frames.append((
Polygon([
Point(0, -0.3),
Point(0, -0.01),
Point(0.5, -0.01),
Point(0.5, -0.3)
]),
SpeakerMsg.RIGHT_LANE,
)) # On right side
frames.append((
Polygon([
Point(0, -0.3),
Point(0, 0.3),
Point(0.5, 0.3),
Point(0.5, -0.3)
]),
SpeakerMsg.LEFT_LANE,
)) # Between left and right should return left!
frames.append((
Polygon([
Point(-0.5, -0.3),
Point(-0.5, 0.3),
Point(0.5, 0.3),
Point(0.5, -0.3)
]),
SpeakerMsg.OFF_ROAD,
)) # Partly in partly out
frames.append((
Polygon([Point(10, 2),
Point(10, 3),
Point(15, 3),
Point(15, 2)]),
SpeakerMsg.OFF_ROAD,
)) # Off road
# Parking
frames.append((
Polygon(
[Point(1, 0.4),
Point(1, 0.7),
Point(2, 0.7),
Point(2, 0.4)]),
SpeakerMsg.PARKING_LOT,
)) # On left parking
frames.append((
Polygon([
Point(1, -0.4),
Point(1, -0.7),
Point(1, -0.7),
Point(2, -0.4)
]),
SpeakerMsg.PARKING_LOT,
)) # On right parking
frames.append((
Polygon([Point(1, 0),
Point(1, -0.7),
Point(2, -0.7),
Point(2, 0)]),
SpeakerMsg.PARKING_LOT,
)) # On right side and parking lot
car_msg = CarStateMsg()
car_msg.pose = pose.to_geometry_msg()
for frame, expected in frames[-1:]:
print(f"Expecting msg {expected} for frame {frame}.")
self.speaker.listen(car_msg)
self.assertTrue(
utils.assert_msgs_for_pos(self.speaker, frame, expected))
def test_line_functions(self):
"""Create a line of points as middle line and check if the following
functions/attributes work:
* get_road_lines()
* section_intervals
* middle_line
* arc_length
* current_section
"""
n_section = 10
n_points = 100
lane_width = 1
# Create points
lines = fake_msgs.create_points(
section_count=n_section, point_count=n_points, offset_right=lane_width
)
# Fake section and lane msg proxies / usually in the groundtruth package
section_msg_proxy = functools.partial(
fake_msgs.section_msgs, section_count=n_section
)
lane_msg_proxy = functools.partial(fake_msgs.lane_msgs, lines)
speaker = Speaker(section_proxy=section_msg_proxy, lane_proxy=lane_msg_proxy)
test_line = Line() # Keep track of the middle line of each section
for idx, line_tuple in enumerate(lines):
# Should return same left/middle/right line
self.assertTupleEqual(line_tuple, speaker.get_road_lines(idx))
# Test if speakers get interval works
interval = speaker.section_intervals[idx]
# Interval should start at end of previous section
self.assertAlmostEqual(interval[0], test_line.length)
test_line += line_tuple.middle
self.assertAlmostEqual(interval[1], test_line.length)
# Modify the pose
speaker.car_pose = Pose(test_line.get_points()[-1], 0) # last point of section
# Test arc_length
self.assertEqual(speaker.arc_length, test_line.length)
speaker.car_pose = Pose(
test_line.get_points()[-int(n_points / n_section / 2)], 0
) # Middle of section
# Test current section
self.assertEqual(speaker.current_section.id, idx)
def assert_line_almost_eq(line1, line2):
self.assertAlmostEqual(line1.get_points()[0], line2.get_points()[0])
self.assertAlmostEqual(line1.get_points()[-1], line2.get_points()[-1])
self.assertAlmostEqual(
Polygon(line1, line2).area / line1.length / line2.length, 0, delta=1e-3
) # The polygon should have almost no area if the lines are approx. equal
assert_line_almost_eq(
speaker.middle_line, test_line
) # The polygon should have almost no area if the lines are approx. equal
assert_line_almost_eq(
speaker.right_line, test_line.parallel_offset(lane_width, side="right")
)
assert_line_almost_eq(
speaker.left_line, test_line.parallel_offset(lane_width, side="left")
)