diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 8af118fa..927fcd94 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -138,26 +138,6 @@ class RRT(): 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 @@ class RRT(): 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 @@ class RRT(): # 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 @@ class RRT(): 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 diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 984c4a21..c90af156 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/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,7 +277,7 @@ 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]") @@ -285,7 +285,7 @@ def main(): 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() diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py index 2b7fbe72..5a0fd17a 100644 --- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py +++ b/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 diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index 6225071e..489cf087 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -186,7 +186,6 @@ class RRT(): 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 @@ class RRT(): # 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) diff --git a/tests/test_a_star.py b/tests/test_a_star.py index c1848956..61276e6a 100644 --- a/tests/test_a_star.py +++ b/tests/test_a_star.py @@ -15,6 +15,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py index 12257285..e83684d9 100644 --- a/tests/test_batch_informed_rrt_star.py +++ b/tests/test_batch_informed_rrt_star.py @@ -17,6 +17,6 @@ class Test(TestCase): m.main(maxIter=10) -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_closed_loop_rrt_star_car.py b/tests/test_closed_loop_rrt_star_car.py index d8dd84fe..f82d3f25 100644 --- a/tests/test_closed_loop_rrt_star_car.py +++ b/tests/test_closed_loop_rrt_star_car.py @@ -20,6 +20,6 @@ class Test(TestCase): 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() diff --git a/tests/test_dynamic_window_approach.py b/tests/test_dynamic_window_approach.py index 901bdcce..ab2e0a27 100644 --- a/tests/test_dynamic_window_approach.py +++ b/tests/test_dynamic_window_approach.py @@ -18,6 +18,6 @@ class Test(TestCase): m.main(gx=1.0, gy=1.0) -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_fast_slam1.py b/tests/test_fast_slam1.py index 9d9bf895..4b15e65c 100644 --- a/tests/test_fast_slam1.py +++ b/tests/test_fast_slam1.py @@ -19,6 +19,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_fast_slam2.py b/tests/test_fast_slam2.py index 2f019089..4c1c967a 100644 --- a/tests/test_fast_slam2.py +++ b/tests/test_fast_slam2.py @@ -19,6 +19,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_frenet_optimal_trajectory.py b/tests/test_frenet_optimal_trajectory.py index 3ba0890b..7a0ba517 100644 --- a/tests/test_frenet_optimal_trajectory.py +++ b/tests/test_frenet_optimal_trajectory.py @@ -21,6 +21,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_graph_based_slam.py b/tests/test_graph_based_slam.py index 7d285de6..89a6e14d 100644 --- a/tests/test_graph_based_slam.py +++ b/tests/test_graph_based_slam.py @@ -19,6 +19,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_informed_rrt_star.py b/tests/test_informed_rrt_star.py index 1aab45ce..a19f2465 100644 --- a/tests/test_informed_rrt_star.py +++ b/tests/test_informed_rrt_star.py @@ -19,6 +19,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_lqr_rrt_star.py b/tests/test_lqr_rrt_star.py index 12964e1e..a9c2f5ee 100644 --- a/tests/test_lqr_rrt_star.py +++ b/tests/test_lqr_rrt_star.py @@ -19,6 +19,6 @@ class Test(TestCase): m.main(maxIter=5) -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_rrt.py b/tests/test_rrt.py index 25aa5810..b0c1504a 100644 --- a/tests/test_rrt.py +++ b/tests/test_rrt.py @@ -24,7 +24,7 @@ class Test(TestCase): m1.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() test.test2() diff --git a/tests/test_rrt_dubins.py b/tests/test_rrt_dubins.py index d55888a4..19983252 100644 --- a/tests/test_rrt_dubins.py +++ b/tests/test_rrt_dubins.py @@ -22,6 +22,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py index cd7a1801..8926a2b0 100644 --- a/tests/test_rrt_star.py +++ b/tests/test_rrt_star.py @@ -20,7 +20,7 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() print(aa) diff --git a/tests/test_rrt_star_dubins.py b/tests/test_rrt_star_dubins.py index f5983de6..8ee3d387 100644 --- a/tests/test_rrt_star_dubins.py +++ b/tests/test_rrt_star_dubins.py @@ -20,6 +20,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index c71cd172..4a1fa132 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/tests/test_rrt_star_reeds_shepp.py @@ -23,6 +23,6 @@ class Test(TestCase): m.main(maxIter=5) -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1() diff --git a/tests/test_state_lattice_planner.py b/tests/test_state_lattice_planner.py index 955209ae..9bf90952 100644 --- a/tests/test_state_lattice_planner.py +++ b/tests/test_state_lattice_planner.py @@ -25,6 +25,6 @@ class Test(TestCase): m.main() -if __name__ == '__main__': +if __name__ == '__main__': # pragma: no cover test = Test() test.test1()