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 extend_spots(msg: ParkingMsg):
"""Extend the parking spots by the spots inside msg."""
parking_spots.extend([
Polygon(
Polygon(s.frame).buffer(
self.parking_spot_buffer).exterior.coords)
for s in msg.spots if s.type == ParkingMsg.SPOT_FREE
])
def polygons_from_msg(msg):
if msg is not None:
return (Polygon(
Polygon(border.points).buffer(
self.area_buffer).exterior.coords)
for border in msg.borders)
else:
return []
def read_car_config(self):
"""Process car parameters."""
car_specs = CarSpecs.from_yaml(self.param.car_specs_path)
camera_specs = CameraSpecs.from_yaml(self.param.camera_specs_path)
""" Car frame config """
# Car frame should have it's origin at the center of the rear axle:
# Distance to rear/front is measured in respect to the front axle!!
self.car_frame = Polygon(
[
[
-car_specs.distance_to_rear_bumper + car_specs.wheelbase,
car_specs.vehicle_width / 2,
],
[
-car_specs.distance_to_rear_bumper + car_specs.wheelbase,
-car_specs.vehicle_width / 2,
],
[
car_specs.distance_to_front_bumper + car_specs.wheelbase,
-car_specs.vehicle_width / 2,
],
[
car_specs.distance_to_front_bumper + car_specs.wheelbase,
car_specs.vehicle_width / 2,
],
]
)
""" Camera config """
# This parameter tells how far the camera can see
view_distance: float = camera_specs.clip["far"]
# field of view (opening angle of camera)
fov: float = camera_specs.horizontal_fov
if self.param.cone_points == 0:
self.view_cone = None
return
# Calculate a few points to approximate view frame
# Add points on horizon of our camera (at view_distance away from vehicle)
# /approximates a view cone
# Create geom.Polygon from points
self.view_cone = Polygon(
[Point(0, 0)]
+ [
Point(r=view_distance, phi=alpha)
for alpha in np.linspace(-fov / 2, fov / 2, self.param.cone_points)
]
)
def __init__(
self,
arc_length: float = 0.4,
y: float = -0.2,
width: float = width,
depth: float = depth,
angle: float = angle,
normalize_x: bool = True,
z: float = 0,
height: float = 0,
):
"""Initialize a retangular road element."""
for obj in arc_length, y, width, depth, angle:
assert isinstance(obj, float) or isinstance(
obj, int
), f"Should be a number but is {obj}"
self.width = width
self.depth = depth
self.angle = angle
self.height = height
super().__init__(
normalize_x=normalize_x,
_frame=Transform(Point(arc_length, y, z), self.angle)
* Polygon(
[
[-self.depth / 2, self.width / 2],
[self.depth / 2, self.width / 2],
[self.depth / 2, -self.width / 2],
[-self.depth / 2, -self.width / 2],
]
),
)
def draw_start_lane(ctx, frame: Polygon):
"""Draw the checkerboard pattern to mark the beginning of a parking area in the given
frame.
Args:
frame: Frame of the start lane.
**Points of the frame must be given in the right order!**
first point : start on left line
second point: end on left line
third point : end on right line
fourth point: start on right line
"""
TILE_LENGTH = 0.02
points = frame.get_points()
left = Line([points[0], points[1]])
right = Line([points[3], points[2]])
for i in range(3):
utils.draw_line(
ctx,
MarkedLine(
[
left.interpolate(TILE_LENGTH * (i + 0.5)),
right.interpolate(TILE_LENGTH * (i + 0.5)),
],
style=RoadSection.DASHED_LINE_MARKING,
prev_length=(i % 2 + 0.5) * TILE_LENGTH,
),
dash_length=TILE_LENGTH,
)
def get_bounds(self) -> Tuple[int, int, int, int]:
"""Get the bounds of this box in the camera image.
Return:
x1, y1, x2, y2
"""
vehicle_points = self.vehicle_points
center_ground: Vector = 1 / len(vehicle_points) * sum(
vehicle_points, Vector(0, 0))
center_ground = Vector(center_ground.x, center_ground.y)
pixels = []
for p in vehicle_points + [center_ground]:
hom_vec = np.ones(4)
hom_vec[:3] = p.to_numpy()
pixel_vec = np.dot(BoundingBox.vehicle_pixel_matrix, hom_vec).T
pixel_vec *= 1 / float(pixel_vec[2])
pixels.append(Vector(pixel_vec[0], pixel_vec[1]))
pixel_ground_center = pixels[-1]
del pixels[-1]
pixel_poly = Polygon(pixels)
x1, y1, x2, y2 = pixel_poly.bounds
x1, x2 = round(min(x1, x2)), round(max(x1, x2))
y1, y2 = round(min(y1, y2)), round(max(y1, y2))
return x1, y1, x2, y2, round(pixel_ground_center.x), round(
pixel_ground_center.y)
def get_bounding_box(self) -> Polygon:
"""Get a polygon around the road section.
Bounding box is an approximate representation of all points within a given distance
of this geometric object.
"""
return Polygon(self.middle_line.buffer(1.5 * Config.road_width))
def draw_crossing_lines(ctx, frame: Polygon):
"""Draw a crossing area for pedestrian, which is only marked by dashed lines, in the
given frame.
The dashed lines are perpendicular to the road.
Args:
frame: Frame of the crossing area.
**Points of the frame must be given in the right order!**
first point : start on left line
second point: end on left line
third point : end on right line
fourth point: start on right line
"""
points = frame.get_points()
dash_length = 0.04
utils.draw_line(
ctx,
MarkedLine(
[points[0], points[3]],
style=RoadSection.DASHED_LINE_MARKING,
),
dash_length=dash_length,
)
utils.draw_line(
ctx,
MarkedLine(
[points[1], points[2]],
style=RoadSection.DASHED_LINE_MARKING,
),
dash_length=dash_length,
)
def draw_zebra_crossing(ctx,
frame: Polygon,
stripe_width: float = 0.04,
offset: float = 0.02):
"""Draw a zebra crossing in the given frame.
Args:
frame: Frame of the zebra crossing.
**Points of the frame must be given in the right order!**
first point : start on left line
second point: end on left line
third point : end on right line
fourth point: start on right line
"""
points = frame.get_points()
left = Line([points[0], points[3]])
right = Line([points[1], points[2]])
flag = True
min_length = min(left.length, right.length)
x = offset
while x < min_length:
l, r = left.interpolate(x), right.interpolate(x)
if flag:
ctx.move_to(l.x, l.y)
ctx.line_to(r.x, r.y)
flag = False
else:
ctx.line_to(r.x, r.y)
ctx.line_to(l.x, l.y)
ctx.close_path()
ctx.fill()
flag = True
x += stripe_width
def start(self):
super().start()
self.detections_subscriber = rospy.Subscriber(
name=self.param.topics.detections,
data_class=TrafficSigns,
callback=self.save_msg,
)
self.publish_counter = 0
self.evaluation_publisher = rospy.Publisher(
self.param.topics.evaluation,
data_class=SignEvaluationMsg,
queue_size=10)
self.marker_publisher = rospy.Publisher(self.param.topics.sign_marking,
data_class=Marker,
queue_size=10)
rospy.wait_for_service(self.param.topics.traffic_sign)
sign_proxy = rospy.ServiceProxy(self.param.topics.traffic_sign,
LabeledPolygonSrv)
rospy.wait_for_service(self.param.topics.section)
sections = rospy.ServiceProxy(self.param.topics.section,
SectionSrv)().sections
self.signs = [
Sign(Point(Polygon(msg.frame).centroid), msg.desc, msg.visible)
for sec in sections for msg in sign_proxy(sec.id).polygons
]
def _show_parking_markers(self):
"""Publish markers for all parking lines and spots of road."""
parking_sections = [
s for s in self.get_sections().sections
if s.type == road_section_type.PARKING_AREA
]
parking_counter = 0
spot_counter = 0
for sec in parking_sections:
parking_srv = self.get_parking(sec.id)
left_msg, right_msg = parking_srv.left_msg, parking_srv.right_msg
def show_borders(borders, topic_name, ns):
nonlocal parking_counter
for border in borders:
self._publish_point_marker(
border.points,
topic_name,
parking_counter,
self.param.colors.parking_border,
ns="/simulation/groundtruth/" + ns,
)
parking_counter += 1
show_borders(left_msg.borders, "parking_line", "parking_left")
show_borders(right_msg.borders, "parking_line", "parking_right")
spots = []
spots.extend(left_msg.spots)
spots.extend(right_msg.spots)
for spot in spots:
# Extract all four points
points = Polygon(spot.frame.points).to_geometry_msg().points
# self.rviz_parking_spot_publisher.publish(poly)
color = self.param.colors.parking_spot_free
if spot.type == ParkingMsg.SPOT_X:
points = [
points[0],
points[1],
points[3],
points[2],
points[0],
points[3],
points[2],
points[1],
] # Create an x in the visualization
color = self.param.colors.parking_spot_x
elif spot.type == ParkingMsg.SPOT_OCCUPIED:
color = self.param.colors.parking_spot_occupied
self._publish_point_marker(points, "parking_spot",
spot_counter, color)
spot_counter += 1
def frame(self) -> Polygon:
"""Polygon: Frame of the element in global coordinates."""
return Transform(self.center, self.orientation) * Polygon([
[-self.depth / 2, self.width / 2],
[self.depth / 2, self.width / 2],
[self.depth / 2, -self.width / 2],
[-self.depth / 2, -self.width / 2],
])
def _poly(self) -> Polygon:
opening_x = self.width / math.tan(self._opening_angle)
return Polygon([
Point(0, -Config.road_width),
Point(opening_x, -Config.road_width + self.width),
Point(self.length - opening_x, -Config.road_width + self.width),
Point(self.length, -Config.road_width),
])
def read_car_config(self):
"""Process car parameters.
"""
car_specs = CarSpecs.from_file(self.param.car_specs_path)
camera_specs = CameraSpecs.from_file(self.param.camera_specs_path)
""" Car frame config """
chassis_size = Vector(
car_specs.distance_to_rear_bumper
# + car_specs.wheelbase
+ car_specs.distance_cog_front,
car_specs.vehicle_width,
)
chassis_position = Point(car_specs.center_rear_axle.x,
car_specs.center_rear_axle.y)
# get dimensions
x_span = Vector(0.5 * chassis_size.x,
0) # Vector in x direction of length = width/2
y_span = Vector(0, 0.5 * chassis_size.y)
self.car_frame = Polygon([
chassis_position + x_span + y_span, # Front right
chassis_position - x_span + y_span, # Front left
chassis_position - x_span - y_span, # Back left
chassis_position + x_span - y_span, # Back right
])
""" Camera config """
# This parameter tells how far the camera can see
view_distance: float = camera_specs.clip["far"]
# field of view (opening angle of camera)
fov: float = camera_specs.horizontal_fov
if self.param.cone_points == 0:
self.view_cone = None
return
# Calculate a few points to approximate view frame
# Add points on horizon of our camera (at view_distance away from vehicle) /approximates a view cone
# Create geom.Polygon from points
self.view_cone = Polygon([Point(0, 0)] + [
Point(r=view_distance, phi=alpha)
for alpha in np.linspace(-fov / 2, fov / 2, self.param.cone_points)
])
def frame(self) -> Polygon:
"""Polygon: Frame of the parking spot in global coordinates."""
poly = Polygon([
Point(0, 0),
Point(self._depth, 0),
Point(self._depth, -self.width),
Point(0, -self.width),
])
return self.transform * poly
def frame(self) -> Polygon:
"""Polygon : Frame for the zebra crossing surface marking."""
poly = Polygon([
Point(0, -Config.road_width),
Point(0, Config.road_width),
Point(self.length, Config.road_width),
Point(self.length, -Config.road_width),
])
return self.transform * poly
def obstacles(self) -> List[Polygon]:
"""All obstacles as a list of polygons."""
return [
# Buffer the obstacle that is received from proxy because of some issues
# with shapely thinking that the points are not a valid polygon!
# And it further simplifies the polygons
Polygon(o.buffer(BUFFER).exterior.coords) for sec in self.sections
for o in self.get_obstacles_in_section(sec.id)
]
class _RoadElementPoly(RoadElement):
frame: Polygon = Polygon(
[Point(0.3, -0.4),
Point(0.5, -0.4),
Point(0.5, 0),
Point(0.3, 0)])
"""Polygon: Frame of the element in global coordinates."""
def __post_init__(self):
super().__post_init__()
self._center = self.frame.centroid
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
def draw_parking_spot_x(ctx, frame: Polygon):
points = frame.get_points()
utils.draw_line(
ctx,
MarkedLine([points[0], points[2]],
style=RoadSection.SOLID_LINE_MARKING))
utils.draw_line(
ctx,
MarkedLine([points[1], points[3]],
style=RoadSection.SOLID_LINE_MARKING))
def frame(self) -> Polygon:
poly = Polygon(
[
Point(0, -Config.road_width),
Point(0, Config.road_width),
Point(self.length, Config.road_width),
Point(self.length, -Config.road_width),
]
)
return self.transform * poly
def blocked_areas(self) -> List[Polygon]:
"""All blocked_areas as a list of polygons."""
return [
# Buffer the blocked_area that is received from proxy because of some issues
# with shapely thinking that the points are not a valid polygon!
# And it further simplifies the polygons
Polygon(sm.frame.buffer(BUFFER).exterior.coords)
for sec in self.sections
for sm in self.get_surface_markings_in_section(sec.id)
if sm.id_ == SurfaceMarking.BLOCKED_AREA[0]
]
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 get_obstacles_in_section(self, section_id: int) -> List[Polygon]:
"""Get all obstacles inside as polygons.
Args:
section_id: id of the section
Returns:
List of polygons describing the frames of the obstacles
"""
obstacle_msgs = self.get_obstacles(section_id).polygons
return [Polygon(o.frame) for o in obstacle_msgs]
def get_bounding_box(self) -> Polygon:
"""Get a polygon around the road section.
Bounding box is an approximate representation of all points within a given distance
of this geometric object.
"""
biggest_depth = 0
for ll, rl in zip(self.left_lots, self.right_lots):
if ll.depth > biggest_depth:
biggest_depth = ll.depth
if rl.depth > biggest_depth:
biggest_depth = rl.depth
return Polygon(
self.middle_line.buffer(1.5 * (biggest_depth + Config.road_width)))
def _show_obstacle_markers(self):
"""Publish polygons for all obstacles on the road."""
obstacles = sum(
([
Polygon(msg.frame).to_geometry_msg().points
for msg in self.get_obstacles(sec.id).polygons
if msg.type == LabeledPolygonMsg.OBSTACLE
] for sec in self.get_sections().sections),
[],
)
for id, obstacle in enumerate(obstacles):
self._publish_point_marker(obstacle, "obstacle", id,
self.param.colors.obstacle)
def test_zebra_crossing(self):
TF = Transform([1, 1], 0)
LENGTH = 2
zc = ZebraCrossing(length=LENGTH, transform=TF)
self.assertEqual(zc.__class__.TYPE, road_section_type.ZEBRA_CROSSING)
self.assertEqual(
zc.frame,
TF * Polygon([
Point(0, -Config.road_width),
Point(0, Config.road_width),
Point(LENGTH, Config.road_width),
Point(LENGTH, -Config.road_width),
]),
)
def test_obstacles(self):
MIDDLE_LINE = Line([[0, 0], [0, 2]])
DummyRoadSection.MIDDLE_LINE = MIDDLE_LINE
# Assume the obstacle class itself works!
# Simply test if the transforms etc. are set correctly.
obstacle = StaticObstacle(center=Point(1.5, -0.2), width=0.2, depth=1)
rs = DummyRoadSection(obstacles=[obstacle])
returned_obs = rs.obstacles[0]
self.assertEqual(returned_obs.transform,
Transform([0, 1.5], math.pi / 2))
self.assertEqual(returned_obs.center, Point(0.2, 1.5))
self.assertEqual(returned_obs.frame,
Polygon([[0.1, 1], [0.1, 2], [0.3, 2], [0.3, 1]]))
def set_transform(self, tf: Transform):
self._frame = Polygon([
[0, 0],
[self.depth, 0],
[self.depth, -self.width],
[0, -self.width],
])
super().set_transform(tf)
if self.obstacle is not None:
self.obstacle.set_transform(self.transform)
if self.x_surface_marking is not None:
self.x_surface_marking.width = self.width
self.x_surface_marking.depth = self.depth
self.x_surface_marking._frame = self._frame
self.x_surface_marking.set_transform(self.transform)