mypy fix test

ArmNavigation/n_joint_arm_3d/NLinkArm3d.py

@@ -7,6 +7,7 @@ import math
 from mpl_toolkits.mplot3d import Axes3D
 import matplotlib.pyplot as plt
 
+
 class Link:
     def __init__(self, dh_params):
         self.dh_params_ = dh_params
@@ -28,16 +29,22 @@ class Link:
 
         return trans
 
-    def basic_jacobian(self, trans_prev, ee_pos):
+    @staticmethod
-        pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
+    def basic_jacobian(trans_prev, ee_pos):
-        z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
+        pos_prev = np.array(
+            [trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
+        z_axis_prev = np.array(
+            [trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
 
-        basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
+        basic_jacobian = np.hstack(
+            (np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
         return basic_jacobian
 
 
 class NLinkArm:
     def __init__(self, dh_params_list):
+        self.fig = plt.figure()
+        self.ax = Axes3D(self.fig)
         self.link_list = []
         for i in range(len(dh_params_list)):
             self.link_list.append(Link(dh_params_list[i]))
@@ -57,9 +64,6 @@ class NLinkArm:
         alpha, beta, gamma = self.euler_angle()
 
         if plot:
-            self.fig = plt.figure()
-            self.ax = Axes3D(self.fig)
-            
             x_list = []
             y_list = []
             z_list = []
@@ -75,7 +79,8 @@ class NLinkArm:
                 y_list.append(trans[1, 3])
                 z_list.append(trans[2, 3])
 
-            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
+                         mew=0.5)
             self.ax.plot([0], [0], [0], "o")
 
             self.ax.set_xlim(-1, 1)
@@ -92,7 +97,8 @@ class NLinkArm:
 
         trans = np.identity(4)
         for i in range(len(self.link_list)):
-            basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos))
+            basic_jacobian_mat.append(
+                self.link_list[i].basic_jacobian(trans, ee_pos))
             trans = np.dot(trans, self.link_list[i].transformation_matrix())
 
         return np.array(basic_jacobian_mat).T
@@ -105,13 +111,16 @@ class NLinkArm:
             basic_jacobian_mat = self.basic_jacobian()
             alpha, beta, gamma = self.euler_angle()
 
-            K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
+            K_zyz = np.array(
-                              [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
+                [[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
-                              [1, 0, math.cos(beta)]])
+                 [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
+                 [1, 0, math.cos(beta)]])
             K_alpha = np.identity(6)
             K_alpha[3:, 3:] = K_zyz
 
-            theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
+            theta_dot = np.dot(
+                np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha),
+                np.array(diff_pose))
             self.update_joint_angles(theta_dot / 100.)
 
         if plot:
@@ -133,26 +142,32 @@ class NLinkArm:
                 y_list.append(trans[1, 3])
                 z_list.append(trans[2, 3])
 
-            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+            self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
+                         mew=0.5)
             self.ax.plot([0], [0], [0], "o")
 
             self.ax.set_xlim(-1, 1)
             self.ax.set_ylim(-1, 1)
             self.ax.set_zlim(-1, 1)
 
-            self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
+            self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]],
+                         "o")
             plt.show()
 
     def euler_angle(self):
         trans = self.transformation_matrix()
 
         alpha = math.atan2(trans[1][2], trans[0][2])
-        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
+        if not (-math.pi / 2 <= alpha <= math.pi / 2):
             alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
-        if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
+        if not (-math.pi / 2 <= alpha <= math.pi / 2):
             alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
-        beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
+        beta = math.atan2(
-        gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
+            trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha),
+            trans[2][2])
+        gamma = math.atan2(
+            -trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha),
+            -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
 
         return alpha, beta, gamma
 
@@ -183,7 +198,8 @@ class NLinkArm:
             y_list.append(trans[1, 3])
             z_list.append(trans[2, 3])
 
-        self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
+        self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
+                     mew=0.5)
         self.ax.plot([0], [0], [0], "o")
 
         self.ax.set_xlabel("x")

ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py

