Source code for simulation.utils.road.sections.intersection

"""Intersection."""

import functools
import math
from dataclasses import dataclass
from typing import List, Tuple

import numpy as np

import simulation.utils.road.sections.type as road_section_type
from simulation.utils.geometry import Line, Point, Polygon, Pose, Vector
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): return np.arange(start, end + step, step)
[docs]@dataclass class Intersection(RoadSection): """Road section representing an intersection. Args: angle (float) = math.pi/2: Angle [radian] between crossing roads. closing (int) = None: Optionally close one direction to create a T-intersection. turn (int) = Intersection.STRAIGHT: Direction in which the road continues. rule (int) = Intersection.EQUAL: Priority-rule at intersection. size (float) = 1.8: Length of the crossing roads. """ TYPE = road_section_type.INTERSECTION 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: str = 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.""" def __post_init__(self): super().__post_init__() self._alpha = self.angle - math.pi / 2 self._closing = self.closing 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! @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 ll(self): return Vector( 0, ( -1 * self.y - self.x - self.u - (2 - 2 * self.sin) / (self.cos * self.cos) * self.v ).y, ) @property def ls(self): return Vector(0, (-1 * self.u + (-1 + self.sin) / (self.cos * self.cos) * self.v).y) @property def rl(self): return Vector( 0, ( self.x + self.u + self.y - (2 + 2 * self.sin) / (self.cos * self.cos) * self.v ).y, ) @property def rs(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 of rendering # TODO remove magic number sign length (0.1), determine real length
[docs] def cp_sign_south(self, sign_dist): return Vector(self.z - self.x + self.u) - Vector( 0.1 + sign_dist, Config.get_sign_road_padding() )
[docs] def cp_surface_south(self): 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): return ( Vector(self.z - self.x - self.u) - Vector((0.1 + sign_dist) * 1 / abs(self.u) * self.u) - Vector(Config.get_sign_road_padding() * 1 / abs(self.v) * self.v) )
[docs] def cp_surface_west(self): return ( Vector(self.z - 0.5 * self.x - self.u) - Vector( (Config.get_surface_mark_dist() + Config.TURN_SF_MARK_LENGTH / 2) * 1 / abs(self.u) * self.u ) - Vector(Config.TURN_SF_MARK_WIDTH / 2 * 1 / abs(self.v) * self.v) )
[docs] def cp_sign_north(self, sign_dist): return Vector(self.z + self.x - self.u) + Vector( 0.1 + sign_dist, Config.get_sign_road_padding() )
[docs] def cp_sign_east(self, sign_dist): return ( Vector(self.z + self.x + self.u) + Vector((0.1 + sign_dist) * 1 / abs(self.u) * self.u) + Vector(Config.get_sign_road_padding() * 1 / abs(self.v) * self.v) )
[docs] def cp_surface_east(self): return ( Vector(self.z + 0.5 * self.x + self.u) + Vector( (Config.get_surface_mark_dist() + Config.TURN_SF_MARK_LENGTH / 2) * 1 / abs(self.u) * self.u ) + Vector(Config.TURN_SF_MARK_WIDTH / 2 * 1 / abs(self.v) * self.v) )
[docs] def get_points(self): return [ self.get_beginning()[0], self.get_ending()[0], ]
# 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)] ) @property def middle_line_east(self) -> Line: return self.transform * Line([Point(self.z + self.u), Point(self.z + self.w)]) @property def left_line_east(self) -> 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: 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: return self.transform * Line([Point(self.z + self.x), Point(2 * self.z)]) @property def left_line_north(self) -> 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: return self.transform * Line( [Point(self.z + self.x + self.u), Point(2 * self.z + self.y)] ) @property def middle_line_west(self) -> Line: return self.transform * Line([Point(self.z - self.u), Point(self.z - self.w)]) @property def left_line_west(self) -> 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: return self.transform * Line( [Point(self.z + self.x - self.u), Point(self.z - self.w + self.v)] ) @property def ls_circle(self) -> Line: if self.turn == Intersection.LEFT: 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.ls - self.ls.rotated(theta))) return self.transform * Line(points_ls) @property def ll_circle(self) -> Line: if self.turn == Intersection.LEFT: 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.ll - self.ll.rotated(theta)) ) return self.transform * Line(points_ll) @property def rs_circle(self) -> Line: if self.turn == Intersection.RIGHT: 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.rs - self.rs.rotated(theta))) return self.transform * Line(points_rs) @property def rl_circle(self) -> Line: if self.turn == Intersection.RIGHT: 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.rl - self.rl.rotated(theta)) ) return self.transform * Line(points_rl) @functools.cached_property def middle_line(self) -> Line: """Line: Middle line of the intersection.""" if self.turn == Intersection.LEFT: return self.middle_line_south + self.ls_circle + self.middle_line_west elif self.turn == Intersection.RIGHT: return self.middle_line_south + self.rs_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]: """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
[docs] def get_beginning(self) -> Tuple[Pose, float]: return (Pose(self.transform * Point(0, 0), self.transform.get_angle() + math.pi), 0)
[docs] def get_ending(self) -> Tuple[Pose, float]: 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 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 * self.size))
[docs] def _get_intersection_traffic_signs(self) -> List[TrafficSign]: 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.YIELD: # 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, ) ) if self.rule != Intersection.YIELD: # 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, ) ) type_map = { Intersection.PRIORITY_YIELD: TrafficSign.PRIORITY, Intersection.PRIORITY_STOP: TrafficSign.PRIORITY, Intersection.YIELD: TrafficSign.YIELD, Intersection.STOP: TrafficSign.STOP, } if self.rule in type_map: signs.append( TrafficSign( type_map[self.rule], *self.cp_sign_south(Config.get_prio_sign_dist(1)).xy, ) ) signs.append( TrafficSign( type_map[self.rule], *self.cp_sign_north(Config.get_prio_sign_dist(1)).xy, angle=math.pi, visible=False, ) ) # stvo-206: Stoppschild, # stvo-306: Vorfahrtsstra├če # todo: also add turning signal if we are not on the outer turn lane # on the opposite side type_map_opposite = { Intersection.PRIORITY_YIELD: TrafficSign.YIELD, Intersection.PRIORITY_STOP: TrafficSign.STOP, Intersection.YIELD: TrafficSign.PRIORITY, Intersection.STOP: TrafficSign.PRIORITY, } if self.rule in type_map_opposite: signs.append( TrafficSign( type_map_opposite[self.rule], *self.cp_sign_west(Config.get_prio_sign_dist(1)).xy, angle=self._alpha - 0.5 * math.pi, visible=False, ) ) signs.append( TrafficSign( type_map_opposite[self.rule], *self.cp_sign_east(Config.get_prio_sign_dist(1)).xy, angle=self._alpha + 0.5 * math.pi, visible=False, ) ) for sign in signs: sign.normalize_x = False sign.set_transform(self.transform) return signs
[docs] def _get_intersection_surface_markings(self) -> List[SurfaceMarkingRect]: 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" 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.YIELD) and (self.rule != Intersection.STOP): 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, ) ) # Add stop lines west_line = None east_line = None north_line = None south_line = None if self.rule == Intersection.EQUAL and self.turn == Intersection.STRAIGHT: west_line = SurfaceMarkingRect.GIVE_WAY_LINE east_line = SurfaceMarkingRect.GIVE_WAY_LINE north_line = SurfaceMarkingRect.GIVE_WAY_LINE south_line = SurfaceMarkingRect.GIVE_WAY_LINE elif ( self.rule == Intersection.PRIORITY_YIELD and self.turn == Intersection.STRAIGHT ): west_line = SurfaceMarkingRect.GIVE_WAY_LINE east_line = SurfaceMarkingRect.GIVE_WAY_LINE elif self.rule == Intersection.PRIORITY_STOP and self.turn == Intersection.STRAIGHT: west_line = SurfaceMarkingRect.STOP_LINE east_line = SurfaceMarkingRect.STOP_LINE elif self.rule == Intersection.YIELD: north_line = SurfaceMarkingRect.GIVE_WAY_LINE south_line = SurfaceMarkingRect.GIVE_WAY_LINE elif self.rule == Intersection.STOP: north_line = SurfaceMarkingRect.STOP_LINE south_line = SurfaceMarkingRect.STOP_LINE # 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: 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: 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: 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