From d2ab0148e8a625e9334f04b0ee134fb6d4b24846 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Sun, 3 Feb 2019 16:33:22 +0900
Subject: [PATCH] code clean up

---
 .../model_predictive_trajectory_generator.py  | 36 +++++--------------
 PathPlanning/RRTDubins/rrt_dubins.py          | 27 --------------
 2 files changed, 8 insertions(+), 55 deletions(-)

diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
index 1da57812..f6aaaa54 100644
--- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
+++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
@@ -16,7 +16,7 @@ max_iter = 100
 h = np.array([0.5, 0.02, 0.02]).T  # parameter sampling distance
 cost_th = 0.1
 
-show_animation = False
+show_animation = True
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):  # pragma: no cover
@@ -91,8 +91,7 @@ def selection_learning_param(dp, p, k0, target):
     return mina
 
 
-def show_trajectory(target, xc, yc):
-
+def show_trajectory(target, xc, yc):  # pragma: no cover
     plt.clf()
     plot_arrow(target.x, target.y, target.yaw)
     plt.plot(xc, yc, "-r")
@@ -142,35 +141,16 @@ def test_optimize_trajectory():
 
     x, y, yaw, p = optimize_trajectory(target, k0, init_p)
 
-    show_trajectory(target, x, y)
-    #  plt.plot(x, y, "-r")
-    plot_arrow(target.x, target.y, target.yaw)
-    plt.axis("equal")
-    plt.grid(True)
-    plt.show()
-
-
-def test_trajectory_generate():
-    s = 5.0  # [m]
-    k0 = 0.0
-    km = np.deg2rad(30.0)
-    kf = np.deg2rad(-30.0)
-
-    #  plt.plot(xk, yk, "xr")
-    #  plt.plot(t, kp)
-    #  plt.show()
-
-    x, y = motion_model.generate_trajectory(s, km, kf, k0)
-
-    plt.plot(x, y, "-r")
-    plt.axis("equal")
-    plt.grid(True)
-    plt.show()
+    if show_animation:
+        show_trajectory(target, x, y)
+        plot_arrow(target.x, target.y, target.yaw)
+        plt.axis("equal")
+        plt.grid(True)
+        plt.show()
 
 
 def main():
     print(__file__ + " start!!")
-    #  test_trajectory_generate()
     test_optimize_trajectory()
 
 
diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py
index 09e35ae8..0c5d41df 100644
--- a/PathPlanning/RRTDubins/rrt_dubins.py
+++ b/PathPlanning/RRTDubins/rrt_dubins.py
@@ -74,33 +74,6 @@ class RRT():
         path = self.gen_final_course(lastIndex)
         return path
 
-    def choose_parent(self, newNode, nearinds):
-        if not nearinds:
-            return newNode
-
-        dlist = []
-        for i in nearinds:
-            dx = newNode.x - self.nodeList[i].x
-            dy = newNode.y - self.nodeList[i].y
-            d = math.sqrt(dx ** 2 + dy ** 2)
-            theta = math.atan2(dy, dx)
-            if self.check_collision_extend(self.nodeList[i], theta, d):
-                dlist.append(self.nodeList[i].cost + d)
-            else:
-                dlist.append(float("inf"))
-
-        mincost = min(dlist)
-        minind = nearinds[dlist.index(mincost)]
-
-        if mincost == float("inf"):
-            print("mincost is inf")
-            return newNode
-
-        newNode.cost = mincost
-        newNode.parent = minind
-
-        return newNode
-
     def pi_2_pi(self, angle):
         return (angle + math.pi) % (2 * math.pi) - math.pi