code clean up

Author: AtsushiSakai

PathPlanning/HybridAStar/a_star.py

@@ -2,23 +2,19 @@
 
 A* grid based planning
 
-author: Atsushi Sakai(@Atsushi_twi)
-        Nikos Kanargias ([email protected])
+author: Nikos Kanargias ([email protected])
 
 See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
 
 """
 
-import matplotlib.pyplot as plt
 import math
 import heapq
-
-# _round = round
-# def round(x):
-#     return int(_round(x))
+import matplotlib.pyplot as plt
 
 show_animation = False
 
+
 class Node:
 
     def __init__(self, x, y, cost, pind):
@@ -67,7 +63,6 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
     openset[calc_index(ngoal, xw, minx, miny)] = ngoal
     pq = []
     pq.append((0, calc_index(ngoal, xw, minx, miny)))
-    
 
     while 1:
         if not pq:
@@ -87,7 +82,6 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
                 plt.pause(0.001)
 
         # Remove the item from the open set
-        
 
         # expand search grid based on motion model
         for i, _ in enumerate(motion):
@@ -104,15 +98,17 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
 
             if n_id not in openset:
                 openset[n_id] = node  # Discover a new node
-                heapq.heappush(pq, (node.cost, calc_index(node, xw, minx, miny)))
+                heapq.heappush(
+                    pq, (node.cost, calc_index(node, xw, minx, miny)))
             else:
                 if openset[n_id].cost >= node.cost:
                     # This path is the best until now. record it!
                     openset[n_id] = node
-                    heapq.heappush(pq, (node.cost, calc_index(node, xw, minx, miny)))
-
+                    heapq.heappush(
+                        pq, (node.cost, calc_index(node, xw, minx, miny)))
 
-    rx, ry = calc_final_path(closedset[calc_index(nstart, xw, minx, miny)], closedset, reso)
+    rx, ry = calc_final_path(closedset[calc_index(
+        nstart, xw, minx, miny)], closedset, reso)
 
     return rx, ry, closedset
 

PathPlanning/HybridAStar/car.py

@@ -1,51 +1,60 @@
+"""
+
+Car model for Hybrid A* path planning
+
+author: Zheng Zh (@Zhengzh)
+
+"""
+
 
 import matplotlib.pyplot as plt
 from math import sqrt, cos, sin, tan, pi
 
-WB = 3. # rear to front wheel
-W = 2. # width of car
-LF = 3.3 # distance from rear to vehicle front end
-LB = 1.0 # distance from rear to vehicle back end
-MAX_STEER = 0.6 #[rad] maximum steering angle 
+WB = 3.  # rear to front wheel
+W = 2.  # width of car
+LF = 3.3  # distance from rear to vehicle front end
+LB = 1.0  # distance from rear to vehicle back end
+MAX_STEER = 0.6  # [rad] maximum steering angle
 
-WBUBBLE_DIST = (LF-LB)/2.0
-WBUBBLE_R = sqrt(((LF+LB)/2.0)**2+1)
+WBUBBLE_DIST = (LF - LB) / 2.0
+WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1)
 
 # vehicle rectangle verticles
 VRX = [LF, LF, -LB, -LB, LF]
-VRY = [W/2,-W/2,-W/2,W/2,W/2]
+VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
 
 
 def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
     for x, y, yaw in zip(xlist, ylist, yawlist):
-        cx = x + WBUBBLE_DIST*cos(yaw)
-        cy = y + WBUBBLE_DIST*sin(yaw)
+        cx = x + WBUBBLE_DIST * cos(yaw)
+        cy = y + WBUBBLE_DIST * sin(yaw)
 
         ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R)
 
         if not ids:
             continue
-        
+
         if not rectangle_check(x, y, yaw,
-                [ox[i] for i in ids], [oy[i] for i in ids]):
-            return False # collision
-    
-    return True # no collision
+                               [ox[i] for i in ids], [oy[i] for i in ids]):
+            return False  # collision
+
+    return True  # no collision
+
 
 def rectangle_check(x, y, yaw, ox, oy):
     # transform obstacles to base link frame
     c, s = cos(-yaw), sin(-yaw)
     for iox, ioy in zip(ox, oy):
         tx = iox - x
         ty = ioy - y
-        rx = c*tx - s*ty
-        ry = s*tx + c*ty
+        rx = c * tx - s * ty
+        ry = s * tx + c * ty
 
-        if not (rx > LF or rx < -LB or ry > W/2.0 or ry < -W/2.0):
+        if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
             # print (x, y, yaw, iox, ioy, c, s, rx, ry)
-            return False # no collision
-    
-    return True # collision
+            return False  # no collision
+
+    return True  # collision
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
@@ -58,35 +67,38 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
                   fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
         # plt.plot(x, y)
 
+
 def plot_car(x, y, yaw):
     car_color = '-k'
     c, s = cos(yaw), sin(yaw)
-    
+
     car_outline_x, car_outline_y = [], []
     for rx, ry in zip(VRX, VRY):
-        tx = c*rx-s*ry + x
-        ty = s*rx+c*ry + y
+        tx = c * rx - s * ry + x
+        ty = s * rx + c * ry + y
         car_outline_x.append(tx)
         car_outline_y.append(ty)
-    
-    arrow_x, arrow_y, arrow_yaw = c*1.5+x, s*1.5+y, yaw
+
+    arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw
     plot_arrow(arrow_x, arrow_y, arrow_yaw)
-    
+
     plt.plot(car_outline_x, car_outline_y, car_color)
 
+
 def pi_2_pi(angle):
     return (angle + pi) % (2 * pi) - pi
 
+
 def move(x, y, yaw, distance, steer, L=WB):
     x += distance * cos(yaw)
     y += distance * sin(yaw)
-    yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
+    yaw += pi_2_pi(distance * tan(steer) / L)  # distance/2
 
     return x, y, yaw
 
+
 if __name__ == '__main__':
     x, y, yaw = 0., 0., 1.
     plt.axis('equal')
     plot_car(x, y, yaw)
-    plt.show()
-    
+    plt.show()