matssteinweg
parent
a773c1893a
commit
c7c0534fef
|
|
@ -0,0 +1,236 @@
|
|||
import numpy as np
|
||||
import math
|
||||
from map import Map
|
||||
from bresenham import bresenham
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
############
|
||||
# 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
|
||||
|
||||
def __sub__(self, other):
|
||||
return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5
|
||||
|
||||
|
||||
##################
|
||||
# Reference Path #
|
||||
##################
|
||||
|
||||
|
||||
class ReferencePath:
|
||||
def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, width_info=False):
|
||||
"""
|
||||
Reference Path object. Create a reference trajectory from specified
|
||||
corner points with given resolution. Smoothing around corners can be
|
||||
applied.
|
||||
"""
|
||||
# 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
|
||||
|
||||
# waypoints with x, y, psi, k
|
||||
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, :]))
|
||||
|
||||
def construct_path(self, wp_x, wp_y):
|
||||
|
||||
# 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]))
|
||||
|
||||
waypoints = list(zip(wp_xs, wp_ys))
|
||||
waypoints = self.spatial_reformulation(waypoints)
|
||||
return waypoints
|
||||
|
||||
def spatial_reformulation(self, waypoints):
|
||||
"""
|
||||
Reformulate conventional waypoints (x, y) coordinates into waypoint
|
||||
objects containing (x, y, psi, kappa)
|
||||
:return: list of waypoint objects for entire reference path
|
||||
"""
|
||||
|
||||
waypoints_spatial = []
|
||||
for wp_id in range(len(waypoints) - 1):
|
||||
|
||||
# get start and goal waypoints
|
||||
current_wp = np.array(waypoints[wp_id])
|
||||
next_wp = np.array(waypoints[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 = current_wp[0]
|
||||
y = current_wp[1]
|
||||
|
||||
# first waypoint
|
||||
if wp_id == 0:
|
||||
kappa = 0
|
||||
else:
|
||||
prev_wp = np.array(waypoints[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))
|
||||
|
||||
waypoints_spatial.append(Waypoint(x, y, psi, kappa))
|
||||
|
||||
return waypoints_spatial
|
||||
|
||||
def compute_width(self, max_dist=2.0):
|
||||
max_dist = max_dist # m
|
||||
width_info = np.zeros((6, len(self.waypoints)))
|
||||
for wp_id, wp in enumerate(self.waypoints):
|
||||
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
|
||||
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
|
||||
|
||||
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 = [], []
|
||||
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)
|
||||
|
||||
# get bresenham paths to all neighboring cells
|
||||
paths = []
|
||||
for t_x, t_y in zip(neighbors_x, neighbors_y):
|
||||
path = list(bresenham(wp_x, wp_y, t_x, t_y))
|
||||
paths.append(path)
|
||||
|
||||
min_dist = max_dist
|
||||
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
|
||||
if self.map.data[t_y, t_x] == 0:
|
||||
# get world coordinates
|
||||
x, y = self.map.m2w(wp_x, wp_y)
|
||||
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
|
||||
min_cell = (c_x, c_y)
|
||||
dist_info = np.array([min_dist, min_cell[0], min_cell[1]])
|
||||
return dist_info
|
||||
|
||||
def show(self):
|
||||
|
||||
# plot map
|
||||
plt.clf()
|
||||
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
|
||||
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')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
# Create Map
|
||||
map = Map(file_path='map_race.png', origin=[-1, -2], resolution=0.005)
|
||||
|
||||
# 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