@@ -3,9 +3,10 @@ Forward Kinematics for an n-link arm in 3D
 Author: Takayuki Murooka (takayuki5168)
 """
 import math
-from NLinkArm import NLinkArm
+from NLinkArm3d import NLinkArm
 import random
 
+
 def random_val(min_val, max_val):
     return min_val + random.random() * (max_val - min_val)
 
@@ -14,17 +15,17 @@ if __name__ == "__main__":
     print("Start solving Forward Kinematics 10 times")
 
     # init NLinkArm with Denavit-Hartenberg parameters of PR2    
-    n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
+    n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
-                           [math.pi/2, math.pi/2, 0., 0.],
+                           [math.pi / 2, math.pi / 2, 0., 0.],
-                           [0., -math.pi/2, 0., .4],
+                           [0., -math.pi / 2, 0., .4],
-                           [0., math.pi/2, 0., 0.],
+                           [0., math.pi / 2, 0., 0.],
-                           [0., -math.pi/2, 0., .321],
+                           [0., -math.pi / 2, 0., .321],
-                           [0., math.pi/2, 0., 0.],
+                           [0., math.pi / 2, 0., 0.],
                            [0., 0., 0., 0.]])
 
     # execute FK 10 times
     for i in range(10):
-        n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
+        n_link_arm.set_joint_angles(
+            [random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
 
         ee_pose = n_link_arm.forward_kinematics(plot=True)
-

ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py

@@ -3,23 +3,24 @@ Inverse Kinematics for an n-link arm in 3D
 Author: Takayuki Murooka (takayuki5168)
 """
 import math
-from NLinkArm import NLinkArm
+from NLinkArm3d import NLinkArm
 import random
 
 
 def random_val(min_val, max_val):
     return min_val + random.random() * (max_val - min_val)
 
+
 if __name__ == "__main__":
     print("Start solving Inverse Kinematics 10 times")
 
     # init NLinkArm with Denavit-Hartenberg parameters of PR2        
-    n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
+    n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
-                           [math.pi/2, math.pi/2, 0., 0.],
+                           [math.pi / 2, math.pi / 2, 0., 0.],
-                           [0., -math.pi/2, 0., .4],
+                           [0., -math.pi / 2, 0., .4],
-                           [0., math.pi/2, 0., 0.],
+                           [0., math.pi / 2, 0., 0.],
-                           [0., -math.pi/2, 0., .321],
+                           [0., -math.pi / 2, 0., .321],
-                           [0., math.pi/2, 0., 0.],
+                           [0., math.pi / 2, 0., 0.],
                            [0., 0., 0., 0.]])
 
     # execute IK 10 times

PathPlanning/AStar/a_star.py

@@ -41,7 +41,8 @@ class AStarPlanner:
             self.pind = pind
 
         def __str__(self):
-            return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
+            return str(self.x) + "," + str(self.y) + "," + str(
+                self.cost) + "," + str(self.pind)
 
     def planning(self, sx, sy, gx, gy):
         """
@@ -72,7 +73,10 @@ class AStarPlanner:
                 break
 
             c_id = min(
-                open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
+                open_set,
+                key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
+                                                                     open_set[
+                                                                         o]))
             current = open_set[c_id]
 
             # show graph
@@ -81,7 +85,8 @@ class AStarPlanner:
                          self.calc_grid_position(current.y, self.miny), "xc")
                 # for stopping simulation with the esc key.
                 plt.gcf().canvas.mpl_connect('key_release_event',
-                        lambda event: [exit(0) if event.key == 'escape' else None])
+                                             lambda event: [exit(
+                                                 0) if event.key == 'escape' else None])
                 if len(closed_set.keys()) % 10 == 0:
                     plt.pause(0.001)
 
@@ -104,7 +109,6 @@ class AStarPlanner:
                                  current.cost + self.motion[i][2], c_id)
                 n_id = self.calc_grid_index(node)
 
-
                 # If the node is not safe, do nothing
                 if not self.verify_node(node):
                     continue

PathPlanning/BSplinePath/bspline_path.py

@@ -11,7 +11,7 @@ import matplotlib.pyplot as plt
 import scipy.interpolate as scipy_interpolate
 
 
-def approximate_b_spline_path(x: int, y: list, n_path_points: int,
+def approximate_b_spline_path(x: list, y: list, n_path_points: int,
                               degree: int = 3) -> tuple:
     """
     approximate points with a B-Spline path

PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py

