Multi-Purpose-MPC/reference_path.py

import numpy as np
import math
from map import Map
from bresenham import bresenham
import matplotlib.pyplot as plt
import matplotlib.patches as plt_patches
from scipy.signal import savgol_filter

# Colors
DRIVABLE_AREA = '#BDC3C7'
WAYPOINTS = '#D0D3D4'
OBSTACLE = '#2E4053'


############
# Waypoint #
############

class Waypoint:
    def __init__(self, x, y, psi, kappa):
        """
        Waypoint object containing x, y location in global coordinate system,
        orientation of waypoint psi and local curvature kappa
        :param x: x position in global coordinate system | [m]
        :param y: y position in global coordinate system | [m]
        :param psi: orientation of waypoint | [rad]
        :param kappa: local curvature | [1 / m]
        """
        self.x = x
        self.y = y
        self.psi = psi
        self.kappa = kappa

        # information about drivable area at waypoint
        # upper and lower bound of drivable area orthogonal to
        # waypoint orientation
        self.lb = None
        self.ub = None
        self.border_cells = None

    def __sub__(self, other):
        """
        Overload subtract operator. Difference of two waypoints is equal to
        their euclidean distance.
        :param other: subtrahend
        :return: euclidean distance between two waypoints
        """
        return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5


############
# Obstacle #
############

class Obstacle:
    def __init__(self, cx, cy, radius):
        """
        Constructor for a circular obstacle to be place on a path.
        :param cx: x coordinate of center of obstacle in world coordinates
        :param cy: y coordinate of center of obstacle in world coordinates
        :param radius: radius of circular obstacle in m
        """

        self.cx = cx
        self.cy = cy
        self.radius = radius

    def show(self):
        """
        Display obstacle.
        """

        # Draw circle
        circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
                                            self.radius, color=OBSTACLE)
        ax = plt.gca()
        ax.add_patch(circle)


##################
# Reference Path #
##################


