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_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 lines(self) -> List[MarkedLine]:
"""List[MarkedLine]: All road lines with their marking type."""
lines = []
lines.append(
MarkedLine.from_line(self.left_line, self.left_line_marking,
self.prev_length))
lines.append(
MarkedLine.from_line(self.middle_line, self.SOLID_LINE_MARKING,
self.prev_length))
lines.append(
MarkedLine.from_line(self.right_line, self.right_line_marking,
self.prev_length))
lines.append(
MarkedLine.from_line(self.middle_line_l, self.SOLID_LINE_MARKING,
self.prev_length))
return lines
def lines(self) -> List[MarkedLine]:
"""List[MarkedLine]: Borderlines for spot if spot is on the left.
Marking type is always solid."""
lines = []
if self._side == ParkingLot.LEFT_SIDE or self.kind == self.BLOCKED:
spot_points = self.frame.get_points()
left_border = Line(spot_points[:2])
right_border = Line(spot_points[2:4])
lines.append(
MarkedLine.from_line(left_border,
RoadSection.SOLID_LINE_MARKING))
lines.append(
MarkedLine.from_line(right_border,
RoadSection.SOLID_LINE_MARKING))
return lines
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 draw_line(
ctx,
marked_line: MarkedLine,
line_width: float = 0.02,
dash_length: float = 0.2,
dash_gap: float = None,
):
if marked_line.style is RoadSection.MISSING_LINE_MARKING:
return
ctx.set_source_rgb(1, 1, 1)
ctx.set_line_width(line_width)
if marked_line.style == RoadSection.DASHED_LINE_MARKING:
if dash_gap is None:
dash_gap = dash_length
ctx.set_dash(
[dash_length, dash_gap],
marked_line.prev_length % (dash_length + dash_gap),
)
else:
ctx.set_dash([])
if marked_line.length == 0:
return
points = marked_line.get_points()
ctx.move_to(points[0].x, points[0].y)
for p in points[1:]:
ctx.line_to(p.x, p.y)
ctx.stroke()
def lines(self) -> List[MarkedLine]:
"""List[MarkedLine]: All border lines with solid marking type."""
lines = []
lines.append(
MarkedLine.from_line(self.border, RoadSection.SOLID_LINE_MARKING))
for spot in self.spots:
lines.extend(spot.lines)
return lines
def _draw_double_line(
ctx,
line: MarkedLine,
style_left: str,
style_right: str,
line_width: float,
double_line_gap: float,
):
left_line = line.parallel_offset(double_line_gap / 2 + line_width / 2,
"left")
right_line = line.parallel_offset(double_line_gap / 2 + line_width / 2,
"right")
marked_left_line = MarkedLine.from_line(left_line, style_left,
line.prev_length)
marked_right_line = MarkedLine.from_line(right_line, style_right,
line.prev_length)
draw_line(ctx, marked_left_line)
draw_line(ctx, marked_right_line)
def draw_parking_spot_x(ctx, frame: Polygon):
"""Draw two crossing lines (X) in the given frame to represent a blocked spot.
Args:
frame: Frame of the parking spot.
**Points of the frame must be given in the right order!**
first point : left lower corner of parking spot
second point: left upper corner
third point : right upper corner
fourth point: right lower corner
"""
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 draw(ctx, surface_marking: SurfaceMarking):
ctx.save()
if (surface_marking.kind == SurfaceMarking.LEFT_TURN_MARKING
or surface_marking.kind == SurfaceMarking.RIGHT_TURN_MARKING):
ctx.translate(surface_marking.center.x, surface_marking.center.y)
ctx.rotate(surface_marking.orientation)
image_file = os.path.join(
os.environ.get("KITCAR_REPO_PATH"),
"kitcar-gazebo-simulation",
"simulation",
"models",
"meshes",
f"Fahrbahnmarkierung_Pfeil_" + (
"L" if surface_marking.kind !=
SurfaceMarking.LEFT_TURN_MARKING # Weird
else "R") + ".svg",
)
svg = Rsvg.Handle().new_from_file(image_file)
ctx.scale(0.001, 0.001)
svg.render_cairo(ctx)
if (surface_marking.kind == SurfaceMarking.GIVE_WAY_LINE
or surface_marking.kind == SurfaceMarking.STOP_LINE):
ctx.translate(surface_marking.center.x, surface_marking.center.y)
ctx.rotate(surface_marking.orientation)
v = 0.5 * Vector(surface_marking.width, 0)
line = MarkedLine(
[-1 * v, v],
style=(RoadSection.DASHED_LINE_MARKING
if surface_marking.kind == SurfaceMarking.GIVE_WAY_LINE else
RoadSection.SOLID_LINE_MARKING),
)
utils.draw_line(
ctx,
line,
line_width=0.04,
dash_length=0.08,
dash_gap=0.06,
)
if surface_marking.kind == SurfaceMarking.START_LINE:
draw_start_lane(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.ZEBRA_CROSSING:
draw_zebra_crossing(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.PARKING_SPOT_X:
draw_parking_spot_x(ctx, surface_marking.frame)
ctx.restore()
def draw_start_lane(ctx, frame: Polygon):
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 draw_line(
ctx,
marked_line: MarkedLine,
line_width: float = 0.02,
dash_length: float = 0.2,
dash_gap: float = None,
double_line_gap: float = 0.02,
):
if marked_line.style is RoadSection.MISSING_LINE_MARKING or marked_line.length == 0:
return
ctx.set_source_rgb(1, 1, 1)
ctx.set_line_width(line_width)
if marked_line.style == RoadSection.DOUBLE_SOLID_LINE_MARKING:
_draw_double_line(
ctx,
line=marked_line,
style_left=RoadSection.SOLID_LINE_MARKING,
style_right=RoadSection.SOLID_LINE_MARKING,
line_width=line_width,
double_line_gap=double_line_gap,
)
return
elif marked_line.style == RoadSection.DOUBLE_DASHED_LINE_MARKING:
_draw_double_line(
ctx,
line=marked_line,
style_left=RoadSection.DASHED_LINE_MARKING,
style_right=RoadSection.DASHED_LINE_MARKING,
line_width=line_width,
double_line_gap=double_line_gap,
)
return
elif marked_line.style == RoadSection.SOLID_DASHED_LINE_MARKING:
_draw_double_line(
ctx,
line=marked_line,
style_left=RoadSection.SOLID_LINE_MARKING,
style_right=RoadSection.DASHED_LINE_MARKING,
line_width=line_width,
double_line_gap=double_line_gap,
)
return
elif marked_line.style == RoadSection.DASHED_SOLID_LINE_MARKING:
_draw_double_line(
ctx,
line=marked_line,
style_left=RoadSection.DASHED_LINE_MARKING,
style_right=RoadSection.SOLID_LINE_MARKING,
line_width=line_width,
double_line_gap=double_line_gap,
)
return
elif marked_line.style == RoadSection.DASHED_LINE_MARKING:
if dash_gap is None:
dash_gap = dash_length
ctx.set_dash(
[dash_length, dash_gap],
marked_line.prev_length % (dash_length + dash_gap),
)
else: # implicit RoadSection.SOLID_LINE_MARKING
ctx.set_dash([])
points = marked_line.get_points()
ctx.move_to(points[0].x, points[0].y)
for p in points[1:]:
ctx.line_to(p.x, p.y)
ctx.stroke()
def lines(self) -> List[MarkedLine]:
"""List[MarkedLine]: All road lines with their marking type."""
lines = []
south_middle_end_length = self.prev_length + self.middle_line_south.length
north_middle_start_length = -0.1
north_left_start_length = -0.1
north_right_start_length = -0.1
west_middle_start_length = -0.1
east_middle_start_length = -0.1
if self.turn == Intersection.LEFT:
lines.append(
MarkedLine.from_line(self.ls_circle, self.DASHED_LINE_MARKING,
south_middle_end_length))
lines.append(
MarkedLine.from_line(self.ll_circle, self.DASHED_LINE_MARKING,
-0.1))
west_middle_start_length = south_middle_end_length + self.ls_circle.length
elif self.turn == Intersection.RIGHT:
lines.append(
MarkedLine.from_line(self.rs_circle, self.DASHED_LINE_MARKING,
south_middle_end_length))
lines.append(
MarkedLine.from_line(self.rl_circle, self.DASHED_LINE_MARKING,
-0.1))
east_middle_start_length = south_middle_end_length + self.rs_circle.length
else:
north_middle_start_length = (south_middle_end_length + Line([
self.middle_line_south.get_points()[-1],
self.middle_line_north.get_points()[0],
]).length)
north_left_start_length = (
self.prev_length + self.left_line_south.length + Line([
self.left_line_south.get_points()[-1],
self.left_line_north.get_points()[0],
]).length)
north_right_start_length = (
self.prev_length + self.right_line_south.length + Line([
self.right_line_south.get_points()[-1],
self.right_line_north.get_points()[0],
]).length)
# south + left west + right east
lines.append(
MarkedLine.from_line(
self.left_line_south + self.left_line_west,
self.left_line_marking,
self.prev_length,
))
lines.append(
MarkedLine.from_line(self.middle_line_south,
self.middle_line_marking, self.prev_length))
lines.append(
MarkedLine.from_line(
self.right_line_south + self.right_line_east,
self.right_line_marking,
self.prev_length,
))
# west
lines.append(
MarkedLine.from_line(self.middle_line_west,
self.middle_line_marking,
west_middle_start_length))
lines.append(
MarkedLine.from_line(self.right_line_west, self.right_line_marking,
south_middle_end_length))
# north
lines.append(
MarkedLine.from_line(self.left_line_north, self.left_line_marking,
north_left_start_length))
lines.append(
MarkedLine.from_line(self.middle_line_north,
self.middle_line_marking,
north_middle_start_length))
lines.append(
MarkedLine.from_line(self.right_line_north,
self.right_line_marking,
north_right_start_length))
# east
lines.append(
MarkedLine.from_line(self.left_line_east, self.left_line_marking,
south_middle_end_length))
lines.append(
MarkedLine.from_line(self.middle_line_east,
self.middle_line_marking,
east_middle_start_length))
return lines
def draw(ctx, surface_marking: SurfaceMarking):
ctx.save()
if (surface_marking.kind == SurfaceMarking.LEFT_TURN_MARKING
or surface_marking.kind == SurfaceMarking.RIGHT_TURN_MARKING):
ctx.translate(surface_marking.center.x, surface_marking.center.y)
ctx.rotate(surface_marking.orientation)
# Translate to left upper corner of png
ctx.translate(
surface_marking.depth / 2,
-surface_marking.width / 2,
)
# Then rotate by 90 degrees to align png properly!
ctx.rotate(math.pi / 2)
image_file = os.path.join(
os.environ.get("KITCAR_REPO_PATH"),
"kitcar-gazebo-simulation",
"simulation",
"models",
"meshes",
"surface_marking_turn_" +
("left" if surface_marking.kind != SurfaceMarking.LEFT_TURN_MARKING
else "right") + ".png",
)
img = cairo.ImageSurface.create_from_png(image_file)
ctx.scale(0.001, 0.001)
ctx.set_source_surface(img, 0, 0)
ctx.paint()
if (surface_marking.kind == SurfaceMarking.GIVE_WAY_LINE
or surface_marking.kind == SurfaceMarking.STOP_LINE):
ctx.translate(surface_marking.center.x, surface_marking.center.y)
ctx.rotate(surface_marking.orientation)
v = 0.5 * Vector(0, surface_marking.width)
line = MarkedLine(
[-1 * v, v],
style=(RoadSection.DASHED_LINE_MARKING
if surface_marking.kind == SurfaceMarking.GIVE_WAY_LINE else
RoadSection.SOLID_LINE_MARKING),
)
utils.draw_line(
ctx,
line,
line_width=0.04,
dash_length=0.08,
dash_gap=0.06,
)
if surface_marking.kind == SurfaceMarking.START_LINE:
draw_start_lane(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.ZEBRA_CROSSING:
draw_zebra_crossing(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.PARKING_SPOT_X:
draw_parking_spot_x(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.BLOCKED_AREA:
draw_blocked_area(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.ZEBRA_LINES:
draw_crossing_lines(ctx, surface_marking.frame)
if surface_marking.kind == SurfaceMarking.TRAFFIC_ISLAND_BLOCKED:
draw_traffic_island_blocked(ctx, surface_marking.frame)
if surface_marking.kind[1].startswith("ZONE_"):
_, limit, type = surface_marking.kind[1].split("_")
draw_speed_limit(ctx, surface_marking, limit, type.lower() == "start")
ctx.restore()
