示例#1
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 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]
示例#3
0
    def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface,
                     starts: List[float], ends: List[float], lats: List[float]) -> None:
        """
        Draw a set of stripes along a lane.

        :param lane: the lane
        :param surface: the surface to draw on
        :param starts: a list of starting longitudinal positions for each stripe [m]
        :param ends: a list of ending longitudinal positions for each stripe [m]
        :param lats: a list of lateral positions for each stripe [m]
        """
        starts = np.clip(starts, 0, lane.length)
        ends = np.clip(ends, 0, lane.length)
        for k, _ in enumerate(starts):
            if abs(starts[k] - ends[k]) > 0.5 * cls.STRIPE_LENGTH:
                pygame.draw.line(surface, surface.WHITE,
                                 (surface.vec2pix(lane.position(starts[k], lats[k]))),
                                 (surface.vec2pix(lane.position(ends[k], lats[k]))),
                                 max(surface.pix(cls.STRIPE_WIDTH), 1))
示例#4
0
    def continuous_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, longitudinal: float,
                        side: int) -> None:
        """
        Draw a continuous line on one side of a lane, on a surface.

        :param lane: the lane
        :param surface: the pygame surface
        :param stripes_count: the number of stripes that would be drawn if the line was striped
        :param longitudinal: the longitudinal position of the start of the line [m]
        :param side: which side of the road to draw [0:left, 1:right]
        """
        starts = [longitudinal + 0 * cls.STRIPE_SPACING]
        ends = [longitudinal + stripes_count * cls.STRIPE_SPACING + cls.STRIPE_LENGTH]
        lats = [(side - 0.5) * lane.width_at(s) for s in starts]
        cls.draw_stripes(lane, surface, starts, ends, lats)
示例#5
0
    def continuous_curve(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int,
                         longitudinal: float, side: int) -> None:
        """
        Draw a striped line on one side of a lane, on a surface.

        :param lane: the lane
        :param surface: the pygame surface
        :param stripes_count: the number of stripes to draw
        :param longitudinal: the longitudinal position of the first stripe [m]
        :param side: which side of the road to draw [0:left, 1:right]
        """
        starts = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING
        ends = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING + cls.STRIPE_SPACING
        lats = [(side - 0.5) * lane.width_at(s) for s in starts]
        cls.draw_stripes(lane, surface, starts, ends, lats)
示例#6
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
示例#7
0
def interval_local_to_absolute(longitudinal_i: Interval, lateral_i: Interval, lane: AbstractLane) -> Interval:
    """
    Converts an interval in local (longiturinal, lateral) coordinates to an interval in absolute x,y coordinates

    :param longitudinal_i: the longitudinal interval [L_min, L_max]
    :param lateral_i: the lateral interval [l_min, l_max]
    :param lane: the lane giving the local frame
    :return: the corresponding absolute interval
    """
    corners_local = [[longitudinal_i[0], lateral_i[0]],
                     [longitudinal_i[0], lateral_i[1]],
                     [longitudinal_i[1], lateral_i[0]],
                     [longitudinal_i[1], lateral_i[1]]]
    corners_absolute = np.array([lane.position(*c) for c in corners_local])
    position_i = np.array([np.amin(corners_absolute, axis=0), np.amax(corners_absolute, axis=0)])
    return position_i
示例#8
0
    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)