6
6
7
7
"""
8
8
import matplotlib .pyplot as plt
9
+ import time
9
10
import cvxpy
10
11
import math
11
12
import numpy as np
12
13
import sys
13
14
import pathlib
14
- from utils .angle import angle_mod
15
15
sys .path .append (str (pathlib .Path (__file__ ).parent .parent .parent ))
16
16
17
+ from utils .angle import angle_mod
18
+
17
19
from PathPlanning .CubicSpline import cubic_spline_planner
18
20
19
21
NX = 4 # x = x, y, v, yaw
@@ -281,7 +283,7 @@ def linear_mpc_control(xref, xbar, x0, dref):
281
283
constraints += [cvxpy .abs (u [1 , :]) <= MAX_STEER ]
282
284
283
285
prob = cvxpy .Problem (cvxpy .Minimize (cost ), constraints )
284
- prob .solve (solver = cvxpy .ECOS , verbose = False )
286
+ prob .solve (solver = cvxpy .CLARABEL , verbose = False )
285
287
286
288
if prob .status == cvxpy .OPTIMAL or prob .status == cvxpy .OPTIMAL_INACCURATE :
287
289
ox = get_nparray_from_matrix (x .value [0 , :])
@@ -545,6 +547,7 @@ def get_switch_back_course(dl):
545
547
546
548
def main ():
547
549
print (__file__ + " start!!" )
550
+ start = time .time ()
548
551
549
552
dl = 1.0 # course tick
550
553
# cx, cy, cyaw, ck = get_straight_course(dl)
@@ -560,6 +563,9 @@ def main():
560
563
t , x , y , yaw , v , d , a = do_simulation (
561
564
cx , cy , cyaw , ck , sp , dl , initial_state )
562
565
566
+ elapsed_time = time .time () - start
567
+ print (f"calc time:{ elapsed_time :.6f} [sec]" )
568
+
563
569
if show_animation : # pragma: no cover
564
570
plt .close ("all" )
565
571
plt .subplots ()
@@ -582,6 +588,7 @@ def main():
582
588
583
589
def main2 ():
584
590
print (__file__ + " start!!" )
591
+ start = time .time ()
585
592
586
593
dl = 1.0 # course tick
587
594
cx , cy , cyaw , ck = get_straight_course3 (dl )
@@ -593,6 +600,9 @@ def main2():
593
600
t , x , y , yaw , v , d , a = do_simulation (
594
601
cx , cy , cyaw , ck , sp , dl , initial_state )
595
602
603
+ elapsed_time = time .time () - start
604
+ print (f"calc time:{ elapsed_time :.6f} [sec]" )
605
+
596
606
if show_animation : # pragma: no cover
597
607
plt .close ("all" )
598
608
plt .subplots ()
0 commit comments