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 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)
print(metrics.summed_distance(x, y)) car = PointCar(*path[0]) car.theta = car.heading(path[1]) for coor in path[1:]: car.update(coor, is_coor=True) print(car.travel_time) if __name__ == "__main__": IMG_PATH = "./tracks/loop.png" track = load_track(IMG_PATH) # Set to a valid point in trajectory car = PointCar(150, 200) STATES = 30 # a waypoint for every 0.5 of the track width trellis = find_valid_trajectory(car, track, states=STATES) run_gridsearch(trellis, n_segments=10) # path = run(trellis, alpha=.9, beta=0.1) # x = [i[0] for i in path] # y = [i[1] for i in path] # print(metrics.summed_distance(x, y)) # car = PointCar(*path[0]) # car.theta = car.heading(path[1]) # for coor in path[1:]: # car.update(coor, is_coor=True) # print(car.travel_time)
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") # distance = additive_viterbi(trellis, starting_position, distance_score) # ax.fill(distance[:split_idx,0], distance[:split_idx,1], facecolor='none', edgecolor='blue', linestyle="-", label="Distance Objective") plt.legend(loc=4) plt.show() ## Trellis Video plot # fig = plt.figure() # for states in trellis: # plt.imshow(track) # plt.xlabel("Meter")