def to_lanelet(self, precision: float = 0.5) -> ConversionLanelet:
"""Convert a ParametricLaneGroup to a Lanelet.
Args:
precision: Number which indicates at which space interval (in curve parameter ds)
the coordinates of the boundaries should be calculated.
mirror_border: Which lane to mirror, if performing merging or splitting of lanes.
distance: Distance at start and end of lanelet, which mirroring lane should
have from the other lane it mirrors.
Returns:
Created Lanelet.
"""
left_vertices, right_vertices = np.array([]), np.array([])
for parametric_lane in self.parametric_lanes:
local_left_vertices, local_right_vertices = parametric_lane.calc_vertices(
precision=precision)
if local_left_vertices is None:
continue
try:
if np.isclose(left_vertices[-1], local_left_vertices[0]).all():
idx = 1
else:
idx = 0
left_vertices = np.vstack(
(left_vertices, local_left_vertices[idx:]))
right_vertices = np.vstack(
(right_vertices, local_right_vertices[idx:]))
except IndexError:
left_vertices = local_left_vertices
right_vertices = local_right_vertices
center_vertices = np.array([
(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)
])
# speed_limit
if (self.speed != {}):
lanelet = ConversionLanelet(self,
left_vertices,
center_vertices,
right_vertices,
self.id_,
speed_limit=int(self.speed._max))
else:
lanelet = ConversionLanelet(self, left_vertices, center_vertices,
right_vertices, self.id_)
# Adjacent lanes
self._set_adjacent_lanes(lanelet)
return lanelet
def _way_rel_to_lanelet(
self,
way_rel: WayRelation,
detect_adjacencies: bool,
left_driving_system: bool = False,
) -> ConversionLanelet:
"""Convert a WayRelation to a Lanelet, add additional adjacency information.
The ConversionLaneletNetwork saves the adjacency and predecessor/successor
information.
Args:
way_rel: Relation of OSM to convert to Lanelet.
osm: OSM object which contains info about nodes and ways.
detect_adjacencies: Compare vertices which might be adjacent. Set
to false if you consider it too computationally intensive.
left_driving_system: Set to true if map describes a left_driving_system.
Returns:
A lanelet with a right and left vertice.
"""
left_way = self.osm.find_way_by_id(way_rel.left_way)
right_way = self.osm.find_way_by_id(way_rel.right_way)
if len(left_way.nodes) != len(right_way.nodes):
print(
f"Error: Relation {way_rel.id_} has left and right ways which are not equally long! Please check your data! Discarding this lanelet relation."
)
return None
# set only if not set before
# one way can only have two lanelet relations which use it
if not self._left_way_ids.get(way_rel.left_way):
self._left_way_ids[way_rel.left_way] = way_rel.id_
if not self._right_way_ids.get(way_rel.right_way):
self._right_way_ids[way_rel.right_way] = way_rel.id_
left_vertices = self._convert_way_to_vertices(left_way)
first_left_node = left_way.nodes[0]
last_left_node = left_way.nodes[-1]
right_vertices = self._convert_way_to_vertices(right_way)
first_right_node = right_way.nodes[0]
last_right_node = right_way.nodes[-1]
start_dist = np.linalg.norm(
left_vertices[0] - right_vertices[0]
) + np.linalg.norm(left_vertices[-1] - right_vertices[-1])
end_dist = np.linalg.norm(
left_vertices[0] - right_vertices[-1]
) + np.linalg.norm(left_vertices[-1] - right_vertices[0])
if start_dist > end_dist:
if left_driving_system:
# reverse right vertices if right_way is reversed
right_vertices = right_vertices[::-1]
first_right_node, last_right_node = (last_right_node, first_right_node)
else:
# reverse left vertices if left_way is reversed
left_vertices = left_vertices[::-1]
first_left_node, last_left_node = (last_left_node, first_left_node)
self.first_left_pts[first_left_node].append(way_rel.id_)
self.last_left_pts[last_left_node].append(way_rel.id_)
self.first_right_pts[first_right_node].append(way_rel.id_)
self.last_right_pts[last_right_node].append(way_rel.id_)
center_vertices = np.array(
[(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)]
)
lanelet = ConversionLanelet(
left_vertices=left_vertices,
center_vertices=center_vertices,
right_vertices=right_vertices,
lanelet_id=way_rel.id_,
parametric_lane_group=None,
)
self._check_right_and_left_neighbors(way_rel, lanelet)
if detect_adjacencies:
self._find_adjacencies_of_coinciding_ways(
lanelet,
first_left_node,
first_right_node,
last_left_node,
last_right_node,
)
potential_successors = self._check_for_successors(
last_left_node=last_left_node, last_right_node=last_right_node
)
self.lanelet_network.add_successors_to_lanelet(lanelet, potential_successors)
potential_predecessors = self._check_for_predecessors(
first_left_node=first_left_node, first_right_node=first_right_node
)
self.lanelet_network.add_predecessors_to_lanelet(
lanelet, potential_predecessors
)
potential_adj_left, potential_adj_right = self._check_for_split_and_join_adjacencies(
first_left_node, first_right_node, last_left_node, last_right_node
)
if potential_adj_left:
self.lanelet_network.set_adjacent_left(lanelet, potential_adj_left[0], True)
if potential_adj_right:
self.lanelet_network.set_adjacent_right(
lanelet, potential_adj_right[0], True
)
return lanelet
def to_lanelet_with_mirroring(
self,
mirror_border: str,
distance: Tuple[float, float],
mirror_interval: Tuple[float, float],
adjacent_lanelet: ConversionLanelet,
precision: float = 0.5,
):
"""Convert a ParametricLaneGroup to a Lanelet with mirroring one of the borders.
Args:
precision: Number which indicates at which space interval (in curve parameter ds)
the coordinates of the boundaries should be calculated.
mirror_border: Which lane to mirror, if performing merging or splitting of lanes.
distance: Distance at start and end of lanelet, which mirroring lane should
have from the other lane it mirrors.
mirror_interval: Position at start and end of mirroring.
Returns:
Created Lanelet.
"""
linear_distance_poly = np.polyfit(mirror_interval, distance, 1)
distance_poly1d = np.poly1d(linear_distance_poly)
global_distance = distance_poly1d([0, self.length])
if self.parametric_lanes[0].reverse:
global_distance[:] = [-x for x in global_distance]
left_vertices, right_vertices = [], []
last_width_difference = 0
poses = self._calc_border_positions(precision)
distance_slope = (global_distance[1] -
global_distance[0]) / self.length
for pos in poses:
inner_pos = self.calc_border("inner", pos)[0]
outer_pos = self.calc_border("outer", pos)[0]
original_width = np.linalg.norm(inner_pos - outer_pos)
# if not mirroring lane or outside of range
if (pos < mirror_interval[0]
or pos > mirror_interval[1]) and not np.isclose(
pos, mirror_interval[1]):
left_vertices.append(inner_pos)
right_vertices.append(outer_pos)
last_width_difference = 0
else:
# calculate positions of adjacent lanelet because new width of lanelet
# cannot be more than width of adjacent lanelet and original width
adj_inner_pos = adjacent_lanelet.calc_border("inner", pos)[0]
adj_outer_pos = adjacent_lanelet.calc_border("outer", pos)[0]
adjacent_width = np.linalg.norm(adj_inner_pos - adj_outer_pos)
local_width_offset = distance_slope * pos + global_distance[0]
if mirror_border == "left":
new_outer_pos = self.calc_border("inner", pos,
local_width_offset)[0]
modified_width = np.linalg.norm(new_outer_pos - inner_pos)
# change width s.t. it does not mirror inner border but instead
# outer border
local_width_offset = (
math.copysign(1, local_width_offset) *
last_width_difference)
if modified_width < original_width:
right_vertices.append(
self.calc_border("outer", pos,
local_width_offset)[0])
elif modified_width > original_width + adjacent_width:
right_vertices.append(adj_outer_pos)
else:
right_vertices.append(new_outer_pos)
last_width_difference = abs(modified_width -
original_width)
left_vertices.append(inner_pos)
elif mirror_border == "right":
new_inner_pos = self.calc_border("outer", pos,
local_width_offset)[0]
modified_width = np.linalg.norm(new_inner_pos - outer_pos)
local_width_offset = (
math.copysign(1, local_width_offset) *
last_width_difference)
if modified_width < original_width:
left_vertices.append(
self.calc_border("inner", pos,
local_width_offset)[0])
elif modified_width > original_width + adjacent_width:
left_vertices.append(adj_inner_pos)
else:
left_vertices.append(new_inner_pos)
last_width_difference = abs(modified_width -
original_width)
right_vertices.append(outer_pos)
left_vertices, right_vertices = (
np.array(left_vertices),
np.array(right_vertices),
)
# right_vertices = np.array(right_vertices)
center_vertices = np.array([
(l + r) / 2 for (l, r) in zip(left_vertices, right_vertices)
])
lanelet = ConversionLanelet(self, left_vertices, center_vertices,
right_vertices, self.id_)
# Adjacent lanes
self._set_adjacent_lanes(lanelet)
return lanelet