12345qiupeng
/
Multi-Purpose-MPC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Multi-Purpose-MPC/reference_path.py

553 lines
21 KiB
Python
Raw Blame History

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.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, n_extension, 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 n_extension: number of samples used for path extension to allow
for MPC
: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
# Number of waypoints used to extend path at the end
self.n_extension = n_extension
# Circular
self.circular = circular
# List of waypoint objects
self.waypoints = self._construct_path(wp_x, wp_y)
# 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]
if self.n_extension is not None:
if self.circular:
wp_x += wp_x[:self.n_extension]
wp_y += wp_y[:self.n_extension]
else:
wp_x += wp_x[-self.n_extension:]
wp_y += wp_y[-self.n_extension:]
# 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)-self.n_extension-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)
# 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
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, y_list):
# 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][:-self.n_extension])
wp_y = np.array([wp.y for wp in self.waypoints][:-self.n_extension])
# 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][:-self.n_extension])
wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints][:-self.n_extension])
wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints][:-self.n_extension])
wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints][:-self.n_extension])
# 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.n_extension] +
[self.waypoints[0].border_cells[0][0]])
bl_y = np.array([wp.border_cells[0][1] for wp in
self.waypoints][:-self.n_extension] +
[self.waypoints[0].border_cells[0][1]])
br_x = np.array([wp.border_cells[1][0] for wp in
self.waypoints][:-self.n_extension] +
[self.waypoints[0].border_cells[1][0]])
br_y = np.array([wp.border_cells[1][1] for wp in
self.waypoints][:-self.n_extension] +
[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 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()
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.22, n_extension=30,
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,
n_extension=30, 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)
reference_path.add_obstacles([obs1, obs2, obs3, obs4])
else:
reference_path = None
print('Invalid path!')
exit(1)
reference_path.show()
plt.show()
Reference in New Issue View Git Blame Copy Permalink