"""StaticObstacle on road and ParkingObstacle on ParkingSpot."""
import uuid
from dataclasses import dataclass, field
from typing import List, Optional, Union
from kitcar_utils.geometry import Line, Point, Pose, Transform, Vector
from simulation.utils.road.sections.road_element import RoadElementRect
from . import ID
[docs]@dataclass
class StaticObstacle(RoadElementRect):
"""Obstacle that can be placed on the road.
Args:
arc_length: x coordinate of the element along the road.
y: y coordinate of the element. (Perpendicular to the road.)
width: Width of the element.
depth: Depth of the element. Component of the size in the direction of the road.
height: Height of the element.
angle: Angle [radian] between the middle line and the element
(measured at the center).
"""
id_ = ID.register()
desc = "StaticObstacle"
def __init__(
self,
arc_length: float = 0.4,
y: float = -0.2,
width: float = 0.2,
depth: float = 0.2,
height: float = 0.2,
angle=0,
):
super().__init__(arc_length, y, width, depth, angle)
self.height = height
self.uuid = uuid.uuid4()
def __eq__(self, other):
return self.uuid == other.uuid
[docs]@dataclass
class ParkingObstacle(StaticObstacle):
"""Obstacle that can be placed on a parking spot.
Args:
x: x coordinate of the element along the road.
y: y coordinate of the element. (Perpendicular to the road.)
width: Width of the element.
depth: Depth of the element. Component of the size in the direction of the spot.
height: Height of the element.
angle: Angle [radian] between the parking spot and the element
(measured at the center).
"""
id_ = ID.register()
desc = "ParkingObstacle"
def __init__(
self,
x: float = 0.15,
y: float = -0.15,
width: float = 0.15,
depth: float = 0.15,
height: float = 0.2,
angle=0,
):
super().__init__(x, y, width, depth, height, angle)
self.normalize_x = False
[docs]@dataclass
class DynamicObstacle(StaticObstacle):
"""DynamicObstacle that can be placed on all road sections."""
id_ = ID.register()
width: float = 0.1
"""Width of the obstacle."""
depth: float = 0.1
"""Width of the obstacle."""
height: float = 0.15
"""Height of the obstacle."""
speed: float = 0.6
"""Speed of the obstacle."""
triggered_at_time: float = -1
"""Defines the time at which the obstacle is triggered."""
waiting_time_after_trigger: float = 0
"""Wait for this amount of seconds after first trigger."""
trigger_distance: float = 2
"""Distance to car along the road at which the obstacle should be triggered."""
reset_trigger_distance: float = 5
"""Distance to car along the road at which resets the obstacle."""
current_pose: Optional[Pose] = None
"""The current pose of the obstacle.
Updated via the obstacle controller node
"""
def __init__(
self,
path_points: List[Point] = field(default_factory=list),
width=width,
depth=depth,
height=height,
speed=speed,
triggered_at_time=triggered_at_time,
waiting_time_after_trigger=waiting_time_after_trigger,
trigger_distance=trigger_distance,
reset_trigger_distance=reset_trigger_distance,
align_middle_line: bool = True,
align_line: Optional[Line] = None,
):
self._path_points = path_points
self.speed = speed
self.trigger_distance = trigger_distance
self.reset_trigger_distance = reset_trigger_distance
self.triggered_at_time = triggered_at_time
self.waiting_time_after_trigger = waiting_time_after_trigger
self.align_middle_line = align_middle_line
self.align_line = align_line
start_point = path_points[0]
super().__init__(
start_point.x,
start_point.y,
width=width,
depth=depth,
height=height,
)
self.height = height
[docs] def get_pose(self, time: float):
"""Get pose of obstacle at provided time."""
if self.triggered_at_time < 0:
return self.path.interpolate_pose(0)
return self.path.interpolate_pose(
min((time - self.triggered_at_time) * self.speed, self.path.length)
)
[docs] def setup(self, road, road_section):
if self.align_middle_line:
middle_line = sum((sec.middle_line for sec in road.sections), Line())
self._align_line(middle_line, road_section.prev_length)
elif self.align_line is not None:
self._align_line(self.align_line)
self.path = self.transform * self.path
[docs] def _align_line(self, line, x_offset: int = 0):
"""Align the dynamic obstacle relative to a given line."""
path = Line()
for previous_point, current_point in zip(
self._path_points[:-1], self._path_points[1:]
):
if current_point.x > line.length - x_offset >= previous_point.x:
section = (
Line.cut(line, previous_point.x + x_offset)[1]
+ Line.cut(line, current_point.x + x_offset - line.length)[0]
)
else:
delta_x = current_point.x - previous_point.x
delta_x = delta_x % line.length if delta_x != line.length else delta_x
x_start = previous_point.x + x_offset
x_start = x_start % line.length if x_start != line.length else x_start
section, _ = Line.cut(Line.cut(line, x_start)[1], delta_x)
if current_point.y == previous_point.y:
# If the points have the same y value just use
path = path + section.parallel_offset(-current_point.y, "right")
else:
# Offset each point parallel to the middle line
start = section.parallel_offset(-previous_point.y, "right").coords[0]
end = section.parallel_offset(-current_point.y, "right").coords[-1]
path = path + Line([start, end])
self.path = path
[docs]@dataclass
class Pedestrian(DynamicObstacle):
"""Pedestrian to use at zebra crossing."""
id_ = ID.register()
width: float = 0.1
"""Width of the pedestrian."""
depth: float = 0.1
"""Width of the pedestrian."""
height: float = 0.15
"""Height of the pedestrian."""
speed: float = 0.3
"""Speed of the obstacle."""
triggered_at_time: float = -1
"""Defines the time at which the pedestrian is triggered."""
waiting_time_after_trigger = 1
"""Wait for this amount of seconds after first trigger."""
trigger_distance: float = 2
"""Distance to car along the road at which the pedestrian should start walking."""
reset_trigger_distance: float = trigger_distance + 7
"""Distance to car along the road at which resets the pedestrian."""
def __init__(
self,
arc_length: float = 0.45 / 2,
y: float = -0.5,
):
path = [self._center + Vector(arc_length, y), self._center + Vector(arc_length, -y)]
super().__init__(
path,
self.width,
self.depth,
self.height,
self.speed,
self.triggered_at_time,
self.waiting_time_after_trigger,
self.trigger_distance,
align_middle_line=False,
)
self.normalize_x = False