mirror of
https://github.com/pim-n/pg-rad
synced 2026-03-23 21:58:12 +01:00
let segmented road generator take specific angles and lengths for segments
This commit is contained in:
@ -11,7 +11,6 @@ from .specs import (
|
|||||||
MetadataSpec,
|
MetadataSpec,
|
||||||
RuntimeSpec,
|
RuntimeSpec,
|
||||||
SimulationOptionsSpec,
|
SimulationOptionsSpec,
|
||||||
SegmentSpec,
|
|
||||||
PathSpec,
|
PathSpec,
|
||||||
ProceduralPathSpec,
|
ProceduralPathSpec,
|
||||||
CSVPathSpec,
|
CSVPathSpec,
|
||||||
@ -122,7 +121,7 @@ class ConfigParser:
|
|||||||
|
|
||||||
def _parse_path(self) -> PathSpec:
|
def _parse_path(self) -> PathSpec:
|
||||||
allowed_csv = {"file", "east_col_name", "north_col_name", "z"}
|
allowed_csv = {"file", "east_col_name", "north_col_name", "z"}
|
||||||
allowed_proc = {"segments", "length", "z"}
|
allowed_proc = {"segments", "length", "z", "alpha"}
|
||||||
|
|
||||||
path = self.config.get("path")
|
path = self.config.get("path")
|
||||||
if path is None:
|
if path is None:
|
||||||
@ -172,13 +171,15 @@ class ConfigParser:
|
|||||||
if isinstance(raw_length, int | float):
|
if isinstance(raw_length, int | float):
|
||||||
raw_length = [float(raw_length)]
|
raw_length = [float(raw_length)]
|
||||||
|
|
||||||
segments = self._process_segment_angles(raw_segments)
|
segments, angles = self._process_segment_angles(raw_segments)
|
||||||
lengths = self._process_segment_lengths(raw_length, len(segments))
|
lengths = self._process_segment_lengths(raw_length, len(segments))
|
||||||
resolved_segments = self._combine_segments_lengths(segments, lengths)
|
|
||||||
|
|
||||||
return ProceduralPathSpec(
|
return ProceduralPathSpec(
|
||||||
segments=resolved_segments,
|
segments=segments,
|
||||||
|
angles=angles,
|
||||||
|
lengths=lengths,
|
||||||
z=path.get("z", defaults.DEFAULT_PATH_HEIGHT),
|
z=path.get("z", defaults.DEFAULT_PATH_HEIGHT),
|
||||||
|
alpha=path.get("alpha", defaults.DEFAULT_ALPHA)
|
||||||
)
|
)
|
||||||
|
|
||||||
def _process_segment_angles(
|
def _process_segment_angles(
|
||||||
@ -186,23 +187,25 @@ class ConfigParser:
|
|||||||
raw_segments: List[Union[str, dict]]
|
raw_segments: List[Union[str, dict]]
|
||||||
) -> List[Dict[str, Any]]:
|
) -> List[Dict[str, Any]]:
|
||||||
|
|
||||||
normalized = []
|
segments, angles = [], []
|
||||||
|
|
||||||
for segment in raw_segments:
|
for segment in raw_segments:
|
||||||
|
|
||||||
if isinstance(segment, str):
|
if isinstance(segment, str):
|
||||||
normalized.append({"type": segment, "angle": None})
|
segments.append(segment)
|
||||||
|
angles.append(None)
|
||||||
|
|
||||||
elif isinstance(segment, dict):
|
elif isinstance(segment, dict):
|
||||||
if len(segment) != 1:
|
if len(segment) != 1:
|
||||||
raise ValueError("Invalid segment definition.")
|
raise ValueError("Invalid segment definition.")
|
||||||
seg_type, angle = list(segment.items())[0]
|
seg_type, angle = list(segment.items())[0]
|
||||||
normalized.append({"type": seg_type, "angle": angle})
|
segments.append(seg_type)
|
||||||
|
angles.append(angle)
|
||||||
|
|
||||||
else:
|
else:
|
||||||
raise ValueError("Invalid segment entry format.")
|
raise ValueError("Invalid segment entry format.")
|
||||||
|
|
||||||
return normalized
|
return segments, angles
|
||||||
|
|
||||||
def _process_segment_lengths(
|
def _process_segment_lengths(
|
||||||
self,
|
self,
|
||||||
@ -219,32 +222,6 @@ class ConfigParser:
|
|||||||
"number of elements equal to the number of segments."
|
"number of elements equal to the number of segments."
|
||||||
)
|
)
|
||||||
|
|
||||||
def _combine_segments_lengths(
|
|
||||||
self,
|
|
||||||
segments: List[Dict[str, Any]],
|
|
||||||
lengths: List[float],
|
|
||||||
) -> List[SegmentSpec]:
|
|
||||||
|
|
||||||
resolved = []
|
|
||||||
|
|
||||||
for seg, length in zip(segments, lengths):
|
|
||||||
angle = seg["angle"]
|
|
||||||
|
|
||||||
if angle is not None and not self._is_turn(seg["type"]):
|
|
||||||
raise ValueError(
|
|
||||||
f"A {seg['type']} segment does not support an angle."
|
|
||||||
)
|
|
||||||
|
|
||||||
resolved.append(
|
|
||||||
SegmentSpec(
|
|
||||||
type=seg["type"],
|
|
||||||
length=length,
|
|
||||||
angle=angle,
|
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
return resolved
|
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def _is_turn(segment_type: str) -> bool:
|
def _is_turn(segment_type: str) -> bool:
|
||||||
return segment_type in {"turn_left", "turn_right"}
|
return segment_type in {"turn_left", "turn_right"}
|
||||||
|
|||||||
@ -15,17 +15,10 @@ class RuntimeSpec:
|
|||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class SimulationOptionsSpec:
|
class SimulationOptionsSpec:
|
||||||
air_density: float = 1.243
|
air_density: float
|
||||||
seed: int | None = None
|
seed: int | None = None
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
|
||||||
class SegmentSpec:
|
|
||||||
type: str
|
|
||||||
length: float
|
|
||||||
angle: float | None
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class PathSpec(ABC):
|
class PathSpec(ABC):
|
||||||
pass
|
pass
|
||||||
@ -33,8 +26,11 @@ class PathSpec(ABC):
|
|||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class ProceduralPathSpec(PathSpec):
|
class ProceduralPathSpec(PathSpec):
|
||||||
segments: list[SegmentSpec]
|
segments: list[str]
|
||||||
|
angles: list[float]
|
||||||
|
lengths: list[int | None]
|
||||||
z: int | float
|
z: int | float
|
||||||
|
alpha: float
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
|
|||||||
@ -56,23 +56,24 @@ class LandscapeBuilder:
|
|||||||
self,
|
self,
|
||||||
sim_spec: SimulationSpec
|
sim_spec: SimulationSpec
|
||||||
):
|
):
|
||||||
|
|
||||||
segments = sim_spec.path.segments
|
segments = sim_spec.path.segments
|
||||||
types = [s.type for s in segments]
|
lengths = sim_spec.path.lengths
|
||||||
lengths = [s.length for s in segments]
|
angles = sim_spec.path.angles
|
||||||
angles = [s.angle for s in segments]
|
alpha = sim_spec.path.alpha
|
||||||
|
|
||||||
|
print(segments, lengths, angles)
|
||||||
|
|
||||||
sg = SegmentedRoadGenerator(
|
sg = SegmentedRoadGenerator(
|
||||||
length=lengths,
|
|
||||||
ds=sim_spec.runtime.speed * sim_spec.runtime.acquisition_time,
|
ds=sim_spec.runtime.speed * sim_spec.runtime.acquisition_time,
|
||||||
velocity=sim_spec.runtime.speed,
|
velocity=sim_spec.runtime.speed,
|
||||||
seed=sim_spec.options.seed
|
seed=sim_spec.options.seed
|
||||||
)
|
)
|
||||||
|
|
||||||
x, y = sg.generate(
|
x, y = sg.generate(
|
||||||
segments=types,
|
segments=segments,
|
||||||
lengths=lengths,
|
lengths=lengths,
|
||||||
angles=angles
|
angles=angles,
|
||||||
|
alpha=alpha
|
||||||
)
|
)
|
||||||
|
|
||||||
self._path = Path(list(zip(x, y)))
|
self._path = Path(list(zip(x, y)))
|
||||||
|
|||||||
@ -7,7 +7,6 @@ class BaseRoadGenerator:
|
|||||||
"""A base generator object for generating a road of a specified length."""
|
"""A base generator object for generating a road of a specified length."""
|
||||||
def __init__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
length: int | float,
|
|
||||||
ds: int | float,
|
ds: int | float,
|
||||||
velocity: int | float,
|
velocity: int | float,
|
||||||
mu: float = 0.7,
|
mu: float = 0.7,
|
||||||
@ -17,7 +16,6 @@ class BaseRoadGenerator:
|
|||||||
"""Initialize a BaseGenerator with a given or random seed.
|
"""Initialize a BaseGenerator with a given or random seed.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
length (int | float): The total length of the road in meters.
|
|
||||||
ds (int | float): The step size in meters.
|
ds (int | float): The step size in meters.
|
||||||
velocity (int | float): Velocity in meters per second.
|
velocity (int | float): Velocity in meters per second.
|
||||||
mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
|
mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
|
||||||
@ -31,9 +29,6 @@ class BaseRoadGenerator:
|
|||||||
if not isinstance(seed, int):
|
if not isinstance(seed, int):
|
||||||
raise TypeError("seed must be an integer or None.")
|
raise TypeError("seed must be an integer or None.")
|
||||||
|
|
||||||
if not isinstance(length, int | float):
|
|
||||||
raise TypeError("Length must be an integer or float in meters.")
|
|
||||||
|
|
||||||
if not isinstance(ds, int | float):
|
if not isinstance(ds, int | float):
|
||||||
raise TypeError("Step size must be integer or float in meters.")
|
raise TypeError("Step size must be integer or float in meters.")
|
||||||
|
|
||||||
@ -42,7 +37,6 @@ class BaseRoadGenerator:
|
|||||||
"Velocity must be integer or float in meters per second."
|
"Velocity must be integer or float in meters per second."
|
||||||
)
|
)
|
||||||
|
|
||||||
self.length = length
|
|
||||||
self.ds = ds
|
self.ds = ds
|
||||||
|
|
||||||
self.velocity = velocity
|
self.velocity = velocity
|
||||||
|
|||||||
@ -16,7 +16,6 @@ logger = logging.getLogger(__name__)
|
|||||||
class SegmentedRoadGenerator(BaseRoadGenerator):
|
class SegmentedRoadGenerator(BaseRoadGenerator):
|
||||||
def __init__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
length: int | float | list[int | float],
|
|
||||||
ds: int | float,
|
ds: int | float,
|
||||||
velocity: int | float,
|
velocity: int | float,
|
||||||
mu: float = 0.7,
|
mu: float = 0.7,
|
||||||
@ -26,7 +25,6 @@ class SegmentedRoadGenerator(BaseRoadGenerator):
|
|||||||
"""Initialize a SegmentedRoadGenerator with given or random seed.
|
"""Initialize a SegmentedRoadGenerator with given or random seed.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
length (int | float): The total length of the road in meters.
|
|
||||||
ds (int | float): The step size in meters.
|
ds (int | float): The step size in meters.
|
||||||
velocity (int | float): Velocity in meters per second.
|
velocity (int | float): Velocity in meters per second.
|
||||||
mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
|
mu (float): Coefficient of friction. Defaults to 0.7 (dry asphalt).
|
||||||
@ -34,18 +32,13 @@ class SegmentedRoadGenerator(BaseRoadGenerator):
|
|||||||
seed (int | None, optional): Set a seed for the generator.
|
seed (int | None, optional): Set a seed for the generator.
|
||||||
Defaults to random seed.
|
Defaults to random seed.
|
||||||
"""
|
"""
|
||||||
|
super().__init__(ds, velocity, mu, g, seed)
|
||||||
if isinstance(length, list):
|
|
||||||
length = sum(
|
|
||||||
[seg_len for seg_len in length if seg_len is not None]
|
|
||||||
)
|
|
||||||
super().__init__(length, ds, velocity, mu, g, seed)
|
|
||||||
|
|
||||||
def generate(
|
def generate(
|
||||||
self,
|
self,
|
||||||
segments: list[str],
|
segments: list[str],
|
||||||
lengths: list[int | float] | None = None,
|
lengths: list[int | float],
|
||||||
angles: list[int | float] | None = None,
|
angles: list[float | None],
|
||||||
alpha: float = defaults.DEFAULT_ALPHA,
|
alpha: float = defaults.DEFAULT_ALPHA,
|
||||||
min_turn_angle: float = defaults.DEFAULT_MIN_TURN_ANGLE,
|
min_turn_angle: float = defaults.DEFAULT_MIN_TURN_ANGLE,
|
||||||
max_turn_angle: float = defaults.DEFAULT_MAX_TURN_ANGLE
|
max_turn_angle: float = defaults.DEFAULT_MAX_TURN_ANGLE
|
||||||
@ -54,6 +47,8 @@ class SegmentedRoadGenerator(BaseRoadGenerator):
|
|||||||
|
|
||||||
Args:
|
Args:
|
||||||
segments (list[str]): List of segments.
|
segments (list[str]): List of segments.
|
||||||
|
lengths (list[int | float]): List of segment lengths.
|
||||||
|
angles (list[float | None]): List of angles.
|
||||||
alpha (float, optional): Dirichlet concentration parameter.
|
alpha (float, optional): Dirichlet concentration parameter.
|
||||||
A higher value leads to more uniform apportionment of the
|
A higher value leads to more uniform apportionment of the
|
||||||
length amongst the segments, while a lower value allows more
|
length amongst the segments, while a lower value allows more
|
||||||
@ -88,26 +83,29 @@ class SegmentedRoadGenerator(BaseRoadGenerator):
|
|||||||
|
|
||||||
self.segments = segments
|
self.segments = segments
|
||||||
self.alpha = alpha
|
self.alpha = alpha
|
||||||
num_points = np.ceil(self.length / self.ds).astype(int)
|
|
||||||
|
total_length = sum(lengths)
|
||||||
|
num_points = np.ceil(total_length / self.ds).astype(int)
|
||||||
|
|
||||||
# divide num_points into len(segments) randomly sized parts.
|
# divide num_points into len(segments) randomly sized parts.
|
||||||
if isinstance(self.length, list):
|
if len(lengths) == len(segments):
|
||||||
parts = self.length
|
parts = np.array([seg_len / total_length for seg_len in lengths])
|
||||||
else:
|
else:
|
||||||
parts = self._rng.dirichlet(
|
parts = self._rng.dirichlet(
|
||||||
np.full(len(segments), alpha),
|
np.full(len(segments), alpha),
|
||||||
size=1)[0]
|
size=1)[0]
|
||||||
parts = parts * num_points
|
|
||||||
parts = np.round(parts).astype(int)
|
|
||||||
|
|
||||||
# correct round off so the sum of parts is still total length L.
|
parts = parts * num_points
|
||||||
if sum(parts) != num_points:
|
parts = np.round(parts).astype(int)
|
||||||
parts[0] += num_points - sum(parts)
|
|
||||||
|
# correct round off so the sum of parts is still total length L.
|
||||||
|
if sum(parts) != num_points:
|
||||||
|
parts[0] += num_points - sum(parts)
|
||||||
|
|
||||||
curvature = np.zeros(num_points)
|
curvature = np.zeros(num_points)
|
||||||
current_index = 0
|
current_index = 0
|
||||||
|
|
||||||
for seg_name, seg_length in zip(segments, parts):
|
for seg_name, seg_length, seg_angle in zip(segments, parts, angles):
|
||||||
seg_function = prefabs.PREFABS[seg_name]
|
seg_function = prefabs.PREFABS[seg_name]
|
||||||
|
|
||||||
if seg_name == 'straight':
|
if seg_name == 'straight':
|
||||||
@ -128,12 +126,15 @@ class SegmentedRoadGenerator(BaseRoadGenerator):
|
|||||||
f"({R_min}, {R_max_angle})"
|
f"({R_min}, {R_max_angle})"
|
||||||
)
|
)
|
||||||
|
|
||||||
rand_radius = self._rng.uniform(R_min, R_max_angle)
|
if seg_angle:
|
||||||
|
radius = seg_length / np.deg2rad(seg_angle)
|
||||||
|
else:
|
||||||
|
radius = self._rng.uniform(R_min, R_max_angle)
|
||||||
|
|
||||||
if seg_name.startswith("u_turn"):
|
if seg_name.startswith("u_turn"):
|
||||||
curvature_s = seg_function(rand_radius)
|
curvature_s = seg_function(radius)
|
||||||
else:
|
else:
|
||||||
curvature_s = seg_function(seg_length, rand_radius)
|
curvature_s = seg_function(seg_length, radius)
|
||||||
|
|
||||||
curvature[current_index:(current_index + seg_length)] = curvature_s
|
curvature[current_index:(current_index + seg_length)] = curvature_s
|
||||||
current_index += seg_length
|
current_index += seg_length
|
||||||
|
|||||||
Reference in New Issue
Block a user