"""StartBox."""
import functools
from dataclasses import dataclass
from kitcar_utils.geometry import Line, Point, Polygon, Pose
from kitcar_utils.geometry.vector import 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 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
) -> str | None:
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)
# A line/line intersection that is a single point is returned as a (kitcar) Vector
# by the geometry layer; an empty geometry means the lines do not cross.
def is_point(geom) -> bool:
return isinstance(geom, Vector) and not geom._geom.is_empty
if is_point(intersection_right) and is_point(intersection_left):
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 is_point(intersection_left):
self.intersection = Point(intersection_left.x, intersection_left.y)
return "left"
elif is_point(intersection_right):
self.intersection = Point(intersection_right.x, intersection_right.y)
return "right"
else:
return None