diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index c55018cf..eb2d7b6a 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -6,14 +6,16 @@ author: Atsushi Sakai (@Atsushi_twi) """ import matplotlib.pyplot as plt +import time import cvxpy import math import numpy as np import sys import pathlib -from utils.angle import angle_mod sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) +from utils.angle import angle_mod + from PathPlanning.CubicSpline import cubic_spline_planner NX = 4 # x = x, y, v, yaw @@ -281,7 +283,7 @@ def linear_mpc_control(xref, xbar, x0, dref): constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER] prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints) - prob.solve(solver=cvxpy.ECOS, verbose=False) + prob.solve(solver=cvxpy.CLARABEL, verbose=False) if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE: ox = get_nparray_from_matrix(x.value[0, :]) @@ -545,6 +547,7 @@ def get_switch_back_course(dl): def main(): print(__file__ + " start!!") + start = time.time() dl = 1.0 # course tick # cx, cy, cyaw, ck = get_straight_course(dl) @@ -560,6 +563,9 @@ def main(): t, x, y, yaw, v, d, a = do_simulation( cx, cy, cyaw, ck, sp, dl, initial_state) + elapsed_time = time.time() - start + print(f"calc time:{elapsed_time:.6f} [sec]") + if show_animation: # pragma: no cover plt.close("all") plt.subplots() @@ -582,6 +588,7 @@ def main(): def main2(): print(__file__ + " start!!") + start = time.time() dl = 1.0 # course tick cx, cy, cyaw, ck = get_straight_course3(dl) @@ -593,6 +600,9 @@ def main2(): t, x, y, yaw, v, d, a = do_simulation( cx, cy, cyaw, ck, sp, dl, initial_state) + elapsed_time = time.time() - start + print(f"calc time:{elapsed_time:.6f} [sec]") + if show_animation: # pragma: no cover plt.close("all") plt.subplots() diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 909b39fa..76f203f4 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -1,7 +1,7 @@ numpy == 1.26.4 scipy == 1.13.0 matplotlib == 3.8.4 -cvxpy == 1.4.3 +cvxpy == 1.5.1 pytest == 8.2.0 # For unit test pytest-xdist == 3.6.1 # For unit test mypy == 1.10.0 # For unit test diff --git a/runtests.sh b/runtests.sh index a2a683e5..12d1b804 100755 --- a/runtests.sh +++ b/runtests.sh @@ -5,4 +5,4 @@ echo "Run test suites! " # -Werror: warning as error # --durations=0: show ranking of test durations # -l (--showlocals); show local variables when test failed -pytest -n auto tests -l -Werror --durations=0 +pytest tests -l -Werror --durations=0