def test_listen_func(self):
"""Test if car state msg is processed correctly.
* listen()
"""
# Dummy msg
pose = Pose(Point(1, 3), math.pi / 3)
frame = Polygon([Point(0, -0.3), Point(0, 0.3), Point(0.5, 0.3), Point(0.5, -0.3)])
linear_twist = Vector(2, 1, 3)
car_msg = gazebo_sim_msgs.CarState()
car_msg.pose = pose.to_geometry_msg()
car_msg.frame = frame.to_geometry_msg()
car_msg.twist = geometry_msgs.Twist()
car_msg.twist.linear = linear_twist.to_geometry_msg()
speaker = Speaker(
section_proxy=fake_msgs.section_msgs, lane_proxy=fake_msgs.lane_msgs
)
speaker.listen(car_msg)
# Test return values
self.assertEqual(speaker.car_frame, frame)
self.assertEqual(speaker.car_pose, pose)
self.assertEqual(speaker.car_speed, abs(linear_twist))
def _get_stop_line(line1: Line, line2: Line, kind) -> SurfaceMarkingRect:
"""Return a line perpendicular to both provided (assumed parallel) lines.
The returned line will be at the first point where both lines are parallel to each other plus 2cm offset.
"""
beginning_line1 = line1.interpolate(0.02)
beginning_line2 = line2.interpolate(0.02)
# Test which line to draw the stop line at
if beginning_line1.distance(line2) < beginning_line2.distance(line1):
# End of line 1 is the starting point of the stop line
p1 = beginning_line1
p2 = line2.interpolate(line2.project(beginning_line1))
else:
# End of line 2 is the starting point
p1 = line1.interpolate(line1.project(beginning_line2))
p2 = beginning_line2
line = Line([p1, p2])
width = line.length
center = 0.5 * (Vector(line.coords[0]) + Vector(line.coords[1]))
angle = Pose([0, 0],
Vector(line.coords[1]) - Vector(line.coords[0])).get_angle()
return SurfaceMarkingRect(kind=kind,
angle=angle,
width=width,
normalize_x=False,
center=center,
depth=0.04)
def test_road_section(self):
MIDDLE_LINE = Line([[0, 0], [1, 0]])
DummyRoadSection.MIDDLE_LINE = MIDDLE_LINE
LEFT_LINE = Line([[0, Config.road_width], [1, Config.road_width]])
RIGHT_LINE = Line([[0, -Config.road_width],
[1 + Config.road_width, -Config.road_width]])
ENDING = (Pose([1, 0], 0), 0)
BEGINNING = (Pose([0, 0], math.pi), 0)
rs = DummyRoadSection()
self.assertIsNotNone(rs.transform)
self.assert_lines_approx_equal(rs.middle_line, MIDDLE_LINE)
self.assert_lines_approx_equal(rs.right_line, RIGHT_LINE)
self.assert_lines_approx_equal(rs.left_line, LEFT_LINE)
self.assertTupleEqual(rs.get_beginning(), BEGINNING)
self.assertTupleEqual(rs.get_ending(), ENDING)
self.assertTrue(rs.get_bounding_box().contains(LEFT_LINE))
self.assertTrue(rs.get_bounding_box().contains(MIDDLE_LINE))
self.assertTrue(rs.get_bounding_box().contains(RIGHT_LINE))
def test_speak_function(self):
"""Test the speakers msgs by putting the car somewhere and checking the output."""
msg_options = dict()
msg_options[SpeakerMsg.START_ZONE] = {(0, 0.5)}
msg_options[SpeakerMsg.DRIVING_ZONE] = {(0.5, 99.5)}
msg_options[SpeakerMsg.END_ZONE] = {(99.5, 100)}
msg_options[SpeakerMsg.OVERTAKING_ZONE] = self.overtaking_intervals
msg_options[
SpeakerMsg.NO_OVERTAKING_ZONE] = self.inv_overtaking_intervals
msg_options[SpeakerMsg.PARKING_ZONE] = {(80, 100)}
msg_options[SpeakerMsg.NO_PARKING_ZONE] = {(0, 80)}
msg_options[SpeakerMsg.HALT_ZONE] = {(20 + self.yield_distance[0],
20 + self.yield_distance[1])}
msg_options[SpeakerMsg.STOP_ZONE] = {(30 + self.yield_distance[0],
30 + self.yield_distance[1])}
msg_options[SpeakerMsg.NO_STOP_ZONE] = {
(0, 20 + self.yield_distance[0]),
(20 + self.yield_distance[1], 30 + self.yield_distance[0]),
(30 + self.yield_distance[1], 100),
}
msg_options[SpeakerMsg.SPEED_UNLIMITED_ZONE] = {(0, 100)}
msg_expects = {
(SpeakerMsg.START_ZONE, SpeakerMsg.DRIVING_ZONE,
SpeakerMsg.END_ZONE),
(SpeakerMsg.OVERTAKING_ZONE, SpeakerMsg.NO_OVERTAKING_ZONE),
(SpeakerMsg.PARKING_ZONE, SpeakerMsg.NO_PARKING_ZONE),
(SpeakerMsg.HALT_ZONE, SpeakerMsg.STOP_ZONE,
SpeakerMsg.NO_STOP_ZONE),
}
for x in numpy.arange(0, 100, 0.1):
# create car msg
self.speaker.car_pose = Pose([x, 0], 0)
result: List[SpeakerMsg] = self.speaker.speak()
msg_types = {msg.type for msg in result}
# Check that each of the expected msgs is in what the speaker says
for exp in msg_expects:
self.assertTrue(len(set(exp) & msg_types) > 0)
for msg in msg_types:
opts = msg_options[msg]
# Check if thats correct
self.assertTrue(
any(interval[0] <= x and interval[1] >= x
for interval in opts))
def save_section(self):
"""Save the current custom section to a file."""
if len(self.section.middle_line_points) > 1:
# Adjust initial position to zero.
initial_pose = Pose(self.section.middle_line.interpolate_pose(0))
tf = Transform(initial_pose.position,
initial_pose.orientation).inverse
self.section.middle_line_points = [
tf * p for p in self.section.middle_line_points
]
self.section.save_as_yaml(self.param.file_path)
def get_current_pose(self):
"""Try to get the current pose from /tf."""
try:
tf_transform = self.listener.lookup_transform(
"odom",
"vehicle",
rospy.Time.now(),
timeout=rospy.Duration(0.01))
return Pose(tf_transform.transform)
except Exception as e:
rospy.logerr(f"Could not lookup transform {e}")
return
def get_ending(self) -> Tuple[Pose, float]:
if self.turn == Intersection.LEFT:
end_angle = math.pi / 2 + self._alpha
end_point = Point(self.z - self.w)
elif self.turn == Intersection.RIGHT:
end_angle = -math.pi / 2 + self._alpha
end_point = Point(self.z + self.w)
elif self.turn == Intersection.STRAIGHT:
end_angle = 0
end_point = Point(2 * self.z)
return (Pose(self.transform * end_point,
self.transform.get_angle() + end_angle), 0)
def close_loop(self, p_curvature: float = 2):
"""Append a road section that connects the last section to the beginning.
The road's beginning and it's end are connected using a cubic bezier curve.
Args:
p_curvature: Scale the curvature of the resulting loop.
"""
# Global position of the end of the road
end_pose_global, _ = self.sections[-1].get_ending()
# Distance to start / p_curvature
approximate_radius = abs(end_pose_global.position) / p_curvature
section = RoadSection.fit_ending(
end_pose_global, Pose([0, 0], 0), approximate_radius
)
self.append(section)
def get_ending(self) -> Tuple[Pose, float]:
"""Get the ending of the section as a pose and the curvature.
Returns:
A tuple consisting of the last point on the middle line together with \
the direction facing along the middle line as a pose and the curvature \
at the ending of the middle line.
"""
_, curvature = super().get_ending()
radius = (-1 if self.__class__.TYPE
== road_section_type.RIGHT_CIRCULAR_ARC else 1) * self.radius
pose = self.transform * Pose(
Point(
math.cos(self.angle - math.pi / 2) * abs(radius),
radius + math.sin(self.angle - math.pi / 2) * radius,
),
-self.angle if self.__class__.TYPE
== road_section_type.RIGHT_CIRCULAR_ARC else self.angle,
)
return (pose, curvature)
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 listen(self, msg: CarStateMsg):
"""Receive information about current observations and update internal values."""
# Store car frame
self.car_frame = Polygon(msg.frame)
self.car_pose = Pose(msg.pose)
self.car_speed = abs(Vector(msg.twist.linear))
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")
)
def test_overlapping_inside_funcs(self):
"""Test polygon functions:
* get_interval_for_polygon()
* car_is_inside()
* car_overlaps_with()
* wheel_count_inside()
"""
# Dummy msg
pose = Pose(Point(1, 3), math.pi / 3)
frame = Polygon(
[Point(0.1, -0.3), Point(0.1, 0.3), Point(0.5, 0.3), Point(0.5, -0.3)]
)
# Fake section and lane msg proxies / usually in the groundtruth package
section_msg_proxy = functools.partial(fake_msgs.section_msgs, section_count=2)
lane_msg_proxy = functools.partial(
fake_msgs.lane_msgs,
fake_msgs.create_points(
section_count=2, point_dist=0.1, point_count=40, deviation=0
),
)
speaker = Speaker(section_proxy=section_msg_proxy, lane_proxy=lane_msg_proxy)
speaker.car_pose = pose
speaker.car_frame = frame
polygon1 = Polygon(
[Point(0, -0.8), Point(0, 0.8), Point(0.8, 0.8), Point(0.8, -0.8)]
) # Contains frame completely
polygon2 = Polygon(
[Point(0, 0), Point(0, 0.8), Point(0.8, 0.8), Point(0.8, 0)]
) # Intersects with frame
# Intersects with frame
polygon3 = Polygon([Point(0, 0), Point(0, -0.8), Point(0.8, -0.8), Point(0.8, 0)])
# p2 and p3 make up p1 together
polygon4 = Polygon([Point(1, 1), Point(1, 2), Point(3, 2)]) # Disjoint to frame
def assertTupleAlmostEqual(tuple1, tuple2):
for t1, t2 in zip(tuple1, tuple2):
self.assertAlmostEqual(t1, t2)
assertTupleAlmostEqual(speaker.get_interval_for_polygon(polygon1), (0, 0.8))
self.assertTrue(speaker.car_is_inside(polygon1))
self.assertTrue(speaker.car_overlaps_with(polygon1))
self.assertEqual(speaker.wheel_count_inside(polygon1), 4)
assertTupleAlmostEqual(speaker.get_interval_for_polygon(polygon2), (0, 0.8))
self.assertFalse(speaker.car_is_inside(polygon2))
self.assertTrue(speaker.car_overlaps_with(polygon2))
self.assertEqual(speaker.wheel_count_inside(polygon2), 2)
assertTupleAlmostEqual(speaker.get_interval_for_polygon(polygon3), (0, 0.8))
self.assertFalse(speaker.car_is_inside(polygon3))
self.assertTrue(speaker.car_overlaps_with(polygon3))
self.assertEqual(speaker.wheel_count_inside(polygon3), 2)
assertTupleAlmostEqual(
speaker.get_interval_for_polygon(polygon2, polygon3), (0, 0.8)
)
self.assertTrue(speaker.car_is_inside(polygon2, polygon3))
self.assertTrue(speaker.car_overlaps_with(polygon2, polygon3))
self.assertEqual(speaker.wheel_count_inside(polygon2, polygon3), 2)
self.assertEqual(speaker.wheel_count_inside(polygon2, polygon3, in_total=True), 4)
assertTupleAlmostEqual(speaker.get_interval_for_polygon(polygon4), (1, 3))
self.assertFalse(speaker.car_is_inside(polygon4))
self.assertFalse(speaker.car_overlaps_with(polygon4))
self.assertEqual(speaker.wheel_count_inside(polygon4), 0)
def get_beginning(self) -> Tuple[Pose, float]:
return (Pose(self.transform * Point(0, 0),
self.transform.get_angle() + math.pi), 0)
def update(self):
"""Calculate and publish new car state information."""
# Update the driving state
current_time = rospy.Time.now().to_sec()
current_speed = self.param.speed
d_time = current_time - self._driving_state.time
if (
len(self.speeds) > 1
and self.speeds[1][0] <= self._driving_state.distance_driven
):
# If we reach a new speed zone we delete the old one
del self.speeds[0]
# Set the current speed
current_speed = self.speeds[0][1] if len(self.speeds) > 0 else self.param.speed
current_speed /= 36 # km/h to m/s and model car scale of 1/10
# Check if the car needs to stop
remaining_stops = self.driving_line[1]
if len(remaining_stops) > 0:
if remaining_stops[0][0] < self._driving_state.distance_driven:
remaining_stops[0][1] -= d_time
if remaining_stops[0][1] > 0:
current_speed = 0
else:
del remaining_stops[0]
self._driving_state.distance_driven += d_time * current_speed
if (
not self.param.loop
and self._driving_state.distance_driven > self.driving_line[0].length
):
rospy.signal_shutdown("Finished driving along the road.")
return
self._driving_state.distance_driven %= self.driving_line[0].length
self._driving_state.time = current_time
rospy.logdebug(f"Current driving state: {self._driving_state}")
# Calculate position, speed, and yaw
position = self.driving_line[0].interpolate(self._driving_state.distance_driven)
# Depending on the align_with_middle_line parameter, the car is always parallel
# to the middle line or to the driving line.
alignment_line = (
self.middle_line if self.param.align_with_middle_line else self.driving_line[0]
)
# Always let the car face into the direction of the middle line.
pose = Pose(
position,
alignment_line.interpolate_direction(alignment_line.project(position)),
)
speed = Vector(current_speed, 0) # Ignore y component of speed
# Yaw rate = curvature * speed
yaw_rate = (
alignment_line.interpolate_curvature(
min(self._driving_state.distance_driven, alignment_line.length)
)
* current_speed
)
# Publish up to date messages!
self.update_world_vehicle_tf(
self.initial_tf.inverse * Transform(pose, pose.get_angle())
)
self.update_state_estimation(speed, yaw_rate)