code clean up

PathPlanning/AStar/a_star.py

@@ -85,7 +85,7 @@ def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
         closedset[c_id] = current
 
         # expand search grid based on motion model
-        for i in range(len(motion)):
+        for i, _ in enumerate(motion):
             node = Node(current.x + motion[i][0],
                         current.y + motion[i][1],
                         current.cost + motion[i][2], c_id)

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -5,9 +5,6 @@ author: AtsushiSakai(@Atsushi_twi)
 
 """
 
-import sys
-sys.path.append("../ReedsSheppPath/")
-
 import random
 import math
 import copy
@@ -15,8 +12,14 @@ import numpy as np
 import pure_pursuit
 import matplotlib.pyplot as plt
 
-import reeds_shepp_path_planning
-import unicycle_model
+import sys
+sys.path.append("../ReedsSheppPath/")
+
+try:
+    import reeds_shepp_path_planning
+    import unicycle_model
+except:
+    raise
 
 show_animation = True
 
@@ -130,8 +133,8 @@ class RRT():
             fy.append(self.end.y)
             fyaw.append(self.end.yaw)
             return True, fx, fy, fyaw, fv, ft, fa, fd
-        else:
-            return False, None, None, None, None, None, None, None
+
+        return False, None, None, None, None, None, None, None
 
     def calc_tracking_path(self, path):
         path = np.array(path[::-1])
@@ -194,7 +197,7 @@ class RRT():
         return find_goal, x, y, yaw, v, t, a, d
 
     def choose_parent(self, newNode, nearinds):
-        if len(nearinds) == 0:
+        if not nearinds:
             return newNode
 
         dlist = []

PathPlanning/Dijkstra/dijkstra.py

@@ -67,7 +67,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
         closedset[c_id] = current
 
         # expand search grid based on motion model
-        for i in range(len(motion)):
+        for i, _ in enumerate(motion):
             node = Node(current.x + motion[i][0], current.y + motion[i][1],
                         current.cost + motion[i][2], c_id)
             n_id = calc_index(node, xw, minx, miny)

PathPlanning/Eta3SplinePath/eta3_spline_path.py

@@ -185,8 +185,8 @@ class eta3_path_segment(object):
         assert(order > 0 and order <= 2)
         if order == 1:
             return self.coeffs[:, 1:].dot(np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6]))
-        else:
-            return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5]))
+
+        return self.coeffs[:, 2:].dot(np.array([2, 6. * u, 12. * u**2, 20. * u**3, 30. * u**4, 42. * u**5]))
 
 
 def test1():

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py

@@ -257,7 +257,7 @@ def check_collision(fp, ob):
 def check_paths(fplist, ob):
 
     okind = []
-    for i in range(len(fplist)):
+    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

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -1,222 +0,0 @@
-"""
-
-Hybrid A* path planning
-
-author: Atsushi Sakai (@Atsushi_twi)
-
-"""
-
-import sys
-sys.path.append("../ReedsSheppPath/")
-
-import math
-import numpy as np
-import scipy.spatial
-import matplotlib.pyplot as plt
-# import reeds_shepp_path_planning as rs
-# import heapq
-
-EXTEND_AREA = 5.0  # [m]
-H_COST = 1.0
-
-show_animation = True
-
-
-class Node:
-
-    def __init__(self, xind, yind, yawind, direction, x, y, yaw, directions, steer, cost, pind):
-        # store kd-tree
-        self.xind = xind
-        self.yind = yind
-        self.yawind = yawind
-        self.direction = direction
-        self.xlist = x
-        self.ylist = y
-        self.yawlist = yaw
-        self.directionlist = directions
-        self.steer = steer
-        self.cost = cost
-        self.pind = pind
-
-
-class KDTree:
-    """
-    Nearest neighbor search class with KDTree
-    """
-
-    def __init__(self, data):
-        # store kd-tree
-        self.tree = scipy.spatial.cKDTree(data)
-
-    def search(self, inp, k=1):
-        """
-        Search NN
-        inp: input data, single frame or multi frame
-        """
-
-        if len(inp.shape) >= 2:  # multi input
-            index = []
-            dist = []
-
-            for i in inp.T:
-                idist, iindex = self.tree.query(i, k=k)
-                index.append(iindex)
-                dist.append(idist)
-
-            return index, dist
-        else:
-            dist, index = self.tree.query(inp, k=k)
-            return index, dist
-
-    def search_in_distance(self, inp, r):
-        """
-        find points with in a distance r
-        """
-
-        index = self.tree.query_ball_point(inp, r)
-        return index
-
-
-class Config:
-
-    def __init__(self, ox, oy, xyreso, yawreso):
-        min_x_m = min(ox) - EXTEND_AREA
-        min_y_m = min(oy) - EXTEND_AREA
-        max_x_m = max(ox) + EXTEND_AREA
-        max_y_m = max(oy) + EXTEND_AREA
-
-        ox.append(min_x_m)
-        oy.append(min_y_m)
-        ox.append(max_x_m)
-        oy.append(max_y_m)
-
-        self.minx = int(min_x_m / xyreso)
-        self.miny = int(min_y_m / xyreso)
-        self.maxx = int(max_x_m / xyreso)
-        self.maxy = int(max_y_m / xyreso)
-
-        self.xw = int(self.maxx - self.minx)
-        self.yw = int(self.maxy - self.miny)
-
-        self.minyaw = int(- math.pi / yawreso) - 1
-        self.maxyaw = int(math.pi / yawreso)
-        self.yaww = int(self.maxyaw - self.minyaw)
-
-
-def analytic_expantion(current, ngoal, c, ox, oy, kdtree):
-
-    return False, None  # no update
-
-
-def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
-    """
-    start
-    goal
-    ox: x position list of Obstacles [m]
-    oy: y position list of Obstacles [m]
-    xyreso: grid resolution [m]
-    yawreso: yaw angle resolution [rad]
-    """
-
-    # start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
-    # tox, toy = ox[:], oy[:]
-
-    # obkdtree = KDTree(np.vstack((tox, toy)).T)
-
-    # c = Config(tox, toy, xyreso, yawreso)
-
-    # nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
-    # True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
-    # ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
-    # True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
-
-    # openList, closedList = {}, {}
-    # h = []
-    # #  goalqueue = queue.PriorityQueue()
-    # pq = []
-    # openList[calc_index(nstart, c)] = nstart
-    # heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c)))
-
-    # while True:
-    # if not openList:
-    # print("Error: Cannot find path, No open set")
-    # return [], [], []
-
-    # c_id, cost = heapq.heappop(pq)
-    # current = openList.pop(c_id)
-    # closedList[c_id] = current
-
-    # isupdated, fpath = analytic_expantion(
-    # current, ngoal, c, ox, oy, obkdtree)
-
-    # #  print(current)
-
-    rx, ry, ryaw = [], [], []
-
-    return rx, ry, ryaw
-
-
-def calc_cost(n, h, ngoal, c):
-
-    hcost = 1.0
-
-    return (n.cost + H_COST * hcost)
-
-
-def calc_index(node, c):
-    ind = (node.yawind - c.minyaw) * c.xw * c.yw + \
-        (node.yind - c.miny) * c.xw + (node.xind - c.minx)
-
-    if ind <= 0:
-        print("Error(calc_index):", ind)
-
-    return ind
-
-
-def main():
-    print("Start Hybrid A* planning")
-
-    ox, oy = [], []
-
-    for i in range(60):
-        ox.append(i)
-        oy.append(0.0)
-    for i in range(60):
-        ox.append(60.0)
-        oy.append(i)
-    for i in range(61):
-        ox.append(i)
-        oy.append(60.0)
-    for i in range(61):
-        ox.append(0.0)
-        oy.append(i)
-    for i in range(40):
-        ox.append(20.0)
-        oy.append(i)
-    for i in range(40):
-        ox.append(40.0)
-        oy.append(60.0 - i)
-
-    # Set Initial parameters
-    start = [10.0, 10.0, np.deg2rad(90.0)]
-    goal = [50.0, 50.0, np.deg2rad(-90.0)]
-
-    xyreso = 2.0
-    yawreso = np.deg2rad(15.0)
-
-    rx, ry, ryaw = hybrid_a_star_planning(
-        start, goal, ox, oy, xyreso, yawreso)
-
-    plt.plot(ox, oy, ".k")
-    # rs.plot_arrow(start[0], start[1], start[2])
-    # rs.plot_arrow(goal[0], goal[1], goal[2])
-
-    plt.grid(True)
-    plt.axis("equal")
-    plt.show()
-
-    print(__file__ + " start!!")
-
-
-if __name__ == '__main__':
-    main()

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -18,7 +18,7 @@ class State:
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2*math.pi) - math.pi
+    return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
 def update(state, v, delta, dt, L):
@@ -74,5 +74,6 @@ def generate_last_state(s, km, kf, k0):
 
     state = State()
 
-    [update(state, v, ikp, dt, L) for ikp in kp]
+    _ = [update(state, v, ikp, dt, L) for ikp in kp]
+
     return state.x, state.y, state.yaw

PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py

@@ -146,17 +146,17 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
             break
 
     if show_animation:
-        for i in range(len(rx)):
+        for i, _ in enumerate(rx):
             plt.cla()
             plt.grid(True)
             plt.axis("equal")
             plot_arrow(sx, sy, syaw)
             plot_arrow(gx, gy, gyaw)
             plot_arrow(rx[i], ry[i], ryaw[i])
-            plt.title("Time[s]:" + str(time[i])[0:4] +
-                      " v[m/s]:" + str(rv[i])[0:4] +
-                      " a[m/ss]:" + str(ra[i])[0:4] +
-                      " jerk[m/sss]:" + str(rj[i])[0:4],
+            plt.title("Time[s]:" + str(time[i])[0:4]
+                      + " v[m/s]:" + str(rv[i])[0:4]
+                      + " a[m/ss]:" + str(ra[i])[0:4]
+                      + " jerk[m/sss]:" + str(rj[i])[0:4],
                       )
             plt.pause(0.001)