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 _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