def set_adjacent_right(self,
lanelet: ConversionLanelet,
adj_right_id: str,
same_direction: bool = True):
"""Set the adj_right of a lanelet to a new value.
Update also the lanelet which is the new adjacent right to
have new adjacent left.
Args:
lanelet: Lanelet which adjacent right should be updated.
adj_right_id: New value for update.
same_direction: New adjacent lanelet has same direction as lanelet.
Returns:
True if operation successful, else false.
"""
new_adj = self.find_lanelet_by_id(adj_right_id)
if not new_adj:
return False
lanelet.adj_right = adj_right_id
lanelet.adj_right_same_direction = same_direction
if same_direction:
new_adj.adj_left = lanelet.lanelet_id
new_adj.adj_left_same_direction = True
else:
new_adj.adj_right = lanelet.lanelet_id
new_adj.adj_right_same_direction = False
return True
def _set_adjacent_lanes(self, lanelet: ConversionLanelet):
"""While converting a ParametricLaneGroup to a Lanelet, set
the proper attributes relating to adjacent lanes.
Args:
lanelet: The lanelet which is created from the ParametricLaneGroup.
"""
if self.inner_neighbour is not None:
lanelet.adj_left = self.inner_neighbour
lanelet.adj_left_same_direction = self.inner_neighbour_same_direction
if self.outer_neighbour is not None:
lanelet.adj_right = self.outer_neighbour
lanelet.adj_right_same_direction = True
def adj_right_consistent_nb(self, lanelet: ConversionLanelet) -> bool:
"""Checks if right neighbor of successor is the successor
of the right adjacent neighbor of the lanelet.
Args:
lanelet: Lanelet to check specified relation for.
Returns:
True if this neighbor requirement is fulfilled.
"""
successor = self.find_lanelet_by_id(lanelet.successor[0])
adj_right = self.find_lanelet_by_id(lanelet.adj_right)
if adj_right:
if lanelet.adj_right_same_direction:
if not self.has_unique_pred_succ_relation(1, adj_right):
return False
if adj_right.successor[0] != successor.adj_right:
return False
else:
if not lanelet.has_unique_pred_succ_relation(-1, adj_right):
return False
if adj_right.predecessor[0] != successor.adj_right:
return False
else:
return successor.adj_right is None
return True
def _add_join_split_pair(self, lanelet: ConversionLanelet,
adjacent_lanelet: ConversionLanelet) -> bool:
"""Add a pair of lanelet and adjacent lanelet to self._js_pairs.
Decide if it is advisable to add another pair to increase join/split area.
Args:
lanelet: Lanelet to be added.
adjacent_lanelet: Lanelet adjacent to lanelet to be added.
Returns:
Indicator whether this was the last pair to be added. False means
it is advisable to add another lanelet pair.
"""
if self.split_and_join:
# one for split at start of lanelet
change_pos, change_width = lanelet.optimal_join_split_values(
is_split=True,
split_and_join=self.split_and_join,
reference_width=adjacent_lanelet.calc_width_at_start(),
)
self._js_pairs.append(
_JoinSplitPair(lanelet, adjacent_lanelet, [0, change_pos]))
self.change_width = [change_width]
# one for join at the end of the lanelet
change_pos, change_width = lanelet.optimal_join_split_values(
is_split=False,
split_and_join=self.split_and_join,
reference_width=adjacent_lanelet.calc_width_at_end(),
)
self._js_pairs.append(
_JoinSplitPair(lanelet, adjacent_lanelet,
[change_pos, lanelet.length]))
self.change_width.append(change_width)
return True
adjacent_width = (adjacent_lanelet.calc_width_at_start() if self.split
else adjacent_lanelet.calc_width_at_end())
change_pos, change_width = lanelet.optimal_join_split_values(
is_split=self.split,
split_and_join=self.split_and_join,
reference_width=adjacent_width,
)
if self.change_width is not None and change_width < self.change_width:
# algorithm to add lanelet should terminate
return True
self.change_width = change_width
if self.split:
self._js_pairs.append(
_JoinSplitPair(lanelet, adjacent_lanelet, [0, change_pos]))
if np.isclose(lanelet.length, change_pos):
return False
else:
self._js_pairs.append(
_JoinSplitPair(lanelet, adjacent_lanelet,
[change_pos, lanelet.length]))
if np.isclose(0, change_pos):
return False
return True
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)
])
lanelet = ConversionLanelet(self, left_vertices, center_vertices,
right_vertices, self.id_)
# Adjacent lanes
self._set_adjacent_lanes(lanelet)
self._set_bound_road_mark(lanelet)
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
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 _set_bound_road_mark(self, lanelet: ConversionLanelet):
"""Sets roadMark of each bound in lanelet. """
if self.parametric_lanes[0].road_mark:
lanelet.line_marking_right_vertices = self.parametric_lanes[
0].road_mark