def init_node(state_idx, cur, nxt):
n = Node()
n.state_idx = state_idx
n.x, n.y = cur
n.vehicle = PointCar(n.x, n.y)
dx, dy = n.vehicle.get_distance_components(nxt)
n.vehicle.theta = acos(dx/sqrt(dx**2 + dy**2))
return n
def min_distance():
IMG_PATH = "./tracks/loop.png"
track = load_track(IMG_PATH)
# Set to a valid point in trajectory
starting_position = (150., 200.)
car = PointCar(*starting_position)
n_loops = 1
trellis = path_finding.find_valid_trajectory(car,
track,
loops=n_loops,
states=31)
split_idx = (len(trellis) // n_loops) + 1 if n_loops > 1 else 0
c_idx = len(trellis[0]) // 2
in_idx = 0
centerline = [t[c_idx] for t in trellis]
print_stats(centerline)
inner_contour = [t[in_idx] for t in trellis]
print_stats(inner_contour)
path_v = viterbi.additive_viterbi(trellis, alpha=1.0, beta=0.0)
print_stats(path_v[split_idx:2 * split_idx])
path_s = optimal.run(trellis, alpha=1.0, beta=0.0)
print_stats(path_s[split_idx:2 * split_idx])
xv = [i[0] for i in path_v[split_idx:2 * split_idx]]
yv = [i[1] for i in path_v[split_idx:2 * split_idx]]
xs = [i[0] for i in path_s[split_idx:2 * split_idx]]
ys = [i[1] for i in path_s[split_idx:2 * split_idx]]
if a.plot:
fig, ax = plt.subplots()
ax.imshow(track)
plt.xlabel("Meters")
plt.ylabel("Meters")
ax.fill(xv,
yv,
facecolor='none',
edgecolor='green',
linestyle="-",
label="Distance Objective")
fig, ax = plt.subplots()
ax.imshow(track)
plt.xlabel("Meters")
plt.ylabel("Meters")
ax.fill(xs,
ys,
facecolor='none',
edgecolor='blue',
linestyle="-",
label="Time Objective")
plt.show()
def get_time(path):
car = PointCar(*path[0])
car.theta = car.heading(path[1])
for coor in path[1:]:
car.update(coor, is_coor=True)
return car.travel_time
def state_tradeoff():
IMG_PATH = "./tracks/loop.png"
track = load_track(IMG_PATH)
# Set to a valid point in trajectory
starting_position = (150., 200.)
car = PointCar(*starting_position)
for s in [1, 5, 10, 20, 40, 80]:
n_loops = 3
trellis = path_finding.find_valid_trajectory(car,
track,
loops=n_loops,
states=s)
split_idx = (len(trellis) // n_loops) + 1 if n_loops > 1 else 0
path_v = viterbi.additive_viterbi(trellis, alpha=1.0,
beta=0.0)[split_idx:2 * split_idx]
print_stats(path_v)
path_s = optimal.run(trellis, alpha=1.0,
beta=0.0)[split_idx:2 * split_idx]
print_stats(path_s)
def min_time():
IMG_PATH = "./tracks/loop.png"
track = load_track(IMG_PATH)
# Set to a valid point in trajectory
starting_position = (150., 200.)
car = PointCar(*starting_position)
n_loops = 3
trellis = path_finding.find_valid_trajectory(car,
track,
loops=n_loops,
states=30)
split_idx = (len(trellis) // n_loops) + 1 if n_loops > 1 else 0
path_v = viterbi.additive_viterbi(trellis, alpha=1.0, beta=0.0)
print_stats(path_v[split_idx:2 * split_idx])
path_s = optimal.run(trellis, alpha=0.5, beta=0.5)
print_stats(path_s[split_idx:])
xv = [i[0] for i in path_v[split_idx:2 * split_idx]]
yv = [i[1] for i in path_v[split_idx:2 * split_idx]]
xs = [i[0] for i in path_v[split_idx:2 * split_idx]]
ys = [i[1] for i in path_v[split_idx:2 * split_idx]]
if a.plot:
fig1, ax1 = plt.subplots()
ax1.imshow(track)
plt.xlabel("Meters")
plt.ylabel("Meters")
ax1.fill(xv, yv, facecolor='none', edgecolor='green', linestyle="-")
fig2, ax2 = plt.subplots()
ax2.imshow(track)
plt.xlabel("Meters")
plt.ylabel("Meters")
ax2.fill(xs, ys, facecolor='none', edgecolor='blue', linestyle="-")
plt.show()
nopass = [x for x in range(mgraph.shape[0]) if x != start]
dis = mgraph[start]
while len(nopass):
idx = nopass[0]
for i in nopass:
if dis[i] < dis[idx]: idx = i
nopass.remove(idx)
passed.append(idx)
for i in nopass:
if dis[idx] + mgraph[idx][i] < dis[i]: dis[i] = dis[idx] + mgraph[idx][i]
return dis
if __name__ == "__main__":
IMG_PATH = "./tracks/loop.png"
track = load_track(IMG_PATH)
a = datetime.datetime.now()
# Set to a valid point in trajectory
car = PointCar(150, 200)
trellis = find_valid_trajectory(car, track)
matrix = build_matrix(trellis)
dijstra_m = dijstra_O2(0, matrix)
for item in dijstra_m[-10:]:
print(item)
b = datetime.datetime.now()
print(b-a)
n_nodes = 0
while cur is not None:
path.append(cur.vehicle.location)
cur = cur.prev
n_nodes += 1
return np.array(list(reversed(path)))
if __name__ == "__main__":
IMG_PATH = "./tracks/loop.png"
track = load_track(IMG_PATH)
# Set to a valid point in trajectory
starting_position = (150., 200.)
car = PointCar(*starting_position)
fig, ax = plt.subplots()
ax.imshow(track)
plt.xlabel("Meters")
plt.ylabel("Meters")
# baseline_trellis = find_valid_trajectory(car, track, states=1)
# baseline = additive_viterbi(baseline_trellis, starting_position, centerline_score)
# ax.fill(baseline[:,0], baseline[:,1], facecolor='none', edgecolor='black', linestyle="-.", label="Centerline")
n_loops = 2
trellis = find_valid_trajectory(car, track, loops=n_loops, states=30)
split_idx = (len(trellis) // n_loops) + 1 if n_loops > 1 else 0
time = additive_viterbi(trellis)
ax.fill(time[split_idx:,0], time[split_idx:,1], facecolor='none', edgecolor='red', linestyle="-", label="Time Objective")