Fix ModuleNotFoundError when executing test in the tests folder and little improve MPC controller (#619)

* Fix ModuleNotFoundError when executing test in the tests folder

Signed-off-by: Trung Kien <letrungkien.k53.hut@gmail.com>

* Improve model_predictive_speed_and_steer_control

- Fix typo
- Using @ for matrix multiplication instead of * with have been deprecated in CVXPY1.1
- Fix missing conftest module in test file

Signed-off-by: Trung Kien <letrungkien.k53.hut@gmail.com>

PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py

@@ -17,7 +17,8 @@ import matplotlib.pyplot as plt
 from matplotlib.collections import LineCollection
 import sys
 import os
-sys.path.append(os.path.relpath("../Eta3SplinePath"))
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../Eta3SplinePath")
 
 try:
     from eta3_spline_path import Eta3Path, Eta3PathSegment

PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py

@@ -7,12 +7,14 @@ author Atsushi Sakai (@Atsushi_twi)
 """
 import math
 import sys
+import os
 
 import matplotlib.pyplot as plt
 import numpy as np
 import scipy.linalg as la
 
-sys.path.append("../../PathPlanning/CubicSpline/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../PathPlanning/CubicSpline/")
 
 try:
     import cubic_spline_planner

PathTracking/lqr_steer_control/lqr_steer_control.py

@@ -10,7 +10,10 @@ import matplotlib.pyplot as plt
 import math
 import numpy as np
 import sys
-sys.path.append("../../PathPlanning/CubicSpline/")
+import os
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../PathPlanning/CubicSpline/")
 
 try:
     import cubic_spline_planner

PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py

@@ -10,7 +10,10 @@ import cvxpy
 import math
 import numpy as np
 import sys
-sys.path.append("../../PathPlanning/CubicSpline/")
+import os
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../PathPlanning/CubicSpline/")
 
 try:
     import cubic_spline_planner
@@ -175,9 +178,9 @@ def update_state(state, a, delta):
     state.yaw = state.yaw + state.v / WB * math.tan(delta) * DT
     state.v = state.v + a * DT
 
-    if state. v > MAX_SPEED:
+    if state.v > MAX_SPEED:
         state.v = MAX_SPEED
-    elif state. v < MIN_SPEED:
+    elif state.v < MIN_SPEED:
         state.v = MIN_SPEED
 
     return state
@@ -272,7 +275,7 @@ def linear_mpc_control(xref, xbar, x0, dref):
 
         A, B, C = get_linear_model_matrix(
             xbar[2, t], xbar[3, t], dref[0, t])
-        constraints += [x[:, t + 1] == A * x[:, t] + B * u[:, t] + C]
+        constraints += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C]
 
         if t < (T - 1):
             cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)

PathTracking/stanley_controller/stanley_controller.py

@@ -12,7 +12,10 @@ Ref:
 import numpy as np
 import matplotlib.pyplot as plt
 import sys
-sys.path.append("../../PathPlanning/CubicSpline/")
+import os
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../PathPlanning/CubicSpline/")
 
 try:
     import cubic_spline_planner

tests/test_model_predictive_speed_and_steer_control.py

@@ -1,4 +1,5 @@
 import sys
+import conftest
 
 if 'cvxpy' in sys.modules:  # pragma: no cover