class ReferencePath:
    def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance,
                 max_width):
        """
        Reference Path object. Create a reference trajectory from specified
        corner points with given resolution. Smoothing around corners can be
        applied. Waypoints represent center-line of the path with specified
        maximum width to both sides.
        :param map: map object on which path will be placed
        :param wp_x: x coordinates of corner points in global coordinates
        :param wp_y: y coordinates of corner points in global coordinates
        :param resolution: resolution of the path in m/wp
        :param smoothing_distance: number of waypoints used for smoothing the
        path by averaging neighborhood of waypoints
        :param max_width: maximum width of path to both sides in m
        """

        # Precision
        self.eps = 1e-12

        # Map
        self.map = map

        # Resolution of the path
        self.resolution = resolution

        # Look ahead distance for path averaging
        self.smoothing_distance = smoothing_distance

        # List of waypoint objects
        self.waypoints = self._construct_path(wp_x, wp_y)

        # Compute path width (attribute of each waypoint)
        self._compute_width(max_width=max_width)

        # Obstacles on path
        self.obstacles = list()

    def _construct_path(self, wp_x, wp_y):
        """
        Construct path from given waypoints.
        :param wp_x: x coordinates of waypoints in global coordinates
        :param wp_y: y coordinates of waypoints in global coordinates
        :return: list of waypoint objects
        """

        # Number of waypoints
        n_wp = [int(np.sqrt((wp_x[i + 1] - wp_x[i]) ** 2 +
                            (wp_y[i + 1] - wp_y[i]) ** 2) /
            self.resolution) for i in range(len(wp_x) - 1)]

        # Construct waypoints with specified resolution
        gp_x, gp_y = wp_x[-1], wp_y[-1]
        wp_x = [np.linspace(wp_x[i], wp_x[i+1], n_wp[i], endpoint=False).
                    tolist() for i in range(len(wp_x)-1)]
        wp_x = [wp for segment in wp_x for wp in segment] + [gp_x]
        wp_y = [np.linspace(wp_y[i], wp_y[i + 1], n_wp[i], endpoint=False).
                    tolist() for i in range(len(wp_y) - 1)]
        wp_y = [wp for segment in wp_y for wp in segment] + [gp_y]

        # Smooth path
        wp_xs = []
        wp_ys = []
        for wp_id in range(self.smoothing_distance, len(wp_x) -
                                                    self.smoothing_distance):
            wp_xs.append(np.mean(wp_x[wp_id - self.smoothing_distance:wp_id
                                            + self.smoothing_distance + 1]))
            wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id
                                            + self.smoothing_distance + 1]))

        # Construct list of waypoint objects
        waypoints = list(zip(wp_xs, wp_ys))
        waypoints = self._construct_waypoints(waypoints)

        return waypoints

    def _construct_waypoints(self, waypoint_coordinates):
        """
        Reformulate conventional waypoints (x, y) coordinates into waypoint
        objects containing (x, y, psi, kappa, ub, lb)
        :param waypoint_coordinates: list of (x, y) coordinates of waypoints in
        global coordinates
        :return: list of waypoint objects for entire reference path
        """

        # List containing waypoint objects
        waypoints = []

        # Iterate over all waypoints
        for wp_id in range(len(waypoint_coordinates) - 1):

            # Get start and goal waypoints
            current_wp = np.array(waypoint_coordinates[wp_id])
            next_wp = np.array(waypoint_coordinates[wp_id + 1])

            # Difference vector
            dif_ahead = next_wp - current_wp

            # Angle ahead
            psi = np.arctan2(dif_ahead[1], dif_ahead[0])

            # Distance to next waypoint
            dist_ahead = np.linalg.norm(dif_ahead, 2)

            # Get x and y coordinates of current waypoint
            x, y = current_wp[0], current_wp[1]

            # Compute local curvature at waypoint
            # first waypoint
            if wp_id == 0:
                kappa = 0
            else:
                prev_wp = np.array(waypoint_coordinates[wp_id - 1])
                dif_behind = current_wp - prev_wp
                angle_behind = np.arctan2(dif_behind[1], dif_behind[0])
                angle_dif = np.mod(psi - angle_behind + math.pi, 2 * math.pi) \
                            - math.pi
                kappa = angle_dif / (dist_ahead + self.eps)

            waypoints.append(Waypoint(x, y, psi, kappa))

        return waypoints

    def _compute_width(self, max_width):
        """
        Compute the width of the path by checking the maximum free space to
        the left and right of the center-line.
        :param max_width: maximum width of the path.
        """

        # Iterate over all waypoints
        for wp_id, wp in enumerate(self.waypoints):
            # List containing information for current waypoint
            width_info = []
            # Check width left and right of the center-line
            for i, dir in enumerate(['left', 'right']):
                # Get angle orthogonal to path in current direction
                if dir == 'left':
                    angle = np.mod(wp.psi + math.pi / 2 + math.pi,
                                 2 * math.pi) - math.pi
                else:
                    angle = np.mod(wp.psi - math.pi / 2 + math.pi,
                                   2 * math.pi) - math.pi
                # Get closest cell to orthogonal vector
                t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y
                                        + max_width * np.sin(angle))
                # Compute distance to orthogonal cell on path border
                b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width)
                # Add information to list for current waypoint
                width_info.append(b_value)
                width_info.append(b_cell)

            # Set waypoint attributes with width to the left and right
            wp.ub = width_info[0]
            wp.lb = -1 * width_info[2]  # minus can be assumed as waypoints
            # represent center-line of the path
            # Set border cells of waypoint
            wp.border_cells = (width_info[1], width_info[3])

    def _get_min_width(self, wp, t_x, t_y, max_width):
        """
        Compute the minimum distance between the current waypoint and the
        orthogonal cell on the border of the path
        :param wp: current waypoint
        :param t_x: x coordinate of border cell in map coordinates
        :param t_y: y coordinate of border cell in map coordinates
        :param max_width: maximum path width in m
        :return: min_width to border and corresponding cell
        """

        # Get neighboring cells of orthogonal cell (account for
        # discretization inaccuracy)
        tn_x, tn_y = [], []
        for i in range(-1, 2, 1):
            for j in range(-1, 2, 1):
                tn_x.append(t_x+i)
                tn_y.append(t_y+j)

        # Get pixel coordinates of waypoint
        wp_x, wp_y = self.map.w2m(wp.x, wp.y)

        # Get Bresenham paths to all possible cells
        paths = []
        for t_x, t_y in zip(tn_x, tn_y):
            path = list(bresenham(wp_x, wp_y, t_x, t_y))
            paths.append(path)

        # Compute minimum distance to border cell
        min_width = max_width
        # map inspected cell to world coordinates
        min_cell = self.map.m2w(t_x, t_y)
        for path in paths:
            for cell in path:
                t_x, t_y = cell[0], cell[1]
                # If path goes through occupied cell
                if self.map.data[t_y, t_x] == 0:
                    # Get world coordinates
                    c_x, c_y = self.map.m2w(t_x, t_y)
                    cell_dist = np.sqrt((wp.x - c_x) ** 2 + (wp.y - c_y) ** 2)
                    if cell_dist < min_width:
                        min_width = cell_dist
                        min_cell = (c_x, c_y)

        # decrease min_width by radius of circle around cell
        min_width -= 1 / np.sqrt(2) * self.map.resolution

        return min_width, min_cell

    def update_bounds(self, car, wp_id):
        """
        Compute upper and lower bounds of the drivable area orthogonal to
        the given waypoint.
        :param car: car model driving on the path
        :param wp_id: ID of reference waypoint
        """

        # Get car-dependent safety margin
        safety_margin = car.safety_margin

        # Get reference waypoint
        wp = self.waypoints[wp_id]

        # Get waypoint's border cells in map coordinates
        ub_p = self.map.w2m(wp.border_cells[0][0], wp.border_cells[0][1])
        lb_p = self.map.w2m(wp.border_cells[1][0], wp.border_cells[1][1])

        # Compute path from left border cell to right border cell
        path = list(bresenham(ub_p[0], ub_p[1], lb_p[0], lb_p[1]))

        # Initialize upper and lower bound of drivable area to
        # upper bound of path
        ub_o, lb_o = ub_p, ub_p

        # Initialize upper and lower bound of best segment to upper bound of
        # path
        ub_ls, lb_ls = ub_p, ub_p

        # Iterate over path from left border to right border
        for x, y in path:
            # If cell is free, update lower bound
            if self.map.data[y, x] == 1:
                lb_o = (x, y)
            # If cell is occupied, end segment. Update best segment if current
            # segment is larger than previous best segment. Then, reset upper
            # and lower bound to current cell
            elif self.map.data[y, x] == 0:
                if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \
                    np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2):
                    ub_ls = ub_o
                    lb_ls = lb_o
                # Start new segment
                ub_o = (x, y)
                lb_o = (x, y)

        # If no segment was set (no obstacle between left and right border),
        # return original bounds of path
        if ub_ls == ub_p and lb_ls == ub_p:
            return wp.lb, wp.ub

        # Transform upper and lower bound cells to world coordinates
        ub_ls = self.map.m2w(ub_ls[0], ub_ls[1])
        lb_ls = self.map.m2w(lb_ls[0], lb_ls[1])
        # Check sign of upper and lower bound
        angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x)
                          - wp.psi + math.pi, 2*math.pi) - math.pi
        angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x)
                          - wp.psi + math.pi, 2*math.pi) - math.pi
        sign_ub = np.sign(angle_ub)
        sign_lb = np.sign(angle_lb)
        # Compute upper and lower bound of largest drivable area
        ub = sign_ub * np.sqrt((ub_ls[0]-wp.x)**2+(ub_ls[1]-wp.y)**2)
        lb = sign_lb * np.sqrt((lb_ls[0]-wp.x)**2+(lb_ls[1]-wp.y)**2)

        # Add safety margin to bounds
        ub = ub - (safety_margin[1] + 2 * self.map.resolution)
        lb = lb + (safety_margin[1] + 2 * self.map.resolution)

        # Update member variables of waypoint
        wp.ub = ub
        wp.lb = lb
        wp.border_cells = (ub_ls, lb_ls)

        return lb, ub

    def add_obstacles(self, obstacles):
        """
        Add obstacles to the path.
        :param obstacles: list of obstacle objects
        """

        # Extend list of obstacles
        self.obstacles.extend(obstacles)

        # Iterate over list of obstacles
        for obstacle in obstacles:
            radius_px = int(np.ceil(obstacle.radius / self.map.resolution))
            cx_px, cy_px = self.map.w2m(obstacle.cx, obstacle.cy)
            y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px]
            index = x ** 2 + y ** 2 <= radius_px ** 2
            self.map.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px:
                                                cx_px+radius_px][index] = 0

    def show(self, display_drivable_area=True):
        """
        Display path object on current figure.
        :param display_drivable_area: If True, display arrows indicating width
        of drivable area
        """

        # Clear figure
        plt.clf()

        # Disabled ticks
        plt.xticks([])
        plt.yticks([])

        # Plot map in gray-scale and set extent to match world coordinates
        canvas = np.ones(self.map.data.shape)
        plt.imshow(canvas, cmap='gray',
                   extent=[self.map.origin[0], self.map.origin[0] +
                           self.map.width * self.map.resolution,
                           self.map.origin[1], self.map.origin[1] +
                           self.map.height * self.map.resolution], vmin=0.0,
                   vmax=1.0)

        # Get x and y coordinates for all waypoints
        wp_x = np.array([wp.x for wp in self.waypoints])
        wp_y = np.array([wp.y for wp in self.waypoints])

        # Get x and y locations of border cells for upper and lower bound
        wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints])
        wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints])
        wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints])
        wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints])

        # Plot waypoints
        plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=3)

        # Plot arrows indicating drivable area
        if display_drivable_area:
            plt.quiver(wp_x, wp_y, wp_ub_x - wp_x, wp_ub_y - wp_y, scale=1,
                   units='xy', width=0.2*self.resolution, color=DRIVABLE_AREA,
                   headwidth=1, headlength=0)
            plt.quiver(wp_x, wp_y, wp_lb_x - wp_x, wp_lb_y - wp_y, scale=1,
                   units='xy', width=0.2*self.resolution, color=DRIVABLE_AREA,
                   headwidth=1, headlength=0)

        # Plot border of path
        bl_x = np.array([wp.border_cells[0][0] for wp in
                         self.waypoints] +
                        [self.waypoints[0].border_cells[0][0]])
        bl_y = np.array([wp.border_cells[0][1] for wp in
                         self.waypoints] +
                        [self.waypoints[0].border_cells[0][1]])
        br_x = np.array([wp.border_cells[1][0] for wp in
                         self.waypoints] +
                        [self.waypoints[0].border_cells[1][0]])
        br_y = np.array([wp.border_cells[1][1] for wp in
                         self.waypoints] +
                        [self.waypoints[0].border_cells[1][1]])
        # Smooth border
        # bl_x = savgol_filter(bl_x, 15, 9)
        # bl_y = savgol_filter(bl_y, 15, 9)
        # br_x = savgol_filter(br_x, 15, 9)
        # br_y = savgol_filter(br_y, 15, 9)

        # If circular path, connect start and end point
        if np.abs(self.waypoints[-1] - self.waypoints[0]) <= 2*self.resolution:
            plt.plot(bl_x, bl_y, color=OBSTACLE)
            plt.plot(br_x, br_y, color=OBSTACLE)
        # If not circular, close path at start and end
        else:
            plt.plot(bl_x[:-1], bl_y[:-1], color=OBSTACLE)
            plt.plot(br_x[:-1], br_y[:-1], color=OBSTACLE)
            plt.plot((bl_x[-2],br_x[-2]), (bl_y[-2], br_y[-2]), color=OBSTACLE)
            plt.plot((bl_x[0],br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE)

        # Plot obstacles
        for obstacle in self.obstacles:
            obstacle.show()


if __name__ == '__main__':

    # Select Path | 'Race' or 'Q'
    path = 'Race'

    # Create Map
    if path == 'Race':
        map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
        # Specify waypoints
        wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
                1.25, -0.75, -0.75, -0.25]
        wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
                -1.5, -1.5]
        # Specify path resolution
        path_resolution = 0.05  # m / wp
        reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
                                       smoothing_distance=5, max_width=0.22)
    elif path == 'Q':
        map = Map(file_path='map_floor2.png')
        wp_x = [-9.169, 11.9, 7.3, -6.95]
        wp_y = [-15.678, 10.9, 14.5, -3.31]
        # Specify path resolution
        path_resolution = 0.20  # m / wp
        reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
                                       smoothing_distance=5, max_width=1.5)
    else:
        reference_path = None
        print('Invalid path!')
        exit(1)

    obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
    obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.05)
    obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
    obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.05)
    obs5 = Obstacle(cx=0.3, cy=-1.0, radius=0.05)
    obs6 = Obstacle(cx=0.75, cy=-1.5, radius=0.05)
    obs7 = Obstacle(cx=0.7, cy=-0.9, radius=0.05)
    obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.05)
    reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
                                  obs8])

    reference_path.show()
    plt.show()