diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index 2e3eda54..6949c266 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 import time +from threading import Lock from numba import njit import numpy as np import osqp -import scipy from scipy import sparse import rospy @@ -20,7 +20,6 @@ from controller import Controller from world import World -from threading import Lock import matplotlib.pyplot as plt @@ -608,7 +607,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current return reference_trajectory state_trajectory += reference_trajectory - steer_rate_trajectory = solution_trajectory[:, :self.N_CONTROLS] + # steer_rate_trajectory = solution_trajectory[:, :self.N_CONTROLS] if self.TIME: solve_time = 1000 * (time.time() - t) @@ -624,7 +623,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current if self.PLOT: # Plot the results - fig, [[ax1, ax2], [ax3, ax4]] = plt.subplots(2, 2) + _, [[ax1, ax2], [ax3, ax4]] = plt.subplots(2, 2) # Extract the states states = np.zeros((self.MPC_HORIZON, self.N_STATES))