@@ -1,239 +0,0 @@
-"""
-cubic spline planner
-
-Author: Atsushi Sakai
-
-"""
-import math
-import numpy as np
-import bisect
-
-
-class Spline:
-    """
-    Cubic Spline class
-    """
-
-    def __init__(self, x, y):
-        self.b, self.c, self.d, self.w = [], [], [], []
-
-        self.x = x
-        self.y = y
-
-        self.nx = len(x)  # dimension of x
-        h = np.diff(x)
-
-        # calc coefficient c
-        self.a = [iy for iy in y]
-
-        # calc coefficient c
-        A = self.__calc_A(h)
-        B = self.__calc_B(h)
-        self.c = np.linalg.solve(A, B)
-        #  print(self.c1)
-
-        # calc spline coefficient b and d
-        for i in range(self.nx - 1):
-            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
-            tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
-                (self.c[i + 1] + 2.0 * self.c[i]) / 3.0
-            self.b.append(tb)
-
-    def calc(self, t):
-        """
-        Calc position
-
-        if t is outside of the input x, return None
-
-        """
-
-        if t < self.x[0]:
-            return None
-        elif t > self.x[-1]:
-            return None
-
-        i = self.__search_index(t)
-        dx = t - self.x[i]
-        result = self.a[i] + self.b[i] * dx + \
-            self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
-
-        return result
-
-    def calcd(self, t):
-        """
-        Calc first derivative
-
-        if t is outside of the input x, return None
-        """
-
-        if t < self.x[0]:
-            return None
-        elif t > self.x[-1]:
-            return None
-
-        i = self.__search_index(t)
-        dx = t - self.x[i]
-        result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
-        return result
-
-    def calcdd(self, t):
-        """
-        Calc second derivative
-        """
-
-        if t < self.x[0]:
-            return None
-        elif t > self.x[-1]:
-            return None
-
-        i = self.__search_index(t)
-        dx = t - self.x[i]
-        result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
-        return result
-
-    def __search_index(self, x):
-        """
-        search data segment index
-        """
-        return bisect.bisect(self.x, x) - 1
-
-    def __calc_A(self, h):
-        """
-        calc matrix A for spline coefficient c
-        """
-        A = np.zeros((self.nx, self.nx))
-        A[0, 0] = 1.0
-        for i in range(self.nx - 1):
-            if i != (self.nx - 2):
-                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
-            A[i + 1, i] = h[i]
-            A[i, i + 1] = h[i]
-
-        A[0, 1] = 0.0
-        A[self.nx - 1, self.nx - 2] = 0.0
-        A[self.nx - 1, self.nx - 1] = 1.0
-        #  print(A)
-        return A
-
-    def __calc_B(self, h):
-        """
-        calc matrix B for spline coefficient c
-        """
-        B = np.zeros(self.nx)
-        for i in range(self.nx - 2):
-            B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
-                h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
-        #  print(B)
-        return B
-
-
-class Spline2D:
-    """
-    2D Cubic Spline class
-
-    """
-
-    def __init__(self, x, y):
-        self.s = self.__calc_s(x, y)
-        self.sx = Spline(self.s, x)
-        self.sy = Spline(self.s, y)
-
-    def __calc_s(self, x, y):
-        dx = np.diff(x)
-        dy = np.diff(y)
-        self.ds = [math.sqrt(idx ** 2 + idy ** 2)
-                   for (idx, idy) in zip(dx, dy)]
-        s = [0]
-        s.extend(np.cumsum(self.ds))
-        return s
-
-    def calc_position(self, s):
-        """
-        calc position
-        """
-        x = self.sx.calc(s)
-        y = self.sy.calc(s)
-
-        return x, y
-
-    def calc_curvature(self, s):
-        """
-        calc curvature
-        """
-        dx = self.sx.calcd(s)
-        ddx = self.sx.calcdd(s)
-        dy = self.sy.calcd(s)
-        ddy = self.sy.calcdd(s)
-        k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
-        return k
-
-    def calc_yaw(self, s):
-        """
-        calc yaw
-        """
-        dx = self.sx.calcd(s)
-        dy = self.sy.calcd(s)
-        yaw = math.atan2(dy, dx)
-        return yaw
-
-
-def calc_spline_course(x, y, ds=0.1):
-    sp = Spline2D(x, y)
-    s = list(np.arange(0, sp.s[-1], ds))
-
-    rx, ry, ryaw, rk = [], [], [], []
-    for i_s in s:
-        ix, iy = sp.calc_position(i_s)
-        rx.append(ix)
-        ry.append(iy)
-        ryaw.append(sp.calc_yaw(i_s))
-        rk.append(sp.calc_curvature(i_s))
-
-    return rx, ry, ryaw, rk, s
-
-
-def main():  # pragma: no cover
-    print("Spline 2D test")
-    import matplotlib.pyplot as plt
-    x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
-    y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
-
-    sp = Spline2D(x, y)
-    s = np.arange(0, sp.s[-1], 0.1)
-
-    rx, ry, ryaw, rk = [], [], [], []
-    for i_s in s:
-        ix, iy = sp.calc_position(i_s)
-        rx.append(ix)
-        ry.append(iy)
-        ryaw.append(sp.calc_yaw(i_s))
-        rk.append(sp.calc_curvature(i_s))
-
-    plt.subplots(1)
-    plt.plot(x, y, "xb", label="input")
-    plt.plot(rx, ry, "-r", label="spline")
-    plt.grid(True)
-    plt.axis("equal")
-    plt.xlabel("x[m]")
-    plt.ylabel("y[m]")
-    plt.legend()
-
-    plt.subplots(1)
-    plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
-    plt.grid(True)
-    plt.legend()
-    plt.xlabel("line length[m]")
-    plt.ylabel("yaw angle[deg]")
-
-    plt.subplots(1)
-    plt.plot(s, rk, "-r", label="curvature")
-    plt.grid(True)
-    plt.legend()
-    plt.xlabel("line length[m]")
-    plt.ylabel("curvature [1/m]")
-
-    plt.show()
-
-
-if __name__ == '__main__':  # pragma: no cover
-    main()

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -6,9 +6,11 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 Ref:
 
-- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
+- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
+(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
 
-- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY)
+- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame]
+(https://www.youtube.com/watch?v=Cj6tAQe7UCY)
 
 """
 
@@ -16,19 +18,20 @@ import numpy as np
 import matplotlib.pyplot as plt
 import copy
 import math
-import cubic_spline_planner
 import sys
 import os
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../QuinticPolynomialsPlanner/")
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../CubicSpline/")
 
 try:
     from quintic_polynomials_planner import QuinticPolynomial
+    import cubic_spline_planner
 except ImportError:
     raise
 
-
 SIM_LOOP = 500
 
 # Parameter
@@ -38,36 +41,35 @@ MAX_CURVATURE = 1.0  # maximum curvature [1/m]
 MAX_ROAD_WIDTH = 7.0  # maximum road width [m]
 D_ROAD_W = 1.0  # road width sampling length [m]
 DT = 0.2  # time tick [s]
-MAXT = 5.0  # max prediction time [m]
+MAX_T = 5.0  # max prediction time [m]
-MINT = 4.0  # min prediction time [m]
+MIN_T = 4.0  # min prediction time [m]
 TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
 D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
 N_S_SAMPLE = 1  # sampling number of target speed
 ROBOT_RADIUS = 2.0  # robot radius [m]
 
 # cost weights
-KJ = 0.1
+K_J = 0.1
-KT = 0.1
+K_T = 0.1
-KD = 1.0
+K_D = 1.0
-KLAT = 1.0
+K_LAT = 1.0
-KLON = 1.0
+K_LON = 1.0
 
 show_animation = True
 
 
-class quartic_polynomial:
+class QuarticPolynomial:
-
-    def __init__(self, xs, vxs, axs, vxe, axe, T):
 
+    def __init__(self, xs, vxs, axs, vxe, axe, time):
         # calc coefficient of quartic polynomial
 
         self.a0 = xs
         self.a1 = vxs
         self.a2 = axs / 2.0
 
-        A = np.array([[3 * T ** 2, 4 * T ** 3],
+        A = np.array([[3 * time ** 2, 4 * time ** 3],
-                      [6 * T, 12 * T ** 2]])
+                      [6 * time, 12 * time ** 2]])
-        b = np.array([vxe - self.a1 - 2 * self.a2 * T,
+        b = np.array([vxe - self.a1 - 2 * self.a2 * time,
                       axe - 2 * self.a2])
         x = np.linalg.solve(A, b)
 
@@ -75,19 +77,19 @@ class quartic_polynomial:
         self.a4 = x[1]
 
     def calc_point(self, t):
-        xt = self.a0 + self.a1 * t + self.a2 * t**2 + \
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
-            self.a3 * t**3 + self.a4 * t**4
+             self.a3 * t ** 3 + self.a4 * t ** 4
 
         return xt
 
     def calc_first_derivative(self, t):
         xt = self.a1 + 2 * self.a2 * t + \
-            3 * self.a3 * t**2 + 4 * self.a4 * t**3
+             3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
 
         return xt
 
     def calc_second_derivative(self, t):
-        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2
+        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
 
         return xt
 
@@ -97,7 +99,7 @@ class quartic_polynomial:
         return xt
 
 
-class Frenet_path:
+class FrenetPath:
 
     def __init__(self):
         self.t = []
@@ -121,15 +123,14 @@ class Frenet_path:
 
 
 def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
-
     frenet_paths = []
 
     # generate path to each offset goal
     for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
 
         # Lateral motion planning
-        for Ti in np.arange(MINT, MAXT, DT):
+        for Ti in np.arange(MIN_T, MAX_T, DT):
-            fp = Frenet_path()
+            fp = FrenetPath()
 
             # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
             lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
@@ -141,9 +142,10 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
             fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
 
             # Longitudinal motion planning (Velocity keeping)
-            for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
+            for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
+                                TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
                 tfp = copy.deepcopy(fp)
-                lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
+                lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
 
                 tfp.s = [lon_qp.calc_point(t) for t in fp.t]
                 tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
@@ -154,11 +156,11 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
                 Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk
 
                 # square of diff from target speed
-                ds = (TARGET_SPEED - tfp.s_d[-1])**2
+                ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
 
-                tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2
+                tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
-                tfp.cv = KJ * Js + KT * Ti + KD * ds
+                tfp.cv = K_J * Js + K_T * Ti + K_D * ds
-                tfp.cf = KLAT * tfp.cd + KLON * tfp.cv
+                tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
 
                 frenet_paths.append(tfp)
 
@@ -166,7 +168,6 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
 
 
 def calc_global_paths(fplist, csp):
-
     for fp in fplist:
 
         # calc global positions
@@ -174,10 +175,10 @@ def calc_global_paths(fplist, csp):
             ix, iy = csp.calc_position(fp.s[i])
             if ix is None:
                 break
-            iyaw = csp.calc_yaw(fp.s[i])
+            i_yaw = csp.calc_yaw(fp.s[i])
             di = fp.d[i]
-            fx = ix + di * math.cos(iyaw + math.pi / 2.0)
+            fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
-            fy = iy + di * math.sin(iyaw + math.pi / 2.0)
+            fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
             fp.x.append(fx)
             fp.y.append(fy)
 
@@ -199,12 +200,11 @@ def calc_global_paths(fplist, csp):
 
 
 def check_collision(fp, ob):
-
     for i in range(len(ob[:, 0])):
-        d = [((ix - ob[i, 0])**2 + (iy - ob[i, 1])**2)
+        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
              for (ix, iy) in zip(fp.x, fp.y)]
 
-        collision = any([di <= ROBOT_RADIUS**2 for di in d])
+        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
 
         if collision:
             return False
@@ -213,38 +213,38 @@ def check_collision(fp, ob):
 
 
 def check_paths(fplist, ob):
-
+    ok_ind = []
-    okind = []
     for i, _ in enumerate(fplist):
         if any([v > MAX_SPEED for v in fplist[i].s_d]):  # Max speed check
             continue
-        elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # Max accel check
+        elif any([abs(a) > MAX_ACCEL for a in
+                  fplist[i].s_dd]):  # Max accel check
             continue
-        elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # Max curvature check
+        elif any([abs(c) > MAX_CURVATURE for c in
+                  fplist[i].c]):  # Max curvature check
             continue
         elif not check_collision(fplist[i], ob):
             continue
 
-        okind.append(i)
+        ok_ind.append(i)
 
-    return [fplist[i] for i in okind]
+    return [fplist[i] for i in ok_ind]
 
 
 def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):
-
     fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
     fplist = calc_global_paths(fplist, csp)
     fplist = check_paths(fplist, ob)
 
     # find minimum cost path
-    mincost = float("inf")
+    min_cost = float("inf")
-    bestpath = None
+    best_path = None
     for fp in fplist:
-        if mincost >= fp.cf:
+        if min_cost >= fp.cf:
-            mincost = fp.cf
+            min_cost = fp.cf
-            bestpath = fp
+            best_path = fp
 
-    return bestpath
+    return best_path
 
 
 def generate_target_course(x, y):
@@ -282,7 +282,7 @@ def main():
     c_speed = 10.0 / 3.6  # current speed [m/s]
     c_d = 2.0  # current lateral position [m]
     c_d_d = 0.0  # current lateral speed [m/s]
-    c_d_dd = 0.0  # current latral acceleration [m/s]
+    c_d_dd = 0.0  # current lateral acceleration [m/s]
     s0 = 0.0  # current course position
 
     area = 20.0  # animation area length [m]
@@ -304,8 +304,9 @@ def main():
         if show_animation:  # pragma: no cover
             plt.cla()
             # for stopping simulation with the esc key.
-            plt.gcf().canvas.mpl_connect('key_release_event',
+            plt.gcf().canvas.mpl_connect(
-                    lambda event: [exit(0) if event.key == 'escape' else None])
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
             plt.plot(tx, ty)
             plt.plot(ob[:, 0], ob[:, 1], "xk")
             plt.plot(path.x[1:], path.y[1:], "-or")

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -14,7 +14,7 @@ import matplotlib.pyplot as plt
 import sys
 sys.path.append("../ReedsSheppPath/")
 try:
-    from a_star import dp_planning  # , calc_obstacle_map
+    from a_star_heuristic import dp_planning  # , calc_obstacle_map
     import reeds_shepp_path_planning as rs
     from car import move, check_car_collision, MAX_STEER, WB, plot_car
 except: