From 6ec93febd3ab65118c392dc5e70f1aeee48b848f Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sun, 1 Dec 2019 15:38:17 +0100 Subject: [PATCH] Add obstacle class and function add_obstacles to add obstacles to path. Restructure class ReferencePath a bit. Add safety_margin to update_bounds function. --- reference_path.py | 133 +++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 125 insertions(+), 8 deletions(-) diff --git a/reference_path.py b/reference_path.py index 1659f82..e8fc8ed 100644 --- a/reference_path.py +++ b/reference_path.py @@ -3,6 +3,13 @@ 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' ############ @@ -41,13 +48,43 @@ class Waypoint: 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): + 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 @@ -80,6 +117,9 @@ class ReferencePath: # 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. @@ -250,13 +290,17 @@ class ReferencePath: return min_width, min_cell - def update_bounds(self, wp_id): + 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] @@ -311,6 +355,10 @@ class ReferencePath: 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 @@ -318,6 +366,24 @@ class ReferencePath: 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. @@ -328,12 +394,18 @@ class ReferencePath: # Clear figure plt.clf() + # Disabled ticks + plt.xticks([]) + plt.yticks([]) + # Plot map in gray-scale and set extent to match world coordinates - plt.imshow(np.flipud(self.map.data), cmap='gray', + 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]) + 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]) @@ -346,22 +418,56 @@ class ReferencePath: 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) + 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=1.5*self.resolution, color='#2ECC71', + 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=1.5*self.resolution, color='#2ECC71', + 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 = 'Q' + path = 'Race' # Create Map if path == 'Race': @@ -388,6 +494,17 @@ if __name__ == '__main__': 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()