From e87596d5e89e7679c82b76114116983b824f07cf Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 01:56:15 +0100 Subject: [PATCH] Add comments and restructure reference path class. Add update_bounds method. --- reference_path.py | 341 +++++++++++++++++++++++++++++++++------------- 1 file changed, 250 insertions(+), 91 deletions(-) diff --git a/reference_path.py b/reference_path.py index 50d21cb..80378ab 100644 --- a/reference_path.py +++ b/reference_path.py @@ -24,7 +24,20 @@ class Waypoint: 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 @@ -34,35 +47,46 @@ class Waypoint: class ReferencePath: - def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, width_info=False): + 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. + 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 + + # Precision self.eps = 1e-12 - # map + # Map self.map = map - # resolution of the path + # Resolution of the path self.resolution = resolution - # look ahead distance for path averaging + # Look ahead distance for path averaging self.smoothing_distance = smoothing_distance - # waypoints with x, y, psi, k - self.waypoints = self.construct_path(wp_x, wp_y) + # List of waypoint objects + self.waypoints = self._construct_path(wp_x, wp_y) - # path width - self.get_width_info = width_info - if self.get_width_info: - self.width_info = self.compute_width() - self.min_width = (np.min(self.width_info[0, :]), - np.min(self.width_info[3, :])) + # Compute path width (attribute of each waypoint) + self._compute_width(max_width=max_width) - def construct_path(self, wp_x, wp_y): + 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 + @@ -78,7 +102,7 @@ class ReferencePath: 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 + # Smooth path wp_xs = [] wp_ys = [] for wp_id in range(self.smoothing_distance, len(wp_x) - @@ -88,147 +112,282 @@ class ReferencePath: 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.spatial_reformulation(waypoints) + waypoints = self._construct_waypoints(waypoints) + return waypoints - def spatial_reformulation(self, waypoints): + def _construct_waypoints(self, waypoint_coordinates): """ Reformulate conventional waypoints (x, y) coordinates into waypoint - objects containing (x, y, psi, kappa) + 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 """ - waypoints_spatial = [] - for wp_id in range(len(waypoints) - 1): + # List containing waypoint objects + waypoints = [] - # get start and goal waypoints - current_wp = np.array(waypoints[wp_id]) - next_wp = np.array(waypoints[wp_id + 1]) + # Iterate over all waypoints + for wp_id in range(len(waypoint_coordinates) - 1): - # difference vector + # 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 + # Angle ahead psi = np.arctan2(dif_ahead[1], dif_ahead[0]) - # distance to next waypoint + # Distance to next waypoint dist_ahead = np.linalg.norm(dif_ahead, 2) - # get x and y coordinates of current waypoint - x = current_wp[0] - y = current_wp[1] + # 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(waypoints[wp_id - 1]) + 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 = np.abs(angle_dif / (dist_ahead + self.eps)) + kappa = angle_dif / (dist_ahead + self.eps) - waypoints_spatial.append(Waypoint(x, y, psi, kappa)) + waypoints.append(Waypoint(x, y, psi, kappa)) - return waypoints_spatial + return waypoints - def compute_width(self, max_dist=2.0): - max_dist = max_dist # m - width_info = np.zeros((6, len(self.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 pixel coordinates of waypoint - wp_x, wp_y = self.map.w2m(wp.x, wp.y) - # get angle orthogonal to path in current direction + # 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_dist * np.cos(angle), wp.y + max_dist * np.sin(angle)) - # compute path between cells - width_info[3*i:3*(i+1), wp_id] = self.get_min_dist(wp_x, wp_y, t_x, t_y, max_dist) - return width_info + # 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) - def get_min_dist(self, wp_x, wp_y, t_x, t_y, max_dist): - # get neighboring cells (account for discretization) - neighbors_x, neighbors_y = [], [] + # 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): - neighbors_x.append(t_x + i) - neighbors_y.append(t_y + j) + tn_x.append(t_x+i) + tn_y.append(t_y+j) - # get bresenham paths to all neighboring cells + # 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(neighbors_x, neighbors_y): + 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) - min_dist = max_dist + # 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 = cell[0] - t_y = cell[1] - # if path goes through occupied cell + 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 - x, y = self.map.m2w(wp_x, wp_y) + # Get world coordinates c_x, c_y = self.map.m2w(t_x, t_y) - cell_dist = np.sqrt((x - c_x) ** 2 + (y - c_y) ** 2) - if cell_dist < min_dist: - min_dist = cell_dist + 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) - dist_info = np.array([min_dist, min_cell[0], min_cell[1]]) - return dist_info - def show(self): + # decrease min_width by radius of circle around cell + min_width -= 1 / np.sqrt(2) * self.map.resolution - # plot map + return min_width, min_cell + + def update_bounds(self, wp_id): + """ + Compute upper and lower bounds of the drivable area orthogonal to + the given waypoint. + :param wp_id: ID of reference waypoint + """ + + # 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) + + # Update member variables of waypoint + wp.ub = ub + wp.lb = lb + wp.border_cells = (ub_ls, lb_ls) + + return lb, ub + + 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() - plt.imshow(np.flipud(self.map.data),cmap='gray', + + # Plot map in gray-scale and set extent to match world coordinates + plt.imshow(np.flipud(self.map.data), 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]) - # plot reference path + + # 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]) - plt.scatter(wp_x, wp_y, color='k', s=5) - if self.get_width_info: - print('Min Width Left: {:f} m'.format(self.min_width[0])) - print('Min Width Right: {:f} m'.format(self.min_width[1])) - plt.quiver(wp_x, wp_y, self.width_info[1, :] - wp_x, - self.width_info[2, :] - wp_y, scale=1, units='xy', - width=0.05, color='#D4AC0D') - plt.quiver(wp_x, wp_y, self.width_info[4, :] - wp_x, - self.width_info[5, :] - wp_y, scale=1, units='xy', - width=0.05, color='#BA4A00') + # 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='#99A3A4', 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='#2ECC71', + headwidth=1, headlength=2) + 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='#2ECC71', + headwidth=1, headlength=2) if __name__ == '__main__': + # Select Path | 'Race' or 'Q' + path = 'Race' + # Create Map - map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005) + 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) - # Specify waypoints - # Floor 2 - # wp_x = [-9.169, -2.7, 11.9, 7.3, -6.95] - # wp_y = [-15.678, -7.12, 10.9, 14.5, -3.31] - # Race Track - 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 - - # Smooth Path - reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5) reference_path.show() plt.show()