"""Road class used to define roads as a python class.
A simulated road can be defined through an object of type Road. It contains all sections of
that road as a list.
"""
import importlib
import math
import os
import random
from dataclasses import dataclass, field
from typing import List, Tuple
from kitcar_utils.geometry import Line, Pose, Transform, Vector
from simulation.utils.road.sections import Intersection
from simulation.utils.road.sections.road_section import RoadSection
from simulation.utils.road.sections.start_box import StartBox
[docs]@dataclass
class Road:
"""Container object for roads.
A road consists of multiple road sections that are concatenated. The sections attribute
contains these sections in the correct order.
"""
_name: str = field(default=None, init=False)
"""Name of the road.
The name attribute is determined by the name of the file. It is filled in when the road
is generated.
"""
_seed: str = field(default=None, init=False)
"""Seed used when generating the road.
Determined when generating the road.
"""
use_seed: bool = True
r"""Use a default seed if none is provided.
By default the `use_seed` attribute is true and a seed is set before creating \ the
road. If `use_seed` is set to False, the seed is invalidated and there will \ be
different random values every time the road is created.
"""
sections: List[RoadSection] = field(default_factory=list)
"""All sections of the road."""
length: float = 0
"""Length of road."""
start_box: bool = False
"""Start road with Start Box."""
def __post_init__(self):
"""Set random seed if specified."""
if not self.use_seed:
# Overwrite seed
random.seed()
[docs] def append(self, section: RoadSection):
"""Append a road section. Determine id of the section.
Args:
section: New road section.
"""
section.id = len(self.sections)
if section.id == 0:
section._is_start = True
if section.id > 0:
# Pass ending of last section as the transformation to next section
ending: Tuple[Pose, float] = self.sections[-1].get_ending()
section.set_transform(Transform(ending[0], ending[0].orientation))
section.prev_length = self.length
self.length = self.length + section.middle_line.length
self.sections.append(section)
[docs] def close_to_section(self, target_pose: Pose, p_curvature: float = 2):
"""Append a road section that connects the last section to the given target section.
The road's beginning and it's end are connected using a cubic bezier curve.
Args:
p_curvature: Scale the curvature of the resulting loop.
"""
current_pose, _ = self.sections[-1].get_ending()
# Distance from current to target pose / p_curvature
approximate_radius = (
abs(Vector(current_pose.position) - Vector(target_pose.position)) / p_curvature
)
section = RoadSection.fit_ending(current_pose, target_pose, approximate_radius)
self.append(section)
[docs] def close_loop(self, p_curvature: float = 2):
"""Append a road section that connects the last section to the beginning.
The road's end and it's beginning are connected using a cubic bezier curve.
Args:
p_curvature: Scale the curvature of the resulting loop.
"""
self.close_to_section(Pose([0, 0], 0), p_curvature)
[docs] def close_intersection(
self,
intersection: Intersection,
ending: int,
turn_direction: int = Intersection.STRAIGHT,
p_curvature: float = 2,
):
"""Append a road section that connects the last section of the road to the given
ending of a intersection.
The road's end and the intersection are connected using a cubic bezier curve.
Args:
intersection: The intersection object to which the bezier curve connects.
ending: The side of the (initial) intersection the bezier curve connects to.
turn_direction: Direction in which the car should turn when crossing the
intersection a second time.
p_curvature: Scale the curvature of the resulting loop.
"""
destination_pose, _ = intersection.get_ending(ending)
# Rotate destination pose by 180 degrees
destination_pose = Pose(
destination_pose.position, destination_pose.get_angle() + math.pi
)
closing = None
rule_map_opposite = {
Intersection.PRIORITY_YIELD: Intersection.YIELD,
Intersection.PRIORITY_STOP: Intersection.STOP,
Intersection.YIELD: Intersection.PRIORITY_YIELD,
Intersection.STOP: Intersection.PRIORITY_STOP,
Intersection.EQUAL: Intersection.EQUAL,
}
if ending == Intersection.STRAIGHT:
angle = intersection.angle
rule = intersection.rule
if intersection.closing == Intersection.RIGHT:
closing = Intersection.LEFT
if intersection.closing == Intersection.LEFT:
closing = Intersection.RIGHT
elif ending == Intersection.RIGHT:
angle = math.pi - intersection.angle
rule = rule_map_opposite[intersection.rule]
if intersection.closing == Intersection.STRAIGHT:
closing = Intersection.RIGHT
if intersection.closing == Intersection.LEFT:
closing = Intersection.STRAIGHT
elif ending == Intersection.LEFT:
angle = math.pi - intersection.angle
rule = rule_map_opposite[intersection.rule]
if intersection.closing == Intersection.RIGHT:
closing = Intersection.STRAIGHT
elif intersection.closing == Intersection.STRAIGHT:
closing = Intersection.LEFT
self.close_to_section(destination_pose, p_curvature)
self.append(
Intersection(
turn=turn_direction,
angle=angle,
size=intersection.size,
closing=closing,
invisible=True,
rule=rule,
)
)
[docs] def setup_dynamic_obstacles(self):
for section in self.sections:
section.setup_dynamic_obstacles(self)
[docs] def finalize(self):
if self.start_box and not any(s for s in self.sections if isinstance(s, StartBox)):
sb = StartBox()
road_origin, _ = self.sections[0].get_beginning()
left_line = sum((s.left_line for s in self.sections), Line())
right_line = sum((s.right_line for s in self.sections), Line())
sb.setup(road_origin, left_line, right_line)
self.append(sb)
[docs] def get_start_box(self):
if self.start_box:
return self.sections[-1]
return None
DEFAULT_ROAD_DIR = os.path.join(
os.environ.get("KITCAR_REPO_PATH"),
"kitcar-gazebo-simulation",
"simulation",
"models",
"env_db",
)
[docs]def _get_module(name: str):
if not os.path.isabs(name):
name = os.path.join(DEFAULT_ROAD_DIR, name)
if not name.endswith(".py"):
name += ".py"
# Remove .py
module_name = os.path.basename(name)[:-3]
try:
spec = importlib.util.spec_from_file_location(module_name, name)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
return module, name, module_name
except AttributeError or ModuleNotFoundError:
raise ValueError(f"Road {module_name} not found at {name}.")
[docs]def load(road_name: str, seed: str = "KITCAR") -> Road:
"""Load road object from file.
Args:
road_name: Name of the file containing the road definition.
seed: Predetermine random values.
"""
# Set random seed
# set it at this point because the module is reloaded afterwards
# the above import is just to ensure that the road is in the module cache
random.seed(seed)
# Ensure that the road is up to date
road_module, file_path, road_name = _get_module(road_name)
road = road_module.road
road._file_path = file_path
road._name = road_name
road._seed = seed
road.setup_dynamic_obstacles()
road.finalize()
return road_module.road