def world_coord_from_offset(self, lane: Lane, offset) -> np.ndarray:
"""Convert offset into lane to world coordinates
Args:
offset: meters into the lane
lane: sumo network lane
Returns:
np.array([x, y]): coordinates in world space
"""
lane_shape = lane.getShape(False)
position_on_lane = sumolib.geomhelper.positionAtShapeOffset(lane_shape, offset)
return np.array(position_on_lane)
def lane_vector_at_offset(self, lane: Lane,
start_offset: float) -> np.ndarray:
"""Computes the lane direction vector at the given offset into the road."""
add_offset = 1
end_offset = start_offset + add_offset # a little further down the lane
lane_length = lane.getLength()
if end_offset > lane_length + add_offset:
raise ValueError(
f"Offset={end_offset} goes out further than the end of "
f"lane=({lane.getID()}, length={lane_length})")
p1 = self.world_coord_from_offset(lane, start_offset)
p2 = self.world_coord_from_offset(lane, end_offset)
return p2 - p1
def split_lane_shape_at_offset(
self, lane_shape: Polygon, lane: Lane, offset: float
):
width_2 = lane.getWidth()
point = self.world_coord_from_offset(lane, offset)
lane_vec = self.lane_vector_at_offset(lane, offset)
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 ops.split(lane_shape, split_line)
def _shape_lanepoints_along_lane(
self, lane: Lane, lanepoint_by_lane_memo: dict
) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]:
lane_queue = queue.Queue()
lane_queue.put((lane, None))
shape_lanepoints = []
initial_lanepoint = None
while not lane_queue.empty():
lane, previous_lp = lane_queue.get()
first_lanepoint = lanepoint_by_lane_memo.get(lane.getID())
if first_lanepoint:
if previous_lp:
previous_lp.nexts.append(first_lanepoint)
continue
lane_data = self._road_network.lane_data_for_lane(lane)
lane_shape = [np.array(p) for p in lane.getShape(False)]
assert len(lane_shape) >= 2, repr(lane_shape)
heading = vec_to_radians(lane_shape[1] - lane_shape[0])
heading = Heading(heading)
first_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=lane_shape[0],
heading=heading,
lane_width=lane.getWidth(),
speed_limit=lane_data.lane_speed,
lane_id=lane.getID(),
lane_index=lane.getIndex(),
),
nexts=[],
is_shape_lp=True,
)
if previous_lp is not None:
previous_lp.nexts.append(first_lanepoint)
if initial_lanepoint is None:
initial_lanepoint = first_lanepoint
lanepoint_by_lane_memo[lane.getID()] = first_lanepoint
shape_lanepoints.append(first_lanepoint)
curr_lanepoint = first_lanepoint
for p1, p2 in zip(lane_shape[1:], lane_shape[2:]):
heading_ = vec_to_radians(p2 - p1)
heading_ = Heading(heading_)
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=p1,
heading=heading_,
lane_width=lane.getWidth(),
speed_limit=lane_data.lane_speed,
lane_id=lane.getID(),
lane_index=lane.getIndex(),
),
nexts=[],
is_shape_lp=True,
)
shape_lanepoints.append(linked_lanepoint)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
# Add a lanepoint for the last point of the current lane
last_linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=lane_shape[-1],
heading=curr_lanepoint.lp.heading,
lane_width=curr_lanepoint.lp.lane_width,
speed_limit=curr_lanepoint.lp.speed_limit,
lane_id=curr_lanepoint.lp.lane_id,
lane_index=curr_lanepoint.lp.lane_index,
),
nexts=[],
is_shape_lp=True,
)
shape_lanepoints.append(last_linked_lanepoint)
curr_lanepoint.nexts.append(last_linked_lanepoint)
curr_lanepoint = last_linked_lanepoint
for out_connection in lane.getOutgoing():
out_lane = out_connection.getToLane()
# Use internal lanes of junctions (if we're at a junction)
via_lane_id = out_connection.getViaLaneID()
if via_lane_id:
out_lane = self._road_network.lane_by_id(via_lane_id)
lane_queue.put((out_lane, curr_lanepoint))
return initial_lanepoint, shape_lanepoints