def lane_distance_to(self, vehicle: "Vehicle", lane: AbstractLane = None) -> float: """ Compute the signed distance to another vehicle along a lane. :param vehicle: the other vehicle :param lane: a lane :return: the distance to the other vehicle [m] """ if not vehicle: return np.nan if not lane: lane = self.lane return lane.local_coordinates(vehicle.position)[0] - lane.local_coordinates(self.position)[0]
def draw_ground(cls, lane: AbstractLane, surface: WorldSurface, color: Tuple[float], width: float, draw_surface: pygame.Surface = None) -> None: draw_surface = draw_surface or surface stripes_count = int(2 * (surface.get_height() + surface.get_width()) / (cls.STRIPE_SPACING * surface.scaling)) s_origin, _ = lane.local_coordinates(surface.origin) s0 = (int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2) * cls.STRIPE_SPACING dots = [] for side in range(2): longis = np.clip( s0 + np.arange(stripes_count) * cls.STRIPE_SPACING, 0, lane.length) lats = [2 * (side - 0.5) * width for _ in longis] new_dots = [ surface.vec2pix(lane.position(longi, lat)) for longi, lat in zip(longis, lats) ] new_dots = reversed(new_dots) if side else new_dots dots.extend(new_dots) pygame.draw.polygon(draw_surface, color, dots, 0)
def interval_absolute_to_local(position_i: Interval, lane: AbstractLane) -> Tuple[np.ndarray, np.ndarray]: """ Converts an interval in absolute x,y coordinates to an interval in local (longiturinal, lateral) coordinates :param position_i: the position interval [x_min, x_max] :param lane: the lane giving the local frame :return: the corresponding local interval """ position_corners = np.array([[position_i[0, 0], position_i[0, 1]], [position_i[0, 0], position_i[1, 1]], [position_i[1, 0], position_i[0, 1]], [position_i[1, 0], position_i[1, 1]]]) corners_local = np.array([lane.local_coordinates(c) for c in position_corners]) longitudinal_i = np.array([min(corners_local[:, 0]), max(corners_local[:, 0])]) lateral_i = np.array([min(corners_local[:, 1]), max(corners_local[:, 1])]) return longitudinal_i, lateral_i
def display(cls, lane: AbstractLane, surface: WorldSurface) -> None: """ Display a lane on a surface. :param lane: the lane to be displayed :param surface: the pygame surface """ stripes_count = int(2 * (surface.get_height() + surface.get_width()) / (cls.STRIPE_SPACING * surface.scaling)) s_origin, _ = lane.local_coordinates(surface.origin) s0 = (int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2) * cls.STRIPE_SPACING for side in range(2): if lane.line_types[side] == LineType.STRIPED: cls.striped_line(lane, surface, stripes_count, s0, side) elif lane.line_types[side] == LineType.CONTINUOUS: cls.continuous_curve(lane, surface, stripes_count, s0, side) elif lane.line_types[side] == LineType.CONTINUOUS_LINE: cls.continuous_line(lane, surface, stripes_count, s0, side)