"""StartBox."""
import functools
from dataclasses import dataclass
from typing import Optional
from kitcar_utils.geometry import Line, Point, Polygon, Pose
from kitcar_utils.geometry.vector import Vector
from shapely.geometry import Point as ShapelyPoint
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 RoadSection
[docs]class StartBoxError(Exception):
pass
[docs]@dataclass
class StartBox(RoadSection):
"""Road section representing the start box.
Todo:
* Change TYPE?
"""
TYPE = road_section_type.CUSTOM
left_line_marking: str = RoadSection.MISSING_LINE_MARKING
"""Marking type of the left line."""
middle_line_marking: str = RoadSection.SOLID_LINE_MARKING
"""Marking type of the middle line."""
right_line_marking: str = RoadSection.SOLID_LINE_MARKING
"""Marking type of the right line."""
[docs] def setup(self, road_origin: Pose, left_line: Line, right_line: Line):
intersection_side = self.get_intersection(road_origin, left_line, right_line)
if intersection_side is None:
raise StartBoxError(
"Unable to generate startbox. No intersection with end of road possible"
)
"""
elif intersection_side == "left":
self.right_end = self.intersection
self.right_start = self.right_end + Vector(-1, 0)
self.left_start = self.right_end + Vector(0, -Config.road_width)
else: # intersection_side == "right"
self.left_end = self.intersection
self.left_start = self.left_end + Vector(-1, 0)
self.right_start = self.left_start + Vector(0, -Config.road_width)
self.right_end = road_origin.position + Vector(0, -Config.road_width)
"""
self.left_end = self.intersection
self.left_start = self.left_end + Vector(-1, 0)
self.right_start = self.left_start + Vector(0, -Config.road_width)
self.right_end = road_origin.position + Vector(0, -Config.road_width)
self.startbox_position = self.left_end + Vector(-0.5, -Config.road_width / 2)
self.startbox_gate_position = self.left_end + Vector(-0.5, -Config.road_width)
self.car_position = self.left_start + Vector(0, -Config.road_width / 2)
@functools.cached_property
def middle_line(self) -> Line:
return Line([self.left_start, self.left_end])
@property
def left_line(self) -> Line:
"""Line: Left line of the road section."""
return Line()
@functools.cached_property
def right_line(self) -> Line:
return Line([self.right_start, self.right_end])
@property
def frame(self) -> Polygon:
"""Polygon: Frame of the blocked area surface marking.
It has the shape of a symmetrical trapezoid.
"""
return Polygon([self.right_start, self.right_end, self.left_end, self.left_start])
[docs] def get_intersection(
self, road_origin: Pose, left_line: Line, right_line: Line
) -> Optional[str]:
test_line = Line([road_origin.position, road_origin.position + Vector(-4, 0)])
intersection_right = right_line.intersection(test_line)
intersection_left = left_line.intersection(test_line)
if issubclass(type(intersection_right), ShapelyPoint) and issubclass(
type(intersection_left), ShapelyPoint
):
distance_intersection_right = road_origin.position - Vector(
intersection_right.x, intersection_right.y
)
distance_intersection_left = road_origin.position - Vector(
intersection_left.x, intersection_left.y
)
if distance_intersection_left < distance_intersection_right:
self.intersection = Point(intersection_left.x, intersection_left.y)
return "left"
else:
self.intersection = Point(intersection_right.x, intersection_right.y)
return "right"
elif issubclass(type(intersection_left), ShapelyPoint):
self.intersection = Point(intersection_left.x, intersection_left.y)
return "left"
elif issubclass(type(intersection_right), ShapelyPoint):
self.intersection = Point(intersection_right.x, intersection_right.y)
return "right"
else:
return None