code clean up

Author: AtsushiSakai

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -42,10 +42,10 @@ def plot(self):
         plt.plot(gx, gy, "-xr")
 
     def calc_global_contour(self):
-        gx = [(ix * math.cos(self.yaw) + iy * math.sin(self.yaw)) +
-              self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
-        gy = [(ix * math.sin(self.yaw) - iy * math.cos(self.yaw)) +
-              self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
+        gx = [(ix * math.cos(self.yaw) + iy * math.sin(self.yaw))
+              + self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
+        gy = [(ix * math.sin(self.yaw) - iy * math.cos(self.yaw))
+              + self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
 
         return gx, gy
 
@@ -131,6 +131,40 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
     return rx, ry
 
 
+def adoptive_range_segmentation(ox, oy):
+
+    S = []
+
+    checked = [False] * len(ox)
+    R = 5.0
+
+    for i, _ in enumerate(ox):
+        if checked[i]:
+            continue
+        C = []
+        r = R
+        for j, _ in enumerate(ox):
+            d = math.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2)
+            if d <= r:
+                C.append(j)
+                checked[j] = True
+        S.append(C)
+
+    # Merge claster
+    fS = []
+    for k, _ in enumerate(S):
+        for l, _ in enumerate(S):
+            if k == l:
+                continue
+
+            for k, _ in enumerate(S[k]):
+
+    print(S)
+    input()
+
+    return S
+
+
 def main():
 
     # simulation parameters
@@ -153,6 +187,9 @@ def main():
 
         ox, oy = get_observation_points([v1, v2], angle_reso)
 
+        # step1: Adaptive Range Segmentation
+        ids = adoptive_range_segmentation(ox, oy)
+
         if show_animation:  # pragma: no cover
             plt.cla()
             plt.axis("equal")

PathPlanning/RRT/simple_rrt.py

@@ -94,7 +94,7 @@ def Planning(self, animation=True):
 
         return path
 
-    def DrawGraph(self, rnd=None):
+    def DrawGraph(self, rnd=None):  # pragma: no cover
         """
         Draw Graph
         """
@@ -162,7 +162,7 @@ def main():
     path = rrt.Planning(animation=show_animation)
 
     # Draw final path
-    if show_animation:
+    if show_animation:  # pragma: no cover
         rrt.DrawGraph()
         plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
         plt.grid(True)

PathTracking/rear_wheel_feedback/rear_wheel_feedback.py

@@ -1,16 +1,21 @@
-import cubic_spline_planner
-import matplotlib.pyplot as plt
-import math
 """
 
 Path tracking simulation with rear wheel feedback steering control and PID speed control.
 
 author: Atsushi Sakai(@Atsushi_twi)
 
 """
+import matplotlib.pyplot as plt
+import math
+import numpy as np
 import sys
 sys.path.append("../../PathPlanning/CubicSpline/")
 
+try:
+    import cubic_spline_planner
+except:
+    raise
+
 
 Kp = 1.0  # speed propotional gain
 # steering control parameter