while car.s < reference_path.length:
# Get control signals
u = mpc.get_control()
# Simulate car
car.drive(u)
# Log car state
x_log.append(car.temporal_state.x)
y_log.append(car.temporal_state.y)
v_log.append(u[0])
# Increment simulation time
t += car.Ts
# Plot path and drivable area
reference_path.show()
# Plot car
car.show()
# Plot MPC prediction
mpc.show_prediction()
# Set figure title
plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: '
'{:.2f} s'.format(u[0], u[1], t))
plt.axis('off')
plt.pause(0.001)
"""
Run MPC main loop:
"""
k = 0
while globals.s < reference_path.length:
vehicle.get_current_waypoint()
print("======= wp_id ======== ", vehicle.wp_id)
u0 = mpc.make_step(x0)
x0 = simulator.make_step(u0)
controller.distance_update(x0)
if show_animation:
# Plot path and drivable area
reference_path.show(wp=vehicle.current_waypoint)
vehicle.show(x0)
plt.axis('off')
plt.pause(0.001)
plt.show()
graphics.plot_results(t_ind=k)
graphics.plot_predictions(t_ind=k)
graphics.reset_axes()
plt.show()
plt.pause(0.01)
k += 1
input('Press any key to exit.')