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