"""Intersection."""
import functools
import math
from dataclasses import dataclass
from typing import List, Optional, Tuple
import numpy as np
from kitcar_utils.geometry import Line, Point, Polygon, Pose, Vector
import simulation.utils.road.sections.type as road_section_type
from simulation.utils.road.config import Config
from simulation.utils.road.sections.road_section import MarkedLine, RoadSection
from simulation.utils.road.sections.surface_marking import SurfaceMarkingRect
from simulation.utils.road.sections.traffic_sign import TrafficSign
[docs]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 = line1.interpolate_direction(arc_length=0).argument
return SurfaceMarkingRect(
kind, *center.xy, angle=angle, width=width, normalize_x=False, depth=0.04
)
[docs]def arange_with_end(start: float, end: float, step: float) -> np.ndarray:
"""NumPy arange, but include end point.
Args:
start: Start of interval
end: End of interval
step: Spacing between values
Returns:
Array of evenly spaced values
"""
return np.arange(start, end + step, step)
[docs]@dataclass
class Intersection(RoadSection):
"""Road section representing an intersection.
Args:
angle: Angle [radian] between crossing roads.
closing: Optionally close one direction to create a T-intersection.
turn: Turning direction.
rule: Priority-rule at intersection.
size: Length of the crossing roads.
exit_direction: Optionally overwrite the visible turning direction.
invisible: Used to close loops at intersection.
**Intersection internal structure:**
.. image:: ../tutorials/resources/intersection_internal_structure.png
:alt: Intersection internal structure
"""
TYPE = road_section_type.INTERSECTION
ORIGIN = -1
STRAIGHT = 0
"""Possible value for :attr:`turn`.
Drive straight through the intersection.
"""
LEFT = 1
"""Possible value for :attr:`turn`.
Turn left at the intersection.
"""
RIGHT = 2
"""Possible value for :attr:`turn`.
Turn right at the intersection.
"""
EQUAL = 0
"""Possible value for :attr:`rule`.
*Rechts vor links.*
"""
YIELD = 1
"""Possible value for :attr:`rule`.
Car must yield.
"""
STOP = 2
"""Possible value for :attr:`rule`.
Car must stop.
"""
PRIORITY_YIELD = 3
"""Possible value for :attr:`rule`. Car will have the right of way.
Intersecting road must yield.
"""
PRIORITY_STOP = 4
"""Possible value for :attr:`rule`. Car will have the right of way.
Intersecting road must stop.
"""
angle: float = math.pi / 2
"""Angle between intersecting roads [radian]."""
closing: Optional[int] = None
"""Closed direction (T-intersection)."""
turn: int = STRAIGHT
"""Direction in which road continues."""
rule: int = EQUAL
"""Priority rule at intersection."""
size: float = 1.8
"""Size of intersection (from one side to the other)."""
exit_direction: int = None
"""Optional parameter to overwrite the visible turning direction."""
invisible: bool = False
"""Enables invisible intersection to close loops at an intersection."""
def __post_init__(self):
super().__post_init__()
# alpha is difference to normal 90° intersection
self._alpha = self.angle - math.pi / 2
self._size = self.size / 2
self.traffic_signs.extend(self._get_intersection_traffic_signs())
self.surface_markings.extend(self._get_intersection_surface_markings())
# Check if size is large enough
assert (-1 * self.w + self.v).y > (-1 * self.u).y and self.z.x > (self.x - self.u).x
# all vectors defined as a property are defined in the local coordinate system!
# See: Intersection internal structure
@property
def sin(self):
return math.sin(self._alpha)
@property
def cos(self):
return math.cos(self._alpha)
@property
def y(self):
return Vector(0, -Config.road_width)
@property
def x(self):
return Vector(Config.road_width / self.cos, 0)
@property
def z(self):
return Vector(self._size, 0)
@property
def u(self):
return Vector(math.tan(self._alpha) * Config.road_width, -Config.road_width)
@property
def v(self):
return Vector(
Config.road_width * self.cos,
Config.road_width * math.sin(self._alpha),
)
@property
def w(self):
return Vector(r=self._size, phi=-math.pi / 2 + self._alpha)
@property
def lo(self):
return Vector(
0,
(
-1 * self.y
- self.x
- self.u
- (2 - 2 * self.sin) / (self.cos * self.cos) * self.v
).y,
)
@property
def li(self):
return Vector(0, (-1 * self.u + (-1 + self.sin) / (self.cos * self.cos) * self.v).y)
@property
def ro(self):
return Vector(
0,
(
self.x
+ self.u
+ self.y
- (2 + 2 * self.sin) / (self.cos * self.cos) * self.v
).y,
)
@property
def ri(self):
return Vector(0, (self.u - (1 + self.sin) / (self.cos * self.cos) * self.v).y)
# all center_points for signs and surface markings are defined in
# the local coordinate system!
# move cp_surface by Config.TURN_SF_MARK_WIDTH/2,
# because render uses left upper corner to place the image
[docs] def cp_sign_south(self, sign_dist: float) -> Vector:
"""Sign Center Point South.
Args:
sign_dist: distance from end of right line
Returns:
Center point
"""
return Vector(self.z - self.x + self.u) - Vector(
sign_dist, Config.get_sign_road_padding()
)
[docs] def cp_surface_south(self) -> Vector:
"""Surface Marking Center Point South.
Returns:
Center point
"""
return Vector(self.z - self.x + 0.5 * self.u) - Vector(
Config.get_surface_mark_dist() + Config.TURN_SF_MARK_LENGTH / 2, 0
)
[docs] def cp_sign_west(self, sign_dist: float) -> Vector:
"""Sign Center Point West.
Args:
sign_dist: distance from end of right line
Returns:
Center point
"""
return (
Vector(self.z - self.x - self.u)
- Vector(sign_dist * self.u / abs(self.u))
- Vector(Config.get_sign_road_padding() * self.v / abs(self.v))
)
[docs] def cp_surface_west(self) -> Vector:
"""Surface Marking Center Point West.
Returns:
Center point
"""
return (
Vector(self.z - 0.5 * self.x - self.u)
- Vector(
(Config.get_surface_mark_dist() + Config.TURN_SF_MARK_LENGTH / 2)
* self.u
/ abs(self.u)
)
- Vector(Config.TURN_SF_MARK_WIDTH / 2 * self.v / abs(self.v))
)
[docs] def cp_sign_north(self, sign_dist: float) -> Vector:
"""Sign Center Point North.
Args:
sign_dist: distance from end of right line
Returns:
Center point
"""
return Vector(self.z + self.x - self.u) + Vector(
sign_dist, Config.get_sign_road_padding()
)
[docs] def cp_sign_east(self, sign_dist: float) -> Vector:
"""Sign Center Point East.
Args:
sign_dist: distance from end of right line
Returns:
Center point
"""
return (
Vector(self.z + self.x + self.u)
+ Vector(sign_dist * self.u) / abs(self.u)
+ Vector(Config.get_sign_road_padding() * self.v / abs(self.v))
)
[docs] def cp_surface_east(self) -> Vector:
"""Surface Marking Center Point East.
Returns:
Center point
"""
return (
Vector(self.z + 0.5 * self.x + self.u)
+ Vector(
(Config.get_surface_mark_dist() + Config.TURN_SF_MARK_LENGTH / 2)
* self.u
/ abs(self.u)
)
+ Vector(Config.TURN_SF_MARK_WIDTH / 2 * self.v) / abs(self.v)
)
# all lines are transformed to the global coordinate system
# south is origin
@property
def middle_line_south(self) -> Line:
return self.transform * Line([Point(0, 0), Point(self.z - self.x)])
@property
def left_line_south(self) -> Line:
return self.transform * Line(
[Point(0, Config.road_width), Point(self.z - self.x - self.u)]
)
@property
def right_line_south(self) -> Line:
return self.transform * Line(
[Point(0, -Config.road_width), Point(self.z - self.x + self.u)]
)
# in case of a T-intersection (one side closed) an empty list is returned
@property
def middle_line_east(self) -> Line:
if self.closing == Intersection.RIGHT:
return Line([])
return self.transform * Line([Point(self.z + self.u), Point(self.z + self.w)])
@property
def left_line_east(self) -> Line:
if self.closing == Intersection.RIGHT:
return Line([])
return self.transform * Line(
[Point(self.z + self.x + self.u), Point(self.z + self.w + self.v)]
)
@property
def right_line_east(self) -> Line:
if self.closing == Intersection.RIGHT:
return Line([])
return self.transform * Line(
[Point(self.z - self.x + self.u), Point(self.z + self.w - self.v)]
)
@property
def middle_line_north(self) -> Line:
if self.closing == Intersection.STRAIGHT:
return Line([])
return self.transform * Line([Point(self.z + self.x), Point(2 * self.z)])
@property
def left_line_north(self) -> Line:
if self.closing == Intersection.STRAIGHT:
return Line([])
return self.transform * Line(
[Point(self.z + self.x - self.u), Point(2 * self.z - self.y)]
)
@property
def right_line_north(self) -> Line:
if self.closing == Intersection.STRAIGHT:
return Line([])
return self.transform * Line(
[Point(self.z + self.x + self.u), Point(2 * self.z + self.y)]
)
@property
def middle_line_west(self) -> Line:
if self.closing == Intersection.LEFT:
return Line([])
return self.transform * Line([Point(self.z - self.u), Point(self.z - self.w)])
@property
def left_line_west(self) -> Line:
if self.closing == Intersection.LEFT:
return Line([])
return self.transform * Line(
[Point(self.z - self.x - self.u), Point(self.z - self.w - self.v)]
)
@property
def right_line_west(self) -> Line:
if self.closing == Intersection.LEFT:
return Line([])
return self.transform * Line(
[Point(self.z + self.x - self.u), Point(self.z - self.w + self.v)]
)
# lines that are used to close on side of the intersection (T-intersection)
@property
def closing_line_east(self) -> Line:
if self.closing != Intersection.RIGHT:
return Line([])
return self.transform * Line(
[Point(self.z - self.x + self.u), Point(self.z + self.x + self.u)]
)
@property
def closing_line_west(self) -> Line:
if self.closing != Intersection.LEFT:
return Line([])
return self.transform * Line(
[Point(self.z - self.x - self.u), Point(self.z + self.x - self.u)]
)
@property
def closing_line_north(self) -> Line:
if self.closing != Intersection.STRAIGHT:
return Line([])
return self.transform * Line(
[Point(self.z + self.x + self.u), Point(self.z + self.x - self.u)]
)
# circles to construct the turning lines within the intersection
@property
def left_inner_circle(self) -> Line:
"""Inner left turn circle.
Circle provides guidance during turn
Circle starts at end point of south middle line
Circle ends at start of west middle line
Returns:
Circle
"""
if self.turn != Intersection.LEFT:
return Line([])
points_ls = []
for theta in arange_with_end(0, 0.5 * math.pi + self._alpha, math.pi / 20):
points_ls.append(Point(self.z - self.x + self.li - self.li.rotated(theta)))
return self.transform * Line(points_ls)
@property
def left_outer_circle(self) -> Line:
"""Outer left turn circle.
Circle provides guidance during turn
Circle starts at end point of south right line
Circle ends at start of west right line
Returns:
Circle
"""
if self.turn != Intersection.LEFT:
return Line([])
points_ll = []
for theta in arange_with_end(0, 0.5 * math.pi + self._alpha, math.pi / 40):
points_ll.append(
Point(self.z - self.x + self.u + self.lo - self.lo.rotated(theta))
)
return self.transform * Line(points_ll)
@property
def right_inner_circle(self) -> Line:
"""Inner right turn circle.
Circle provides guidance during turn
Circle starts at end point of south middle line
Circle ends at start of east middle line
Returns:
Circle
"""
if self.turn != Intersection.RIGHT:
return Line([])
points_rs = []
for theta in arange_with_end(0, -math.pi / 2 + self._alpha, -math.pi / 20):
points_rs.append(Point(self.z - self.x + self.ri - self.ri.rotated(theta)))
return self.transform * Line(points_rs)
@property
def right_outer_circle(self) -> Line:
"""Outer right turn circle.
Circle provides guidance during turn
Circle starts at end point of south left line
Circle ends at start of east left line
Returns:
Circle
"""
if self.turn != Intersection.RIGHT:
return Line([])
points_rl = []
for theta in arange_with_end(0, -math.pi / 2 + self._alpha, -math.pi / 40):
points_rl.append(
Point(self.z - self.x - self.u + self.ro - self.ro.rotated(theta))
)
return self.transform * Line(points_rl)
@functools.cached_property
def middle_line(self) -> Line:
"""Middle line of the intersection."""
if self.turn == Intersection.LEFT:
return self.middle_line_south + self.left_inner_circle + self.middle_line_west
elif self.turn == Intersection.RIGHT:
return self.middle_line_south + self.right_inner_circle + self.middle_line_east
else:
straight_m_l = Line(
[
self.middle_line_south.get_points()[-1],
self.middle_line_north.get_points()[0],
]
)
return self.middle_line_south + straight_m_l + self.middle_line_north
@property
def lines(self) -> List[MarkedLine]:
"""All road lines with their marking type."""
if self.invisible:
return []
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.left_inner_circle,
self.DASHED_LINE_MARKING,
south_middle_end_length,
)
)
lines.append(
MarkedLine.from_line(self.left_outer_circle, self.DASHED_LINE_MARKING, -0.1)
)
west_middle_start_length = (
south_middle_end_length + self.left_inner_circle.length
)
elif self.turn == Intersection.RIGHT:
lines.append(
MarkedLine.from_line(
self.right_inner_circle,
self.DASHED_LINE_MARKING,
south_middle_end_length,
)
)
lines.append(
MarkedLine.from_line(
self.right_outer_circle, self.DASHED_LINE_MARKING, -0.1
)
)
east_middle_start_length = (
south_middle_end_length + self.right_inner_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
)
)
lines.append(MarkedLine.from_line(self.closing_line_west, self.SOLID_LINE_MARKING))
# 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
)
)
lines.append(MarkedLine.from_line(self.closing_line_north, self.SOLID_LINE_MARKING))
# 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
)
)
lines.append(MarkedLine.from_line(self.closing_line_east, self.SOLID_LINE_MARKING))
return lines
[docs] def get_beginning(self) -> Tuple[Pose, float]:
"""Get the beginning of the intersection as a pose and the curvature.
Returns:
A tuple consisting of the first point on the middle line together with \
the direction facing away from the road section as a pose and the curvature \
at the beginning of the middle line.
"""
return (Pose(self.transform * Point(0, 0), self.transform.get_angle() + math.pi), 0)
[docs] def get_ending(self, turn_direction: Optional[int] = None) -> Tuple[Pose, float]:
"""Get the ending of the intersection as a pose and the curvature.
Args:
turn_direction: Get ending for given direction.
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.
"""
if turn_direction is None:
turn_direction = (
self.exit_direction if self.exit_direction is not None else self.turn
)
if turn_direction == Intersection.LEFT:
end_angle = math.pi / 2 + self._alpha
end_point = Point(self.z - self.w)
elif turn_direction == Intersection.RIGHT:
end_angle = -math.pi / 2 + self._alpha
end_point = Point(self.z + self.w)
elif turn_direction == Intersection.STRAIGHT:
end_angle = 0
end_point = Point(2 * self.z)
return (Pose(self.transform * end_point, self.transform.get_angle() + end_angle), 0)
[docs] def get_bounding_box(self) -> Polygon:
"""Get a polygon around the intersection.
Bounding box is an approximate representation of all points within a given distance
of this geometric object.
Returns:
Bounding box
"""
return Polygon(self.middle_line.buffer(1.5 * self.size))
[docs] def _get_intersection_traffic_signs(self) -> List[TrafficSign]:
"""Get a list of all traffic signs.
Returns:
All traffic signs
"""
signs = []
if self.turn == Intersection.LEFT:
# sign "turn left" in south
signs.append(
TrafficSign(
TrafficSign.TURN_LEFT,
*self.cp_sign_south(Config.get_turn_sign_dist()).xy,
)
)
if (
self.rule == Intersection.PRIORITY_YIELD
or self.rule == Intersection.PRIORITY_STOP
) and not self.invisible:
# sign "turn right" in west
signs.append(
TrafficSign(
TrafficSign.TURN_RIGHT,
*self.cp_sign_west(Config.get_turn_sign_dist()).xy,
angle=self._alpha - 0.5 * math.pi,
visible=False,
)
)
elif self.turn == Intersection.RIGHT:
# sign "turn right" in south
signs.append(
TrafficSign(
TrafficSign.TURN_RIGHT,
*self.cp_sign_south(Config.get_turn_sign_dist()).xy,
)
)
signs.append(
TrafficSign(
TrafficSign.TURN_RIGHT,
*self.cp_sign_south(Config.get_turn_sign_dist()).xy,
)
)
if (
self.rule == Intersection.PRIORITY_YIELD
or self.rule == Intersection.PRIORITY_STOP
) and not self.invisible:
# sign "turn left" in east
signs.append(
TrafficSign(
TrafficSign.TURN_LEFT,
*self.cp_sign_east(Config.get_turn_sign_dist()).xy,
angle=self._alpha + 0.5 * math.pi,
visible=False,
)
)
rule_map = {
Intersection.PRIORITY_YIELD: TrafficSign.PRIORITY,
Intersection.PRIORITY_STOP: TrafficSign.PRIORITY,
Intersection.YIELD: TrafficSign.YIELD,
Intersection.STOP: TrafficSign.STOP,
}
rule_map_opposite = {
Intersection.PRIORITY_YIELD: TrafficSign.YIELD,
Intersection.PRIORITY_STOP: TrafficSign.STOP,
Intersection.YIELD: TrafficSign.PRIORITY,
Intersection.STOP: TrafficSign.PRIORITY,
}
# check if intersection is a turning priority road
# (Abknickende Vorfahrtsstraße)
priority_turn = (
self.rule == Intersection.PRIORITY_YIELD
or self.rule == Intersection.PRIORITY_STOP
) and self.turn != Intersection.STRAIGHT
priority_turn_left = priority_turn and self.turn == Intersection.LEFT
priority_turn_right = priority_turn and self.turn == Intersection.RIGHT
if self.rule in rule_map and not self.invisible:
signs.append(
TrafficSign(
rule_map[self.rule],
*self.cp_sign_south(Config.get_prio_sign_dist(1)).xy,
)
)
if self.closing != Intersection.STRAIGHT:
r = rule_map_opposite if priority_turn else rule_map
signs.append(
TrafficSign(
r[self.rule],
*self.cp_sign_north(Config.get_prio_sign_dist(1)).xy,
angle=math.pi,
)
)
if self.closing != Intersection.LEFT:
r = rule_map if priority_turn_left else rule_map_opposite
signs.append(
TrafficSign(
r[self.rule],
*self.cp_sign_west(Config.get_prio_sign_dist(1)).xy,
angle=self._alpha - 0.5 * math.pi,
)
)
if self.closing != Intersection.RIGHT:
r = rule_map if priority_turn_right else rule_map_opposite
signs.append(
TrafficSign(
r[self.rule],
*self.cp_sign_east(Config.get_prio_sign_dist(1)).xy,
angle=self._alpha + 0.5 * math.pi,
)
)
for sign in signs:
sign.normalize_x = False
sign.set_transform(self.transform)
return signs
[docs] def _get_intersection_surface_markings(self) -> List[SurfaceMarkingRect]:
"""Get a list of all surface markings.
Returns:
All surface markings
"""
markings = []
if self.turn == Intersection.LEFT or self.turn == Intersection.RIGHT:
own_marking = (
SurfaceMarkingRect.LEFT_TURN_MARKING
if self.turn == Intersection.LEFT
else SurfaceMarkingRect.RIGHT_TURN_MARKING
)
# roadmarking "turn left/right" in south
markings.append(
SurfaceMarkingRect(
own_marking,
*self.cp_surface_south().xy,
angle=0,
width=Config.TURN_SF_MARK_WIDTH,
depth=Config.TURN_SF_MARK_LENGTH,
)
)
if (
self.rule == Intersection.PRIORITY_YIELD
or self.rule == Intersection.PRIORITY_STOP
) and not self.invisible:
opposite_marking = (
SurfaceMarkingRect.RIGHT_TURN_MARKING
if self.turn == Intersection.LEFT
else SurfaceMarkingRect.LEFT_TURN_MARKING
)
opposite_angle = self.angle + (
0 if self.turn == Intersection.RIGHT else math.pi
)
opposite_center = Point(
self.cp_surface_west()
if self.turn == Intersection.LEFT
else self.cp_surface_east()
)
markings.append(
SurfaceMarkingRect(
opposite_marking,
*opposite_center.xy,
angle=opposite_angle,
width=Config.TURN_SF_MARK_WIDTH,
depth=Config.TURN_SF_MARK_LENGTH,
)
)
if not self.invisible:
# Add stop/give way lines
west_line = None
east_line = None
north_line = None
south_line = None
line_type = (
SurfaceMarkingRect.STOP_LINE
if self.rule == Intersection.STOP or self.rule == Intersection.PRIORITY_STOP
else SurfaceMarkingRect.GIVE_WAY_LINE
)
if self.rule == Intersection.EQUAL:
west_line = line_type
east_line = line_type
north_line = line_type
south_line = line_type
elif (
self.rule == Intersection.PRIORITY_YIELD
or self.rule == Intersection.PRIORITY_STOP
):
if self.turn == Intersection.RIGHT:
north_line = line_type
west_line = line_type
elif self.turn == Intersection.LEFT:
north_line = line_type
east_line = line_type
else:
west_line = line_type
east_line = line_type
elif self.rule == Intersection.YIELD or self.rule == Intersection.STOP:
north_line = line_type
south_line = line_type
# These stop lines are always the direction's middle and right line
# going away from the center of the intersection in local coordinates
if west_line is not None and self.closing != Intersection.RIGHT:
markings.append(
_get_stop_line(
Line([Point(self.z - self.u), Point(self.z - self.w)]),
Line(
[
Point(self.z - self.x - self.u),
Point(self.z - self.w - self.v),
]
),
kind=west_line,
)
)
if north_line is not None and self.closing != Intersection.STRAIGHT:
markings.append(
_get_stop_line(
Line(
[Point(self.z + self.x), Point(2 * self.z)]
), # Middle line north in local coords
Line(
[Point(self.z + self.x - self.u), Point(2 * self.z - self.y)]
), # Right line
kind=north_line,
)
)
if south_line is not None:
markings.append(
_get_stop_line(
Line([Point(self.z - self.x), Point(0, 0)]),
Line(
[Point(self.z - self.x + self.u), Point(0, -Config.road_width)]
),
kind=south_line,
)
)
if east_line is not None and self.closing != Intersection.LEFT:
markings.append(
_get_stop_line(
Line([Point(self.z + self.u), Point(self.z + self.w)]),
Line(
[
Point(self.z + self.x + self.u),
Point(self.z + self.w + self.v),
]
),
kind=east_line,
)
)
for marking in markings:
marking.normalize_x = False
marking.set_transform(self.middle_line)
return markings
[docs] def add_dynamic_obstacle(
self,
start: int,
end: int,
y_offset: float = 0.2,
**kwargs,
):
def get_middle_line(direction: int):
if direction == Intersection.ORIGIN:
return self.middle_line_south.parallel_offset(0, "right")
elif direction == Intersection.RIGHT:
return self.middle_line_east
elif direction == Intersection.LEFT:
return self.middle_line_west
elif direction == Intersection.STRAIGHT:
return self.middle_line_north
line = get_middle_line(start).parallel_offset(0, "right") + get_middle_line(end)
if start == Intersection.RIGHT and end == Intersection.ORIGIN and y_offset < 0:
line = get_middle_line(start).parallel_offset(
y_offset, "right"
) + get_middle_line(end).parallel_offset(-y_offset, "right")
else:
line = line.parallel_offset(
y_offset if y_offset > 0 else -y_offset,
"left" if y_offset >= 0 else "right",
)
start_point = Point(0, y_offset)
end_point = Point(line.length, y_offset)
super().add_dynamic_obstacle(
start_point,
end_point,
align_middle_line=False,
align_line=line.parallel_offset(-y_offset, "right"),
**kwargs,
)