Multi-Purpose-MPC/reference_path.py

import numpy as np
import math
from map import Map
from skimage.draw import line
import matplotlib.pyplot as plt
import matplotlib.patches as plt_patches
from scipy import sparse
import osqp

# 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

        # Reference velocity at this waypoint according to speed profile
        self.v_ref = None

        # 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, circular):
        """
        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
        :param circular: True if path circular
        """

        # 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

        # Circular flag
        self.circular = circular

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

        # Number of waypoints
        self.n_waypoints = len(self.waypoints)

        # Length of path
        self.length, self.segment_lengths = self._compute_length()

        # 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_length(self):
        """
        Compute length of center-line path as sum of euclidean distance between
        waypoints.
        :return: length of center-line path in m
        """
        segment_lengths = [0.0] + [self.waypoints[wp_id+1] - self.waypoints
                    [wp_id] for wp_id in range(len(self.waypoints)-1)]
        s = sum(segment_lengths)
        return s, segment_lengths

    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):
            x_list, y_list = line(wp_x, wp_y, t_x, t_y)
            paths.append(zip(x_list, y_list))

        # 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)

        return min_width, min_cell

    def compute_speed_profile(self, Constraints):
        """
        Compute a speed profile for the path. Assign a reference velocity
        to each waypoint based on its curvature.
        """

        # Set optimization horizon
        N = self.n_waypoints - 1

        # Constraints
        a_min = np.ones(N-1) * Constraints['a_min']
        a_max = np.ones(N-1) * Constraints['a_max']
        v_min = np.ones(N) * Constraints['v_min']
        v_max = np.ones(N) * Constraints['v_max']

        # Maximum lateral acceleration
        ay_max = Constraints['ay_max']

        # Inequality Matrix
        D1 = np.zeros((N-1, N))

        # Iterate over horizon
        for i in range(N):

            look_ahead = 30

            # Get information about current waypoint
            current_waypoint = self.get_waypoint(i)
            next_waypoint = self.get_waypoint(i+1)
            # distance between waypoints
            li = next_waypoint - current_waypoint
            # curvature of waypoint
            ki = current_waypoint.kappa
            if np.abs(ki) <= 0.1:
                kis = [wp.kappa for wp in self.waypoints[i:i+look_ahead]]
                ki = np.mean(kis)

            # Fill operator matrix
            # dynamics of acceleration
            if i < N-1:
                D1[i, i:i+2] = np.array([-1/(2*li), 1/(2*li)])

            # Compute dynamic constraint on velocity
            v_max_dyn = np.sqrt(ay_max / (np.abs(ki) + self.eps))
            if v_max_dyn < v_max[i]:
                v_max[i] = v_max_dyn

        # Construct inequality matrix
        D1 = sparse.csc_matrix(D1)
        D2 = sparse.eye(N)
        D = sparse.vstack([D1, D2], format='csc')

        # Get upper and lower bound vectors for inequality constraints
        l = np.hstack([a_min, v_min])
        u = np.hstack([a_max, v_max])

        # Set cost matrices
        P = sparse.eye(N, format='csc')
        q = -1 * v_max

        # Solve optimization problem
        problem = osqp.OSQP()
        problem.setup(P=P, q=q, A=D, l=l, u=u, verbose=False)
        speed_profile = problem.solve().x

        # Assign reference velocity to every waypoint
        for i, wp in enumerate(self.waypoints[:-1]):
            wp.v_ref = speed_profile[i]
        self.waypoints[-1].v_ref = self.waypoints[-2].v_ref

    def get_waypoint(self, wp_id):
        """
        Get waypoint corresponding to wp_id. Circular indexing supported.
        :param wp_id: unique waypoint ID
        :return: waypoint object
        """

        # Allow circular indexing if circular path
        if wp_id >= self.n_waypoints and self.circular:
            wp_id = np.mod(wp_id, self.n_waypoints)
        # Terminate execution if end of path reached
        elif wp_id >= self.n_waypoints and not self.circular:
            print('Reached end of path!')
            exit(1)

        return self.waypoints[wp_id]

    def update_bounds(self, wp_id, safety_margin):
        """
        Compute upper and lower bounds of the drivable area orthogonal to
        the given waypoint.
        :param safety_margin: safety margin of the car orthogonal to path in m
        :param wp_id: ID of reference waypoint
        """

        # Get reference waypoint
        wp = self.get_waypoint(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
        x_list, y_list = line(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 zip(x_list[1:], y_list[1:]):
            # 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
            if self.map.data[y, x] == 0 or (x, y) == lb_p:
                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)

        # 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 (attribute of car) to bounds
        ub = ub - safety_margin
        lb = lb + safety_margin

        # Check feasibility of the path
        if ub < lb:
            # Upper and lower bound of 0 indicate an infeasible path
            # given the specified safety margin
            ub, lb = 0.0, 0.0

        # Compute absolute angle of bound cell
        angle_ub = np.mod(math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi
        angle_lb = np.mod(-math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi

        # Compute cell on bound for computed distance ub and lb
        ub_ls = wp.x + ub * np.cos(angle_ub), wp.y + ub * np.sin(angle_ub)
        lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin(angle_lb)
        border_cells = (ub_ls, lb_ls)

        return lb, ub, border_cells

    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:
            # Compute radius of circular object in pixels
            radius_px = int(np.ceil(obstacle.radius / self.map.resolution))
            # Get center coordinates of obstacle in map coordinates
            cx_px, cy_px = self.map.w2m(obstacle.cx, obstacle.cy)

            # Add circular object to map
            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)
        canvas = np.flipud(self.map.data)
        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=10)

        # 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]])

        # If circular path, connect start and end point
        if self.circular:
            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()

    def _compute_free_segments(self, wp, min_width):
        """
        Compute free path segments.
        :param wp: waypoint object
        :param min_width: minimum width of valid segment
        :return: segment candidates as list of tuples (ub_cell, lb_cell)
        """

        # Candidate segments
        free_segments = []

        # 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
        x_list, y_list = line(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

        # Assume occupied path
        free_cells = False

        # Iterate over path from left border to right border
        for x, y in zip(x_list[1:], y_list[1:]):
            # If cell is free, update lower bound
            if self.map.data[y, x] == 1:
                # Free cell detected
                free_cells = True
                lb_o = (x, y)
            # If cell is occupied or end of path, end segment. Add segment
            # to list of candidates. Then, reset upper and lower bound to
            # current cell.
            if (self.map.data[y, x] == 0 or (x, y) == lb_p) and free_cells:
                # Set lower bound to border cell of segment
                lb_o = (x, y)
                # Transform upper and lower bound cells to world coordinates
                ub_o = self.map.m2w(ub_o[0], ub_o[1])
                lb_o = self.map.m2w(lb_o[0], lb_o[1])
                # If segment larger than threshold, add to candidates
                if np.sqrt((ub_o[0]-lb_o[0])**2 + (ub_o[1]-lb_o[1])**2) > \
                    min_width:
                    free_segments.append((ub_o, lb_o))
                # Start new segment
                ub_o = (x, y)
                free_cells = False
            elif self.map.data[y, x] == 0 and not free_cells:
                ub_o = (x, y)
                lb_o = (x, y)

        return free_segments

    def update_path_constraints(self, wp_id, N, min_width, safety_margin):
        """
        Compute upper and lower bounds of the drivable area orthogonal to
        the given waypoint.
        """

        # container for constraints and border cells
        ub_hor = []
        lb_hor = []
        border_cells_hor = []

        # Iterate over horizon
        for n in range(N):

            # get corresponding waypoint
            wp = self.waypoints[wp_id+n]

            # Get list of free segments
            free_segments = self._compute_free_segments(wp, min_width)

            # First waypoint in horizon uses largest segment
            if n == 0:
                segment_lengths = [np.sqrt((seg[0][0]-seg[1][0])**2 +
                            (seg[0][1]-seg[1][1])**2) for seg in free_segments]
                ls_id = segment_lengths.index(max(segment_lengths))
                ub_ls, lb_ls = free_segments[ls_id]

            else:

                # Get border cells of selected segment at previous waypoint
                ub_pw, lb_pw = border_cells_hor[n-1]
                ub_pw, lb_pw = list(ub_pw), list(lb_pw)

                # Project border cells onto new waypoint in path direction
                wp_prev = self.waypoints[wp_id+n-1]
                delta_s = wp_prev - wp
                ub_pw[0] += delta_s * np.cos(wp_prev.psi)
                ub_pw[1] += delta_s * np.cos(wp_prev.psi)
                lb_pw[0] += delta_s * np.sin(wp_prev.psi)
                lb_pw[1] += delta_s * np.sin(wp_prev.psi)

                # Iterate over free segments for current waypoint
                if len(free_segments) >= 2:

                    # container for overlap of segments with projection
                    segment_offsets = []

                    for free_segment in free_segments:

                        # Get border cells of segment
                        ub_fs, lb_fs = free_segment

                        # distance between upper bounds and lower bounds
                        d_ub = np.sqrt((ub_fs[0]-ub_pw[0])**2 + (ub_fs[1]-ub_pw[1])**2)
                        d_lb = np.sqrt((lb_fs[0]-lb_pw[0])**2 + (lb_fs[1]-lb_pw[1])**2)
                        mean_dist = (d_ub + d_lb) / 2

                        # Append offset to projected previous segment
                        segment_offsets.append(mean_dist)

                    # Select segment with minimum offset to projected previous
                    # segment
                    ls_id = segment_offsets.index(min(segment_offsets))
                    ub_ls, lb_ls = free_segments[ls_id]

                # Select free segment in case of only one candidate
                elif len(free_segments) == 1:
                    ub_ls, lb_ls = free_segments[0]

                # Set waypoint coordinates as bound cells if no feasible
                # segment available
                else:
                    ub_ls, lb_ls = (wp.x, wp.y), (wp.x, wp.y)

            # 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)

            # Subtract safety margin
            ub -= safety_margin
            lb += safety_margin

            # Check feasibility of the path
            if ub < lb:
                # Upper and lower bound of 0 indicate an infeasible path
                # given the specified safety margin
                ub, lb = 0.0, 0.0

            # Compute absolute angle of bound cell
            angle_ub = np.mod(math.pi / 2 + wp.psi + math.pi,
                                  2 * math.pi) - math.pi
            angle_lb = np.mod(-math.pi / 2 + wp.psi + math.pi,
                                  2 * math.pi) - math.pi
            # Compute cell on bound for computed distance ub and lb
            ub_ls = wp.x + ub * np.cos(angle_ub), wp.y + ub * np.sin(
                    angle_ub)
            lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin(
                    angle_lb)
            bound_cells = (ub_ls, lb_ls)

            # Append results
            ub_hor.append(ub)
            lb_hor.append(lb)
            border_cells_hor.append(list(bound_cells))

        return np.array(ub_hor), np.array(lb_hor), border_cells_hor


if __name__ == '__main__':

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

    # 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.15,
                                       circular=True)
        # Add obstacles
        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])

    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,
                                    circular=False)
        obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20)
        obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25)
        obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15)
        obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25)
        obs5 = Obstacle(cx=2.2, cy=-0.86, radius=0.1)
        obs6 = Obstacle(cx=2.33, cy=-0.7, radius=0.1)
        obs7 = Obstacle(cx=2.67, cy=-0.73, radius=0.1)
        obs8 = Obstacle(cx=6.42, cy=3.97, radius=0.3)
        obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3)
        obs10 = Obstacle(cx=7.14, cy=5.7, radius=0.1)
        reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9, obs10])

    else:
        reference_path = None
        print('Invalid path!')
        exit(1)

    ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.60, 0.1)
    # Get x and y locations of border cells for upper and lower bound
    for wp_id in range(reference_path.n_waypoints):
        if ub[wp_id] > 0.0 and lb[wp_id] > 0.0:
            print(wp_id)
        reference_path.waypoints[wp_id].border_cells = border_cells[wp_id]
    reference_path.show()

    plt.show()