def to_scenario_via(
vias: Tuple[SSVia, ...], road_map: RoadMap
) -> Tuple[Via, ...]:
s_vias = []
for via in vias:
road = road_map.road_by_id(via.road_id)
lane = road.lane_at_index(via.lane_index)
lane_width = lane.width_at_offset(via.lane_offset)
hit_distance = (
via.hit_distance if via.hit_distance > 0 else lane_width / 2
)
via_position = lane.from_lane_coord(RefLinePoint(via.lane_offset))
s_vias.append(
Via(
lane_id=lane.lane_id,
lane_index=via.lane_index,
road_id=via.road_id,
position=tuple(via_position[:2]),
hit_distance=hit_distance,
required_speed=via.required_speed,
)
)
return tuple(s_vias)
def to_position_and_heading(road_id, lane_index, offset, road_map):
road = road_map.road_by_id(road_id)
lane = road.lane_at_index(lane_index)
offset = resolve_offset(offset, lane.length)
position = lane.from_lane_coord(RefLinePoint(s=offset))
lane_vector = lane.vector_at_offset(offset)
heading = vec_to_radians(lane_vector[:2])
return position, Heading(heading)
def split_lane_shape_at_offset(
lane_shape: Polygon, lane: RoadMap.Lane, offset: float
):
# XXX: generalize to n-dim
width_2 = lane.width_at_offset(offset)
point = np.array(lane.from_lane_coord(RefLinePoint(offset)))[:2]
lane_vec = lane.vector_at_offset(offset)[:2]
perp_vec_right = rotate_around_point(lane_vec, np.pi / 2, origin=(0, 0))
perp_vec_right = (
perp_vec_right / max(np.linalg.norm(perp_vec_right), 1e-3) * width_2
+ point
)
perp_vec_left = rotate_around_point(lane_vec, -np.pi / 2, origin=(0, 0))
perp_vec_left = (
perp_vec_left / max(np.linalg.norm(perp_vec_left), 1e-3) * width_2
+ point
)
split_line = LineString([perp_vec_left, perp_vec_right])
return split(lane_shape, split_line)
def to_geometry(self, road_map: RoadMap) -> Polygon:
"""Generates a map zone over a stretch of the given lanes."""
def resolve_offset(offset, geometry_length, lane_length):
if offset == "base":
return 0
# push off of end of lane
elif offset == "max":
return lane_length - geometry_length
elif offset == "random":
return random.uniform(0, lane_length - geometry_length)
else:
return float(offset)
def pick_remaining_shape_after_split(geometry_collection, expected_point):
lane_shape = geometry_collection
if not isinstance(lane_shape, GeometryCollection):
return lane_shape
# For simplicity, we only deal w/ the == 1 or 2 case
if len(lane_shape.geoms) not in {1, 2}:
return None
if len(lane_shape.geoms) == 1:
return lane_shape.geoms[0]
# We assume that there are only two split shapes to choose from
keep_index = 0
if lane_shape.geoms[1].minimum_rotated_rectangle.contains(expected_point):
# 0 is the discard piece, keep the other
keep_index = 1
lane_shape = lane_shape.geoms[keep_index]
return lane_shape
def split_lane_shape_at_offset(
lane_shape: Polygon, lane: RoadMap.Lane, offset: float
):
# XXX: generalize to n-dim
width_2 = lane.width_at_offset(offset)
point = np.array(lane.from_lane_coord(RefLinePoint(offset)))[:2]
lane_vec = lane.vector_at_offset(offset)[:2]
perp_vec_right = rotate_around_point(lane_vec, np.pi / 2, origin=(0, 0))
perp_vec_right = (
perp_vec_right / max(np.linalg.norm(perp_vec_right), 1e-3) * width_2
+ point
)
perp_vec_left = rotate_around_point(lane_vec, -np.pi / 2, origin=(0, 0))
perp_vec_left = (
perp_vec_left / max(np.linalg.norm(perp_vec_left), 1e-3) * width_2
+ point
)
split_line = LineString([perp_vec_left, perp_vec_right])
return split(lane_shape, split_line)
lane_shapes = []
road_id, lane_idx, offset = self.start
road = road_map.road_by_id(road_id)
buffer_from_ends = 1e-6
for lane_idx in range(lane_idx, lane_idx + self.n_lanes):
lane = road.lane_at_index(lane_idx)
lane_length = lane.length
geom_length = self.length
if geom_length > lane_length:
logging.debug(
f"Geometry is too long={geom_length} with offset={offset} for "
f"lane={lane.lane_id}, using length={lane_length} instead"
)
geom_length = lane_length
assert geom_length > 0 # Geom length is negative
lane_offset = resolve_offset(offset, geom_length, lane_length)
lane_offset += buffer_from_ends
width = lane.width_at_offset(lane_offset)
lane_shape = lane.shape(0.3, width)
geom_length = max(geom_length - buffer_from_ends, buffer_from_ends)
lane_length = max(lane_length - buffer_from_ends, buffer_from_ends)
min_cut = min(lane_offset, lane_length)
# Second cut takes into account shortening of geometry by `min_cut`.
max_cut = min(min_cut + geom_length, lane_length)
midpoint = Point(
*lane.from_lane_coord(RefLinePoint(s=lane_offset + geom_length * 0.5))
)
lane_shape = split_lane_shape_at_offset(lane_shape, lane, min_cut)
lane_shape = pick_remaining_shape_after_split(lane_shape, midpoint)
if lane_shape is None:
continue
lane_shape = split_lane_shape_at_offset(
lane_shape,
lane,
max_cut,
)
lane_shape = pick_remaining_shape_after_split(lane_shape, midpoint)
if lane_shape is None:
continue
lane_shapes.append(lane_shape)
geom = unary_union(MultiPolygon(lane_shapes))
return geom