"""The RoadSection is parent to all other RoadSection classes."""
import functools
import itertools
import math
from dataclasses import dataclass, field
from typing import List, Optional, Tuple
from kitcar_utils.geometry import Line, Point, Polygon, Pose, Transform, Vector
from simulation.utils.road.config import Config
from simulation.utils.road.sections import (
DynamicObstacle,
StaticObstacle,
SurfaceMarking,
TrafficSign,
)
from simulation.utils.road.sections.speed_limit import SpeedLimit
from simulation.utils.road.sections.transformable import Transformable
[docs]class MarkedLine(Line):
"""Line with a defined line marking style."""
def __init__(self, *args, **kwargs):
assert "style" in kwargs
if "prev_length" not in kwargs:
kwargs["prev_length"] = 0
self.style = kwargs["style"]
self.prev_length = kwargs["prev_length"]
del kwargs["style"]
del kwargs["prev_length"]
super().__init__(*args, **kwargs)
[docs] @classmethod
def from_line(cls, line: Line, style, prev_length=0):
m = cls(style=style, prev_length=prev_length)
m._set_coords(line.coords)
return m
def __repr__(self) -> str:
return super().__repr__()[:-1] + f", style={self.style})"
[docs]@dataclass
class RoadSection(Transformable):
"""Base class of all road sections."""
SOLID_LINE_MARKING = "solid"
"""Continuous white line."""
DASHED_LINE_MARKING = "dashed"
"""Dashed white line."""
MISSING_LINE_MARKING = "missing"
"""No line at all."""
DOUBLE_SOLID_LINE_MARKING = "double_solid"
"""Double solid line."""
DOUBLE_DASHED_LINE_MARKING = "double_dashed"
"""Double dashed line."""
DASHED_SOLID_LINE_MARKING = "dashed_solid"
"""Double line, left dashed, right solid."""
SOLID_DASHED_LINE_MARKING = "solid_dashed"
"""Double line, left solid, right dashed."""
id: int = 0
"""Road section id (consecutive integers by default)."""
is_start: bool = False
"""Road section is beginning of the road."""
left_line_marking: str = SOLID_LINE_MARKING
"""Marking type of the left line."""
middle_line_marking: str = DASHED_LINE_MARKING
"""Marking type of the middle line."""
right_line_marking: str = SOLID_LINE_MARKING
"""Marking type of the right line."""
obstacles: List[StaticObstacle] = field(default_factory=list)
"""Obstacles in the road section."""
traffic_signs: List[TrafficSign] = field(default_factory=list)
"""Traffic signs in the road section."""
surface_markings: List[SurfaceMarking] = field(default_factory=list)
"""Surface markings in the road section."""
_speed_limits: List[SpeedLimit] = field(default_factory=list)
"""Speed limits in the road section."""
TYPE = None
"""Type of the road section."""
prev_length: float = 0
"""Length of Road up to this section."""
def __post_init__(self):
assert (
self.__class__.TYPE is not None
), "Subclass of RoadSection missing TYPE declaration!"
super().__post_init__()
self.set_transform(self.transform)
@functools.cached_property
def middle_line(self) -> Line:
"""Line: Middle line of the road section."""
return Line()
@property
def left_line(self) -> Line:
"""Line: Left line of the road section."""
return self.middle_line.parallel_offset(Config.road_width, "left")
@property
def right_line(self) -> Line:
"""Line: Right line of the road section."""
return self.middle_line.parallel_offset(Config.road_width, "right")
@property
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.middle_line_marking, self.prev_length
)
)
lines.append(
MarkedLine.from_line(self.right_line, self.right_line_marking, self.prev_length)
)
return lines
@property
def speed_limits(self) -> List[SpeedLimit]:
"""Speed limits in the road section."""
return self._speed_limits
[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 * Config.road_width))
[docs] def get_beginning(self) -> Tuple[Pose, float]:
"""Get the beginning of the section 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.
"""
pose = Transform([0, 0], math.pi) * self.middle_line.interpolate_pose(arc_length=0)
curvature = self.middle_line.interpolate_curvature(arc_length=0)
return (pose, curvature)
[docs] def get_ending(self) -> Tuple[Pose, float]:
"""Get the ending of the section as a pose and the curvature.
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.
"""
pose = self.middle_line.interpolate_pose(arc_length=self.middle_line.length)
curvature = self.middle_line.interpolate_curvature(
arc_length=self.middle_line.length
)
return (pose, curvature)
[docs] def add_speed_limit(self, arc_length: float, speed: int):
"""Add a speed limit to this road section.
Args:
arc_length: Direction along the road to the speed limit.
speed: Speed limit. Negative values correspond to the end of a speed limit zone.
"""
speed_limit = SpeedLimit(arc_length, limit=speed)
sm = speed_limit.surface_marking
ts = speed_limit.traffic_sign
sm.set_transform(self.middle_line)
ts.set_transform(self.middle_line)
self.speed_limits.append(speed_limit)
self.surface_markings.append(sm)
self.traffic_signs.append(ts)
return ts
[docs] def add_obstacle(
self,
arc_length: float = 0.2,
y_offset: float = -0.2,
angle: float = 0,
width: float = 0.2,
length: float = 0.3,
height: float = 0.25,
):
"""Add an obstacle to the road.
Args:
arc_length: Direction along the road to the obstacle.
y_offset: Offset orthogonal to the middle line.
angle: Orientation offset of the obstacle.
width: Width of the obstacle.
length: Length of the obstacle.
height: Heigth of the obstacle.
"""
o = StaticObstacle(
arc_length=arc_length,
y=y_offset,
angle=angle,
width=width,
depth=length,
height=height,
)
o.set_transform(self.middle_line)
self.obstacles.append(o)
return o
[docs] def add_dynamic_obstacle(
self,
*path: List[Point],
width: float = 0.2,
length: float = 0.3,
height: float = 0.25,
speed: float = 0.6,
align_middle_line: bool = True,
align_line: Optional[Line] = None,
trigger_distance: float = 2,
waiting_time_after_trigger: float = 0,
reset_trigger_distance: float = 2,
):
"""Add an obstacle to the road."""
o = DynamicObstacle(
path_points=path,
width=width,
depth=length,
height=height,
speed=speed,
waiting_time_after_trigger=waiting_time_after_trigger,
trigger_distance=trigger_distance,
align_middle_line=align_middle_line,
align_line=align_line,
reset_trigger_distance=reset_trigger_distance,
)
o.set_transform(self.middle_line)
self.obstacles.append(o)
return o
[docs] def setup_dynamic_obstacles(self, road):
for obstacle in self.obstacles:
if isinstance(obstacle, DynamicObstacle):
obstacle.setup(road, self)
[docs] @classmethod
def fit_ending(
_, current_ending: Pose, desired_ending: Pose, control_point_distance=0.4
) -> "RoadSection":
"""Add a cubic bezier curve to adjust the current ending to equal a desired ending.
Args:
current_ending: Current ending of the last section.
desired_ending: Ending that the last section should have.
control_point_distance: Distance to the bezier curve's control points.
"""
from .bezier_curve import CubicBezier # Import here to prevent cyclic dependency!
current_tf = Transform(current_ending.position, current_ending.orientation)
d_pose = current_tf.inverse * desired_ending
cb = CubicBezier(
p1=Point(control_point_distance, 0),
p2=d_pose.position
- Vector(control_point_distance, 0).rotated(d_pose.orientation),
p3=d_pose.position,
)
return cb