matssteinweg
parent
76df3ce814
commit
e87596d5e8
|
|
@ -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()
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue