def split_by(self,
sections: List[Tuple[int, RoadSection]],
x_buffer=1) -> List[RoadSection]:
"""Add a number of other sections along this section.
Args:
sections: Road sections and arc lengths at which they should be added.
x_buffer: Buffer after a road section to improve the adjustment of the end
pose.
Return:
New road sections that replace this section.
"""
if len(sections) == 0:
return [self]
assert 0 < sections[0][0] < self.middle_line.length
x_beginning, section = sections[0]
x_end = x_beginning + section.middle_line.length
x_buffered_end = x_end + x_buffer
start_pose = self.middle_line.interpolate_pose(x_beginning)
end_pose = self.middle_line.interpolate_pose(x_buffered_end)
beginning = CustomSection(middle_line_points=Line.cut(
self.middle_line, x_beginning)[0].get_points())
intermediate_section = RoadSection.fit_ending(
Transform(start_pose.position, start_pose.orientation) *
(section.get_ending()[0]),
end_pose,
)
tf = Transform(end_pose.position, end_pose.orientation).inverse
remaining_custom_section = CustomSection(middle_line_points=[
tf * p for p in Line.cut(self.middle_line, x_buffered_end)
[1].get_points()
])
def distribute_objects(attribute_name: str):
"""Distribute objects (traffic signs, obstacles, surface_markings, ...) to
resulting sections."""
for obj in self.__getattribute__(attribute_name):
x = self.middle_line.project(obj.center)
if x < x_beginning:
x_shift = 0
beginning.__getattribute__(attribute_name).append(obj)
elif x_beginning < x < x_end:
x_shift = -x_beginning
section.__getattribute__(attribute_name).append(obj)
elif x_end < x < x_buffered_end:
x_shift = -x_end
intermediate_section.__getattribute__(
attribute_name).append(obj)
else:
x_shift = -x_buffered_end
remaining_custom_section.__getattribute__(
attribute_name).append(obj)
obj._frame = Transform([x_shift, 0], 0) * obj._frame
distribute_objects("obstacles")
distribute_objects("surface_markings")
distribute_objects("traffic_signs")
return [
beginning,
section,
intermediate_section,
] + remaining_custom_section.split_by(
sections=[(x - x_buffered_end, section)
for x, section in sections[1:]])
def driving_line(self) -> Tuple[Line, List[List[float]]]:
"""Tuple[Line, List[List[float]]]: Line where car drives.
And points to stop at.
"""
path = Line()
stops = []
def append(offset, segment, stop):
nonlocal path
if offset > 0:
segment = segment.parallel_offset(offset, "left")
elif offset < 0:
segment = segment.parallel_offset(-offset, "right")
path += segment
if stop > 0:
stops.append([path.length, stop])
if self.param.randomize_path:
# Stitch a line together from varied offsets along the middle line
length = self.middle_line.length
x = 0
offset = 0
max_step = 4
road_width = 0.4
while x < length:
offset = max(
min((0.5 - random.random()) * 2 * road_width, road_width),
-road_width,
)
p = self.middle_line.interpolate_pose(x)
orth = Vector(1, 0, 0).rotated(p.get_angle() + math.pi / 2)
path += Line([p.position + offset * orth, p.position + offset * orth])
x += max_step * random.random()
else:
param_path: List[Dict[str, float]] = self.param.path
param_path = [obj for obj in param_path if "offset" in obj]
current_start = param_path[0]["start"]
current_offset = param_path[0]["offset"]
current_stop = 0
param_path.remove(param_path[0])
# Read the path from the parameters
for obj in param_path:
end_arc_length = obj["start"]
before_end_line = Line.cut(self.middle_line, end_arc_length)[0]
current_segment = Line.cut(before_end_line, current_start)[1]
append(current_offset, current_segment, current_stop)
current_offset = obj["offset"]
current_start = obj["start"]
current_stop = obj.get("stop", 0)
current_segment = Line.cut(self.middle_line, current_start)[1]
append(current_offset, current_segment, 0)
return path.simplify(0.05).smooth(0.1), stops