From 692b7bd68411117204636f3534b4ea1eb661d872 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 23 Oct 2019 10:14:14 +0300 Subject: [PATCH 1/7] Fix wrong rotation direction - The obstacle rotation for the rectangle obstacle calculation is wrong. It should be positive instead of negative. --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c2a8674ae36..ebf254e1108 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -172,7 +172,7 @@ def calc_obstacle_cost(trajectory, ob, config): rot = np.transpose(rot, [2, 0, 1]) local_ob = ob[:, None] - trajectory[:, 0:2] local_ob = local_ob.reshape(-1, local_ob.shape[-1]) - local_ob = np.array([local_ob @ -x for x in rot]) + local_ob = np.array([local_ob @ x for x in rot]) local_ob = local_ob.reshape(-1, local_ob.shape[-1]) upper_check = local_ob[:, 0] <= config.robot_length / 2 right_check = local_ob[:, 1] <= config.robot_width / 2 From 9f4102378a75b99ec3e8c2f9c465015a5ed2d093 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 23 Oct 2019 21:46:33 +0300 Subject: [PATCH 2/7] change resampling calculation to match with the book --- Localization/particle_filter/particle_filter.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 6643116f5a9..8859b514283 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -141,9 +141,8 @@ def re_sampling(px, pw): N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number if N_eff < NTh: w_cum = np.cumsum(pw) - base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP - re_sample_id = base + np.random.rand(base.shape[0]) / NP - + base = np.arange(0.0, 1.0, 1/NP) + re_sample_id = base + np.random.uniform(0, 1/NP) indexes = [] ind = 0 for ip in range(NP): From 7a1784ad4798b4febd82a9a276fdae79a116a57b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Thu, 24 Oct 2019 19:43:22 +0300 Subject: [PATCH 3/7] condition check is moved condition check is moved from the inside of the resampling function to the outside of the function, this way the purpose of the function is much clear. Instead "resampling" every iteration, resample if the condition holds. --- .../particle_filter/particle_filter.py | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 8859b514283..0ebb5fc3d13 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -128,8 +128,9 @@ def pf_localization(px, pw, z, u): xEst = px.dot(pw.T) PEst = calc_covariance(xEst, px, pw) - px, pw = re_sampling(px, pw) - + N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number + if N_eff < NTh: + px, pw = re_sampling(px, pw) return xEst, PEst, px, pw @@ -138,20 +139,18 @@ def re_sampling(px, pw): low variance re-sampling """ - N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number - if N_eff < NTh: - w_cum = np.cumsum(pw) - base = np.arange(0.0, 1.0, 1/NP) - re_sample_id = base + np.random.uniform(0, 1/NP) - indexes = [] - ind = 0 - for ip in range(NP): - while re_sample_id[ip] > w_cum[ind]: - ind += 1 - indexes.append(ind) - - px = px[:, indexes] - pw = np.zeros((1, NP)) + 1.0 / NP # init weight + w_cum = np.cumsum(pw) + base = np.arange(0.0, 1.0, 1/NP) + re_sample_id = base + np.random.uniform(0, 1/NP) + indexes = [] + ind = 0 + for ip in range(NP): + while re_sample_id[ip] > w_cum[ind]: + ind += 1 + indexes.append(ind) + + px = px[:, indexes] + pw = np.zeros((1, NP)) + 1.0 / NP # init weight return px, pw From bf0652970abe99f42e9e175c55da0d8e5e38f540 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Fri, 25 Oct 2019 15:30:16 +0300 Subject: [PATCH 4/7] optimize nearest_neighbor_assosiation --- .../iterative_closest_point.py | 22 ++++++------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 73651d8ec5f..924619fdf1b 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -39,7 +39,7 @@ def ICP_matching(ppoints, cpoints): plt.plot(cpoints[0, :], cpoints[1, :], ".b") plt.plot(0.0, 0.0, "xr") plt.axis("equal") - plt.pause(1.0) + plt.pause(0.1) inds, error = nearest_neighbor_assosiation(ppoints, cpoints) Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) @@ -93,18 +93,10 @@ def nearest_neighbor_assosiation(ppoints, cpoints): error = sum(d) # calc index with nearest neighbor assosiation - inds = [] - for i in range(cpoints.shape[1]): - minid = -1 - mind = float("inf") - for ii in range(ppoints.shape[1]): - d = np.linalg.norm(ppoints[:, ii] - cpoints[:, i]) - - if mind >= d: - mind = d - minid = ii - - inds.append(minid) + d = np.linalg.norm( + np.repeat(cpoints, ppoints.shape[1], axis=1) - np.tile(ppoints, (1, + cpoints.shape[1])), axis=0) + inds = np.argmin(d.reshape(cpoints.shape[1], ppoints.shape[1]), axis=1) return inds, error @@ -130,7 +122,7 @@ def main(): print(__file__ + " start!!") # simulation parameters - nPoint = 10 + nPoint = 1000 fieldLength = 50.0 motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]] @@ -156,4 +148,4 @@ def main(): if __name__ == '__main__': - main() \ No newline at end of file + main() From d019e416ba051a08db153c3177ef97a715031ed8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Sat, 7 Dec 2019 14:30:18 +0300 Subject: [PATCH 5/7] exit on key --- AerialNavigation/drone_3d_trajectory_following/Quadrotor.py | 6 ++++-- .../rocket_powered_landing/rocket_powered_landing.py | 2 ++ .../arm_obstacle_navigation/arm_obstacle_navigation.py | 4 +++- .../arm_obstacle_navigation/arm_obstacle_navigation_2.py | 2 ++ ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py | 2 ++ .../two_joint_arm_to_point_control.py | 2 ++ Bipedal/bipedal_planner/bipedal_planner.py | 2 ++ .../ensemble_kalman_filter/ensemble_kalman_filter.py | 2 ++ .../extended_kalman_filter/extended_kalman_filter.py | 2 ++ Localization/histogram_filter/histogram_filter.py | 2 ++ Localization/particle_filter/particle_filter.py | 2 ++ .../unscented_kalman_filter/unscented_kalman_filter.py | 2 ++ Mapping/circle_fitting/circle_fitting.py | 2 ++ Mapping/gaussian_grid_map/gaussian_grid_map.py | 2 ++ Mapping/kmeans_clustering/kmeans_clustering.py | 2 ++ Mapping/raycasting_grid_map/raycasting_grid_map.py | 2 ++ Mapping/rectangle_fitting/rectangle_fitting.py | 2 ++ PathPlanning/AStar/a_star.py | 2 ++ PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py | 2 ++ PathPlanning/ClosedLoopRRTStar/pure_pursuit.py | 2 ++ PathPlanning/Dijkstra/dijkstra.py | 2 ++ PathPlanning/DubinsPath/dubins_path_planning.py | 2 ++ .../DynamicWindowApproach/dynamic_window_approach.py | 2 ++ PathPlanning/Eta3SplinePath/eta3_spline_path.py | 2 ++ .../FrenetOptimalTrajectory/frenet_optimal_trajectory.py | 2 ++ .../grid_based_sweep_coverage_path_planner.py | 4 ++++ PathPlanning/HybridAStar/a_star.py | 4 +++- PathPlanning/HybridAStar/hybrid_a_star.py | 2 ++ PathPlanning/InformedRRTStar/informed_rrt_star.py | 3 ++- PathPlanning/LQRPlanner/LQRplanner.py | 2 ++ PathPlanning/LQRRRTStar/lqr_rrt_star.py | 2 ++ .../PotentialFieldPlanning/potential_field_planning.py | 2 ++ PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py | 2 ++ .../quintic_polynomials_planner.py | 2 ++ PathPlanning/RRT/rrt.py | 2 ++ PathPlanning/RRTDubins/rrt_dubins.py | 2 ++ PathPlanning/RRTStarDubins/rrt_star_dubins.py | 2 ++ PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py | 2 ++ PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 2 ++ PathPlanning/VoronoiRoadMap/voronoi_road_map.py | 2 ++ PathTracking/cgmres_nmpc/cgmres_nmpc.py | 2 ++ .../lqr_speed_steer_control/lqr_speed_steer_control.py | 2 ++ PathTracking/lqr_steer_control/lqr_steer_control.py | 2 ++ .../model_predictive_speed_and_steer_control.py | 2 ++ PathTracking/move_to_pose/move_to_pose.py | 3 +++ PathTracking/pure_pursuit/pure_pursuit.py | 2 ++ PathTracking/rear_wheel_feedback/rear_wheel_feedback.py | 2 ++ PathTracking/stanley_controller/stanley_controller.py | 2 ++ SLAM/EKFSLAM/ekf_slam.py | 2 ++ SLAM/FastSLAM1/fast_slam1.py | 2 ++ SLAM/FastSLAM2/fast_slam2.py | 2 ++ SLAM/GraphBasedSLAM/graph_based_slam.py | 5 +++-- SLAM/iterative_closest_point/iterative_closest_point.py | 2 ++ 53 files changed, 114 insertions(+), 7 deletions(-) diff --git a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py index 9b38677cbea..89171da212c 100644 --- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py +++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py @@ -8,7 +8,6 @@ import numpy as np import matplotlib.pyplot as plt - class Quadrotor(): def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True): self.p1 = np.array([size / 2, 0, 0, 1]).T @@ -24,6 +23,9 @@ def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animat if self.show_animation: plt.ion() fig = plt.figure() + fig.canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + self.ax = fig.add_subplot(111, projection='3d') self.update_pose(x, y, z, roll, pitch, yaw) @@ -81,4 +83,4 @@ def plot(self): # pragma: no cover plt.ylim(-5, 5) self.ax.set_zlim(0, 10) - plt.pause(0.001) \ No newline at end of file + plt.pause(0.001) diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index 9f1e16fc1d5..c7ab998bec2 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -567,6 +567,8 @@ def plot_animation(X, U): # pragma: no cover fig = plt.figure() ax = fig.gca(projection='3d') + rig.canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) for k in range(K): plt.cla() diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py index 2bcc33d0f7f..b435e62d2b7 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py @@ -162,6 +162,8 @@ def astar_torus(grid, start_node, goal_node): for i in range(1, len(route)): grid[route[i]] = 6 plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None) plt.show() plt.pause(1e-2) @@ -262,4 +264,4 @@ def plot(self, obstacles=[]): # pragma: no cover if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py index b8c1ce89e91..5a21b806d30 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py @@ -193,6 +193,8 @@ def astar_torus(grid, start_node, goal_node): for i in range(1, len(route)): grid[route[i]] = 6 plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None) plt.show() plt.pause(1e-2) diff --git a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py index 8ed12c565be..576bd29a8fa 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py @@ -51,6 +51,8 @@ def update_points(self): def plot(self): # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) for i in range(self.n_links + 1): if i is not self.n_links: diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index e674ae8ac23..134583a95c3 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -114,6 +114,8 @@ def animation(): def main(): # pragma: no cover fig = plt.figure() fig.canvas.mpl_connect("button_press_event", click) + fig.canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) two_joint_arm() diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index 3cf2936ee44..afa68afb25b 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -111,6 +111,8 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False): if c > len(com_trajectory_for_plot): # set up plotter plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) ax.set_zlim(0, z_c * 2) ax.set_aspect('equal', 'datalim') diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 293509157b6..6ac6144f368 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -213,6 +213,8 @@ def main(): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) for i in range(len(z[:, 0])): plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k") diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index d2d7178ae6c..029dac4bf8e 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -191,6 +191,8 @@ def main(): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(hz[0, :], hz[1, :], ".g") plt.plot(hxTrue[0, :].flatten(), hxTrue[1, :].flatten(), "-b") diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index bdfbb13fa75..ac07c59fbd4 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -233,6 +233,8 @@ def main(): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) draw_heat_map(grid_map.data, mx, my) plt.plot(xTrue[0, :], xTrue[1, :], "xr") plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k") diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 0ebb5fc3d13..4b9abd78bd6 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -230,6 +230,8 @@ def main(): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) for i in range(len(z[:, 0])): plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k") diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 45d5b781328..0005d12fe73 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -240,6 +240,8 @@ def main(): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(hz[0, :], hz[1, :], ".g") plt.plot(np.array(hxTrue[0, :]).flatten(), np.array(hxTrue[1, :]).flatten(), "-b") diff --git a/Mapping/circle_fitting/circle_fitting.py b/Mapping/circle_fitting/circle_fitting.py index dae9413d27c..27ceac21f71 100644 --- a/Mapping/circle_fitting/circle_fitting.py +++ b/Mapping/circle_fitting/circle_fitting.py @@ -124,6 +124,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") plt.plot(0.0, 0.0, "*r") plot_circle(cx, cy, cr) diff --git a/Mapping/gaussian_grid_map/gaussian_grid_map.py b/Mapping/gaussian_grid_map/gaussian_grid_map.py index b520b639448..094b094fdd0 100644 --- a/Mapping/gaussian_grid_map/gaussian_grid_map.py +++ b/Mapping/gaussian_grid_map/gaussian_grid_map.py @@ -73,6 +73,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) plt.plot(ox, oy, "xr") plt.plot(0.0, 0.0, "ob") diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py index 8edfa11e3ab..7900e30457d 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -163,6 +163,8 @@ def main(): # for animation if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) inds = calc_association(cx, cy, clusters) for ic in inds: x, y = calc_labeled_points(ic, clusters) diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/raycasting_grid_map/raycasting_grid_map.py index 997687423e6..cae92a9dcbc 100644 --- a/Mapping/raycasting_grid_map/raycasting_grid_map.py +++ b/Mapping/raycasting_grid_map/raycasting_grid_map.py @@ -124,6 +124,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso) plt.plot(ox, oy, "xr") plt.plot(0.0, 0.0, "ob") diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index e40fc2e4c75..77c66db2509 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -242,6 +242,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") plt.plot(0.0, 0.0, "*r") v1.plot() diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 6b0392f515f..e4f7dd4d97b 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -79,6 +79,8 @@ def planning(self, sx, sy, gx, gy): if show_animation: # pragma: no cover plt.plot(self.calc_grid_position(current.x, self.minx), self.calc_grid_position(current.y, self.miny), "xc") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if len(closed_set.keys()) % 10 == 0: plt.pause(0.001) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 37ac75ada9b..46ff3caf5c1 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -534,6 +534,8 @@ def update_graph(self): def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, samples=None, start=None, end=None): plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) for rnd in samples: if rnd is not None: plt.plot(rnd[0], rnd[1], "^k") diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 04907279333..d078338d673 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -135,6 +135,8 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal): if target_ind % 1 == 0 and animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) 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") diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index bb73ed501a9..8e719f50e26 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -69,6 +69,8 @@ def planning(self, sx, sy, gx, gy): if show_animation: # pragma: no cover plt.plot(self.calc_position(current.x, self.minx), self.calc_position(current.y, self.miny), "xc") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if len(closedset.keys()) % 10 == 0: plt.pause(0.001) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 706b5a49af7..8690fbe8dbf 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -318,6 +318,8 @@ def test(): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(px, py, label="final course " + str(mode)) # plotting diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c8a4434958c..c2cc358b136 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -268,6 +268,8 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle): if show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g") plt.plot(x[0], x[1], "xr") plt.plot(goal[0], goal[1], "xb") diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py index 072b0bb46cc..51ac612c282 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -213,6 +213,8 @@ def test1(): if show_animation: # plot the path plt.plot(pos[0, :], pos[1, :]) + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(1.0) if show_animation: diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 5c756a9dfeb..1abda92646f 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -347,6 +347,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(tx, ty) plt.plot(ob[:, 0], ob[:, 1], "xk") plt.plot(path.x[1:], path.y[1:], "-or") diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index f22b058fa8c..049373ece2f 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -213,6 +213,8 @@ def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False): if grid_search_animation: fig, ax = plt.subplots() + fig.canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) while True: cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap) @@ -266,6 +268,8 @@ def planning_animation(ox, oy, reso): # pragma: no cover if do_animation: for ipx, ipy in zip(px, py): plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(ox, oy, "-xb") plt.plot(px, py, "-r") plt.plot(ipx, ipy, "or") diff --git a/PathPlanning/HybridAStar/a_star.py b/PathPlanning/HybridAStar/a_star.py index c1c82edd0db..918469f0d3f 100644 --- a/PathPlanning/HybridAStar/a_star.py +++ b/PathPlanning/HybridAStar/a_star.py @@ -78,6 +78,8 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): # show graph if show_animation: # pragma: no cover plt.plot(current.x * reso, current.y * reso, "xc") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if len(closedset.keys()) % 10 == 0: plt.pause(0.001) @@ -228,4 +230,4 @@ def main(): if __name__ == '__main__': show_animation = True - main() \ No newline at end of file + main() diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index e82003ed251..70ffcee8655 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -327,6 +327,8 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): if show_animation: # pragma: no cover plt.plot(current.xlist[-1], current.ylist[-1], "xc") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if len(closedList.keys()) % 10 == 0: plt.pause(0.001) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 175749183d8..ef15022acac 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -266,8 +266,9 @@ def get_final_course(self, lastIndex): return path def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None): - plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd[0], rnd[1], "^k") if cBest != float('inf'): diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/LQRplanner.py index 5a0b453f78f..d5cda0bc503 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -54,6 +54,8 @@ def lqr_planning(self, sx, sy, gx, gy, show_animation=True): # animation if show_animation: # pragma: no cover + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(sx, sy, "or") plt.plot(gx, gy, "ob") plt.plot(rx, ry, "-r") diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index 2e276b9f9b0..c1f45445ffa 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -102,6 +102,8 @@ def planning(self, animation=True, search_until_max_iter=True): def draw_graph(self, rnd=None): plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py index 1e918fb158d..642f1cbff93 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py @@ -98,6 +98,8 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr): if show_animation: draw_heatmap(pmap) + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(ix, iy, "*k") plt.plot(gix, giy, "*m") diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 351232d849e..3fef2d0f7a3 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -194,6 +194,8 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): # show graph if show_animation and len(closedset.keys()) % 2 == 0: + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(current.x, current.y, "xg") plt.pause(0.001) diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index ecef7d454cd..b8d533d4dc8 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -151,6 +151,8 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_ if show_animation: # pragma: no cover for i, _ in enumerate(time): plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.grid(True) plt.axis("equal") plot_arrow(sx, sy, syaw) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 3bcc622100c..9d0bc36272a 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -138,6 +138,8 @@ def get_random_node(self): def draw_graph(self, rnd=None): plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 339acd2c3a9..2a842aa42c2 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -106,6 +106,8 @@ def planning(self, animation=True, search_until_max_iter=True): def draw_graph(self, rnd=None): # pragma: no cover plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 4d7312933dc..d4601c0f12e 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -112,6 +112,8 @@ def planning(self, animation=True, search_until_max_iter=True): def draw_graph(self, rnd=None): plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index 5d78f98f318..d37ae906341 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -122,6 +122,8 @@ def try_goal_path(self, node): def draw_graph(self, rnd=None): plt.clf() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 54b5adbe6d2..b2f8d4a57dc 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -395,6 +395,8 @@ def test(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(px, py, label="final course " + str(mode)) # plotting diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 3ac02993b73..86371b20518 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -185,6 +185,8 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): # show graph if show_animation and len(closedset.keys()) % 2 == 0: # pragma: no cover plt.plot(current.x, current.y, "xg") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(0.001) if c_id == (len(road_map) - 1): diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py index 74a0f9cc570..1be91b2d207 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -546,6 +546,8 @@ def animation(plant, controller, dt): steer = math.atan2(controller.history_u_2[t] * WB / v, 1.0) plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(plant.history_x, plant.history_y, "-r", label="trajectory") plot_car(x, y, yaw, steer=steer) plt.axis("equal") diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index 6dec12bd30a..0e225296233 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -224,6 +224,8 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal): if target_ind % 1 == 0 and show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) 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") diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index f977cdc2a1f..9cc36f231f8 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -203,6 +203,8 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): if target_ind % 1 == 0 and show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) 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") diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index 0f5104e954d..8bff2e107da 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -429,6 +429,8 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) if ox is not None: plt.plot(ox, oy, "xr", label="MPC") plt.plot(cx, cy, "-r", label="course") diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 0814ef62eff..838b780f484 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -93,6 +93,9 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover plt.plot(x_traj, y_traj, 'b--') + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.xlim(0, 20) plt.ylim(0, 20) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 38a84affce4..f9624956c43 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -166,6 +166,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plot_arrow(state.x, state.y, state.yaw) plt.plot(cx, cy, "-r", label="course") plt.plot(x, y, "-b", label="trajectory") diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 285d9404e3c..155eeec2ce0 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -149,6 +149,8 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): if target_ind % 1 == 0 and show_animation: plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) 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") diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index addd5ad3a15..cbfa4c667e8 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -186,6 +186,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(cx, cy, ".r", label="course") plt.plot(x, y, "-b", label="trajectory") plt.plot(cx[target_idx], cy[target_idx], "xg", label="target") diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 27e50354591..33a9b1ddb1b 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -235,6 +235,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(xEst[0], xEst[1], ".r") diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index c16407e89f4..ef1282995bf 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -365,6 +365,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") for i in range(N_PARTICLE): diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index 73624dbf1f7..f8958b8c737 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -390,6 +390,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") for iz in range(len(z[:, 0])): diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index b40ad6494ee..e73e8d0d4ab 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -303,7 +303,8 @@ def main(): if show_animation: # pragma: no cover plt.cla() - + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(hxTrue[0, :].flatten(), @@ -319,4 +320,4 @@ def main(): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 3512ef976be..4a4d29eb5ed 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -36,6 +36,8 @@ def icp_matching(previous_points, current_points): if show_animation: # pragma: no cover plt.cla() + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(previous_points[0, :], previous_points[1, :], ".r") plt.plot(current_points[0, :], current_points[1, :], ".b") plt.plot(0.0, 0.0, "xr") From 25cffd70d21389b76760b44da2697cde35175472 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Sat, 14 Dec 2019 14:39:45 +0300 Subject: [PATCH 6/7] fix typo --- .../rocket_powered_landing/rocket_powered_landing.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index c7ab998bec2..51e09b4e3c2 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -567,7 +567,7 @@ def plot_animation(X, U): # pragma: no cover fig = plt.figure() ax = fig.gca(projection='3d') - rig.canvas.mpl_connect('key_release_event', + fig.canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) for k in range(K): From 9ca7d8f14808f535b5e698bc57e5ab163b3dae4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Sat, 14 Dec 2019 14:44:01 +0300 Subject: [PATCH 7/7] add comment for stopping the simulation --- AerialNavigation/drone_3d_trajectory_following/Quadrotor.py | 1 + .../rocket_powered_landing/rocket_powered_landing.py | 1 + .../arm_obstacle_navigation/arm_obstacle_navigation.py | 1 + .../arm_obstacle_navigation/arm_obstacle_navigation_2.py | 1 + ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py | 1 + .../two_joint_arm_to_point_control.py | 1 + Bipedal/bipedal_planner/bipedal_planner.py | 1 + Localization/ensemble_kalman_filter/ensemble_kalman_filter.py | 1 + Localization/extended_kalman_filter/extended_kalman_filter.py | 1 + Localization/histogram_filter/histogram_filter.py | 1 + Localization/particle_filter/particle_filter.py | 1 + Localization/unscented_kalman_filter/unscented_kalman_filter.py | 1 + Mapping/circle_fitting/circle_fitting.py | 1 + Mapping/gaussian_grid_map/gaussian_grid_map.py | 1 + Mapping/kmeans_clustering/kmeans_clustering.py | 1 + Mapping/raycasting_grid_map/raycasting_grid_map.py | 1 + Mapping/rectangle_fitting/rectangle_fitting.py | 1 + PathPlanning/AStar/a_star.py | 1 + PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py | 1 + PathPlanning/ClosedLoopRRTStar/pure_pursuit.py | 1 + PathPlanning/Dijkstra/dijkstra.py | 1 + PathPlanning/DubinsPath/dubins_path_planning.py | 1 + PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 1 + PathPlanning/Eta3SplinePath/eta3_spline_path.py | 1 + .../FrenetOptimalTrajectory/frenet_optimal_trajectory.py | 1 + .../GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py | 2 ++ PathPlanning/HybridAStar/a_star.py | 1 + PathPlanning/HybridAStar/hybrid_a_star.py | 1 + PathPlanning/InformedRRTStar/informed_rrt_star.py | 1 + PathPlanning/LQRPlanner/LQRplanner.py | 1 + PathPlanning/LQRRRTStar/lqr_rrt_star.py | 1 + PathPlanning/PotentialFieldPlanning/potential_field_planning.py | 1 + PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py | 1 + .../QuinticPolynomialsPlanner/quintic_polynomials_planner.py | 1 + PathPlanning/RRT/rrt.py | 1 + PathPlanning/RRTDubins/rrt_dubins.py | 1 + PathPlanning/RRTStarDubins/rrt_star_dubins.py | 1 + PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py | 1 + PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 1 + PathPlanning/VoronoiRoadMap/voronoi_road_map.py | 1 + PathTracking/cgmres_nmpc/cgmres_nmpc.py | 1 + PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py | 1 + PathTracking/lqr_steer_control/lqr_steer_control.py | 1 + .../model_predictive_speed_and_steer_control.py | 1 + PathTracking/move_to_pose/move_to_pose.py | 1 + PathTracking/pure_pursuit/pure_pursuit.py | 1 + PathTracking/rear_wheel_feedback/rear_wheel_feedback.py | 1 + PathTracking/stanley_controller/stanley_controller.py | 1 + SLAM/EKFSLAM/ekf_slam.py | 1 + SLAM/FastSLAM1/fast_slam1.py | 1 + SLAM/FastSLAM2/fast_slam2.py | 1 + SLAM/GraphBasedSLAM/graph_based_slam.py | 1 + SLAM/iterative_closest_point/iterative_closest_point.py | 1 + 53 files changed, 54 insertions(+) diff --git a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py index 89171da212c..413a8625a5d 100644 --- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py +++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py @@ -23,6 +23,7 @@ def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animat if self.show_animation: plt.ion() fig = plt.figure() + # for stopping simulation with the esc key. fig.canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index 51e09b4e3c2..ee0f5e26a54 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -567,6 +567,7 @@ def plot_animation(X, U): # pragma: no cover fig = plt.figure() ax = fig.gca(projection='3d') + # for stopping simulation with the esc key. fig.canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py index b435e62d2b7..f3bd25577c1 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py @@ -162,6 +162,7 @@ def astar_torus(grid, start_node, goal_node): for i in range(1, len(route)): grid[route[i]] = 6 plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None) diff --git a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py index 5a21b806d30..429cd4d2119 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py @@ -193,6 +193,7 @@ def astar_torus(grid, start_node, goal_node): for i in range(1, len(route)): grid[route[i]] = 6 plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.imshow(grid, cmap=cmap, norm=norm, interpolation=None) diff --git a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py index 576bd29a8fa..854ade90383 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py @@ -51,6 +51,7 @@ def update_points(self): def plot(self): # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index 134583a95c3..60a7cd5f0f8 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -114,6 +114,7 @@ def animation(): def main(): # pragma: no cover fig = plt.figure() fig.canvas.mpl_connect("button_press_event", click) + # for stopping simulation with the esc key. fig.canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) two_joint_arm() diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index afa68afb25b..6502ce5bed1 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -111,6 +111,7 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False): if c > len(com_trajectory_for_plot): # set up plotter plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) ax.set_zlim(0, z_c * 2) diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 6ac6144f368..244cfccd9c0 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -213,6 +213,7 @@ def main(): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index 029dac4bf8e..2b057e21de1 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -191,6 +191,7 @@ def main(): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(hz[0, :], hz[1, :], ".g") diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index ac07c59fbd4..79302c8abb4 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -233,6 +233,7 @@ def main(): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) draw_heat_map(grid_map.data, mx, my) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 4b9abd78bd6..3e901f8da02 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -230,6 +230,7 @@ def main(): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 0005d12fe73..7bf279ced07 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -240,6 +240,7 @@ def main(): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(hz[0, :], hz[1, :], ".g") diff --git a/Mapping/circle_fitting/circle_fitting.py b/Mapping/circle_fitting/circle_fitting.py index 27ceac21f71..c331d567969 100644 --- a/Mapping/circle_fitting/circle_fitting.py +++ b/Mapping/circle_fitting/circle_fitting.py @@ -124,6 +124,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") diff --git a/Mapping/gaussian_grid_map/gaussian_grid_map.py b/Mapping/gaussian_grid_map/gaussian_grid_map.py index 094b094fdd0..7893d6a5ee6 100644 --- a/Mapping/gaussian_grid_map/gaussian_grid_map.py +++ b/Mapping/gaussian_grid_map/gaussian_grid_map.py @@ -73,6 +73,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py index 5345813096b..d658b84f48f 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -133,6 +133,7 @@ def main(): # for animation if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) clusters.plot_cluster() diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/raycasting_grid_map/raycasting_grid_map.py index cae92a9dcbc..372e31eccde 100644 --- a/Mapping/raycasting_grid_map/raycasting_grid_map.py +++ b/Mapping/raycasting_grid_map/raycasting_grid_map.py @@ -124,6 +124,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso) diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 77c66db2509..1c4d4122070 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -242,6 +242,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index e4f7dd4d97b..d8ef09de850 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -79,6 +79,7 @@ def planning(self, sx, sy, gx, gy): if show_animation: # pragma: no cover plt.plot(self.calc_grid_position(current.x, self.minx), self.calc_grid_position(current.y, self.miny), "xc") + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if len(closed_set.keys()) % 10 == 0: diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 46ff3caf5c1..e84f65d6172 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -534,6 +534,7 @@ def update_graph(self): def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, samples=None, start=None, end=None): plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) for rnd in samples: diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index d078338d673..5833f6e91ef 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -135,6 +135,7 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal): if target_ind % 1 == 0 and animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(cx, cy, "-r", label="course") diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index 8e719f50e26..a8f64413286 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -69,6 +69,7 @@ def planning(self, sx, sy, gx, gy): if show_animation: # pragma: no cover plt.plot(self.calc_position(current.x, self.minx), self.calc_position(current.y, self.miny), "xc") + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if len(closedset.keys()) % 10 == 0: diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 8690fbe8dbf..de5db2a5aa8 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -318,6 +318,7 @@ def test(): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(px, py, label="final course " + str(mode)) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c2cc358b136..b0d020302fc 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -268,6 +268,7 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle): if show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g") diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py index 51ac612c282..414f6b4534d 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -213,6 +213,7 @@ def test1(): if show_animation: # plot the path plt.plot(pos[0, :], pos[1, :]) + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(1.0) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 1abda92646f..861260e9c9c 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -347,6 +347,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(tx, ty) diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index 049373ece2f..5c39a1ee810 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -213,6 +213,7 @@ def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False): if grid_search_animation: fig, ax = plt.subplots() + # for stopping simulation with the esc key. fig.canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) @@ -268,6 +269,7 @@ def planning_animation(ox, oy, reso): # pragma: no cover if do_animation: for ipx, ipy in zip(px, py): plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(ox, oy, "-xb") diff --git a/PathPlanning/HybridAStar/a_star.py b/PathPlanning/HybridAStar/a_star.py index 918469f0d3f..a27293288cf 100644 --- a/PathPlanning/HybridAStar/a_star.py +++ b/PathPlanning/HybridAStar/a_star.py @@ -78,6 +78,7 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): # show graph if show_animation: # pragma: no cover plt.plot(current.x * reso, current.y * reso, "xc") + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if len(closedset.keys()) % 10 == 0: diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 70ffcee8655..8a87be85a32 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -327,6 +327,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): if show_animation: # pragma: no cover plt.plot(current.xlist[-1], current.ylist[-1], "xc") + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if len(closedList.keys()) % 10 == 0: diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index ef15022acac..a77b1cea742 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -267,6 +267,7 @@ def get_final_course(self, lastIndex): def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None): plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/LQRplanner.py index d5cda0bc503..ba01526a2c4 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -54,6 +54,7 @@ def lqr_planning(self, sx, sy, gx, gy, show_animation=True): # animation if show_animation: # pragma: no cover + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(sx, sy, "or") diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index c1f45445ffa..177131ac965 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -102,6 +102,7 @@ def planning(self, animation=True, search_until_max_iter=True): def draw_graph(self, rnd=None): plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py index 642f1cbff93..31904fa45d9 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py @@ -98,6 +98,7 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr): if show_animation: draw_heatmap(pmap) + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(ix, iy, "*k") diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 3fef2d0f7a3..0a8ea037d1d 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -194,6 +194,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): # show graph if show_animation and len(closedset.keys()) % 2 == 0: + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(current.x, current.y, "xg") diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index b8d533d4dc8..406dbc1ee2f 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -151,6 +151,7 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_ if show_animation: # pragma: no cover for i, _ in enumerate(time): plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.grid(True) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 9d0bc36272a..f8e99082874 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -138,6 +138,7 @@ def get_random_node(self): def draw_graph(self, rnd=None): plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 2a842aa42c2..4599904cc1d 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -106,6 +106,7 @@ def planning(self, animation=True, search_until_max_iter=True): def draw_graph(self, rnd=None): # pragma: no cover plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index d4601c0f12e..65fac54c788 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -112,6 +112,7 @@ def planning(self, animation=True, search_until_max_iter=True): def draw_graph(self, rnd=None): plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index d37ae906341..6ea66dc729d 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -122,6 +122,7 @@ def try_goal_path(self, node): def draw_graph(self, rnd=None): plt.clf() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index b2f8d4a57dc..fbf94957900 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -395,6 +395,7 @@ def test(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(px, py, label="final course " + str(mode)) diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 86371b20518..7517669628e 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -185,6 +185,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): # show graph if show_animation and len(closedset.keys()) % 2 == 0: # pragma: no cover plt.plot(current.x, current.y, "xg") + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(0.001) diff --git a/PathTracking/cgmres_nmpc/cgmres_nmpc.py b/PathTracking/cgmres_nmpc/cgmres_nmpc.py index 1be91b2d207..98a12ab10cb 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -546,6 +546,7 @@ def animation(plant, controller, dt): steer = math.atan2(controller.history_u_2[t] * WB / v, 1.0) plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(plant.history_x, plant.history_y, "-r", label="trajectory") diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index 0e225296233..77409c7f423 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -224,6 +224,7 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal): if target_ind % 1 == 0 and show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(cx, cy, "-r", label="course") diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index 9cc36f231f8..fe440a17349 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -203,6 +203,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): if target_ind % 1 == 0 and show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(cx, cy, "-r", label="course") diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index 8bff2e107da..85aea2ed1b1 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -429,6 +429,7 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if ox is not None: diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 838b780f484..685df1979dc 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -93,6 +93,7 @@ def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover plt.plot(x_traj, y_traj, 'b--') + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index f9624956c43..473feea5da1 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -166,6 +166,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plot_arrow(state.x, state.y, state.yaw) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 155eeec2ce0..c6a3501368e 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -149,6 +149,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): if target_ind % 1 == 0 and show_animation: plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(cx, cy, "-r", label="course") diff --git a/PathTracking/stanley_controller/stanley_controller.py b/PathTracking/stanley_controller/stanley_controller.py index cbfa4c667e8..604ed93f015 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -186,6 +186,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(cx, cy, ".r", label="course") diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index b6698be2282..4bcafef2fa6 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -235,6 +235,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index ef1282995bf..892010c53d4 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -365,6 +365,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index f8958b8c737..25712fc0cbc 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -390,6 +390,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index e73e8d0d4ab..2a5acfab1f7 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -303,6 +303,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(RFID[:, 0], RFID[:, 1], "*k") diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 4a4d29eb5ed..0b2802603c6 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -36,6 +36,7 @@ def icp_matching(previous_points, current_points): if show_animation: # pragma: no cover plt.cla() + # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(previous_points[0, :], previous_points[1, :], ".r")