"""The RoadSection is parent to all other RoadSection classes."""
import functools
import itertools
import math
from dataclasses import dataclass, field
from typing import List, Tuple
from simulation.utils.geometry import Line, Point, Polygon, Pose, Transform, Vector
from simulation.utils.road.config import Config
from simulation.utils.road.sections import 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] @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