imporve covergage ratio

Author: AtsushiSakai

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -138,26 +138,6 @@ def search_best_feasible_path(self, path_indexs):
 
         return False, None, None, None, None, None, None, None
 
-    def calc_tracking_path(self, path):
-        path = np.array(path[::-1])
-        ds = 0.2
-        for i in range(10):
-            lx = path[-1, 0]
-            ly = path[-1, 1]
-            lyaw = path[-1, 2]
-            move_yaw = math.atan2(path[-2, 1] - ly, path[-2, 0] - lx)
-            if abs(lyaw - move_yaw) >= math.pi / 2.0:
-                print("back")
-                ds *= -1
-
-            lstate = np.array(
-                [lx + ds * math.cos(lyaw), ly + ds * math.sin(lyaw), lyaw])
-            #  print(lstate)
-
-            path = np.vstack((path, lstate))
-
-        return path
-
     def check_tracking_path_is_feasible(self, path):
         #  print("check_tracking_path_is_feasible")
         cx = np.array(path[:, 0])
@@ -311,9 +291,9 @@ def find_near_nodes(self, newNode):
         nnode = len(self.nodeList)
         r = 50.0 * math.sqrt((math.log(nnode) / nnode))
         #  r = self.expandDis * 5.0
-        dlist = [(node.x - newNode.x) ** 2 +
-                 (node.y - newNode.y) ** 2 +
-                 (node.yaw - newNode.yaw) ** 2
+        dlist = [(node.x - newNode.x) ** 2
+                 + (node.y - newNode.y) ** 2
+                 + (node.yaw - newNode.yaw) ** 2
                  for node in self.nodeList]
         nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
         return nearinds
@@ -336,7 +316,7 @@ def rewire(self, newNode, nearinds):
                 #  print("rewire")
                 self.nodeList[i] = tNode
 
-    def DrawGraph(self, rnd=None):
+    def DrawGraph(self, rnd=None):  # pragma: no cover
         """
         Draw Graph
         """
@@ -359,9 +339,9 @@ def DrawGraph(self, rnd=None):
         plt.pause(0.01)
 
     def GetNearestListIndex(self, nodeList, rnd):
-        dlist = [(node.x - rnd.x) ** 2 +
-                 (node.y - rnd.y) ** 2 +
-                 (node.yaw - rnd.yaw) ** 2 for node in nodeList]
+        dlist = [(node.x - rnd.x) ** 2
+                 + (node.y - rnd.y) ** 2
+                 + (node.yaw - rnd.yaw) ** 2 for node in nodeList]
         minind = dlist.index(min(dlist))
 
         return minind

PathPlanning/ClosedLoopRRTStar/pure_pursuit.py

@@ -131,15 +131,15 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
         a.append(ai)
         d.append(di)
 
-        if target_ind % 1 == 0 and animation:
+        if target_ind % 1 == 0 and animation:  # pragma: no cover
             plt.cla()
             plt.plot(cx, cy, "-r", label="course")
             plt.plot(x, y, "ob", label="trajectory")
             plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
             plt.axis("equal")
             plt.grid(True)
-            plt.title("speed:" + str(round(state.v, 2)) +
-                      "tind:" + str(target_ind))
+            plt.title("speed:" + str(round(state.v, 2))
+                      + "tind:" + str(target_ind))
             plt.pause(0.0001)
 
     else:
@@ -196,7 +196,7 @@ def calc_speed_profile(cx, cy, cyaw, target_speed):
 
     speed_profile, d = set_stop_point(target_speed, cx, cy, cyaw)
 
-    if animation:
+    if animation:  # pragma: no cover
         plt.plot(speed_profile, "xb")
 
     return speed_profile
@@ -220,7 +220,7 @@ def extend_path(cx, cy, cyaw):
     return cx, cy, cyaw
 
 
-def main():
+def main():  # pragma: no cover
     #  target course
     cx = np.arange(0, 50, 0.1)
     cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
@@ -277,15 +277,15 @@ def main():
     plt.axis("equal")
     plt.grid(True)
 
-    subplots(1)
+    plt.subplots(1)
     plt.plot(t, [iv * 3.6 for iv in v], "-r")
     plt.xlabel("Time[s]")
     plt.ylabel("Speed[km/h]")
     plt.grid(True)
     plt.show()
 
 
-def main2():
+def main2():  # pragma: no cover
     import pandas as pd
     data = pd.read_csv("rrt_course.csv")
     cx = np.array(data["x"])
@@ -321,7 +321,7 @@ def main2():
     plt.show()
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     print("Pure pursuit path tracking simulation start")
     #  main()
     main2()

PathPlanning/ClosedLoopRRTStar/unicycle_model.py

@@ -42,7 +42,7 @@ def pi_2_pi(angle):
     return (angle + math.pi) % (2 * math.pi) - math.pi
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     print("start unicycle simulation")
     import matplotlib.pyplot as plt
 

PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

@@ -186,7 +186,6 @@ def gen_final_course(self, goalind):
             node = self.nodeList[goalind]
             for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
                 path.append([ix, iy])
-            #  path.append([node.x, node.y])
             goalind = node.parent
         path.append([self.start.x, self.start.y])
         return path
@@ -222,10 +221,7 @@ def rewire(self, newNode, nearinds):
                 #  print("rewire")
                 self.nodeList[i] = tNode
 
-    def DrawGraph(self, rnd=None):
-        """
-        Draw Graph
-        """
+    def DrawGraph(self, rnd=None):  # pragma: no cover
         plt.clf()
         if rnd is not None:
             plt.plot(rnd.x, rnd.y, "^k")
@@ -313,7 +309,7 @@ def main(maxIter=200):
     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)

tests/test_a_star.py

@@ -15,6 +15,6 @@ def test1(self):
         m.main()
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()

tests/test_batch_informed_rrt_star.py

@@ -17,6 +17,6 @@ def test1(self):
         m.main(maxIter=10)
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()

tests/test_closed_loop_rrt_star_car.py

@@ -20,6 +20,6 @@ def test1(self):
         m.main(gx=1.0, gy=0.0, gyaw=0.0, maxIter=5)
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()

tests/test_dynamic_window_approach.py

@@ -18,6 +18,6 @@ def test1(self):
         m.main(gx=1.0, gy=1.0)
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()

tests/test_fast_slam1.py

@@ -19,6 +19,6 @@ def test1(self):
         m.main()
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()

tests/test_fast_slam2.py

@@ -19,6 +19,6 @@ def test1(self):
         m.main()
 
 
-if __name__ == '__main__':
+if __name__ == '__main__':  # pragma: no cover
     test = Test()
     test.test1()