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 01/19] 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 c2a8674a..ebf254e1 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 02/19] 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 6643116f..8859b514 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 03/19] 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 | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 8859b514..0ebb5fc3 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) + 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 + 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 04/19] 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 73651d8e..924619fd 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 05/19] 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 9b38677c..89171da2 100644 --- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py +++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py @@ -8,7 +8,6 @@ from math import cos, sin 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 @@ class Quadrotor(): 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 @@ class Quadrotor(): 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 9f1e16fc..c7ab998b 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 2bcc33d0..b435e62d 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 @@ class NLinkArm(object): 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 b8c1ce89..5a21b806 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 8ed12c56..576bd29a 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 @@ class NLinkArm(object): 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 e674ae8a..134583a9 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 3cf2936e..afa68afb 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -111,6 +111,8 @@ class BipedalPlanner(object): 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 29350915..6ac6144f 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 d2d7178a..029dac4b 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 bdfbb13f..ac07c59f 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 0ebb5fc3..4b9abd78 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 45d5b781..0005d12f 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 dae9413d..27ceac21 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 b520b639..094b094f 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 8edfa11e..7900e304 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 99768742..cae92a9d 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 e40fc2e4..77c66db2 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 6b0392f5..e4f7dd4d 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -79,6 +79,8 @@ class AStarPlanner: 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 37ac75ad..46ff3caf 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -534,6 +534,8 @@ class BITStar(object): 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 04907279..d078338d 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 bb73ed50..8e719f50 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -69,6 +69,8 @@ class Dijkstra: 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 706b5a49..8690fbe8 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 c8a44349..c2cc358b 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 072b0bb4..51ac612c 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 5c756a9d..1abda926 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 f22b058f..049373ec 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 c1c82edd..918469f0 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 e82003ed..70ffcee8 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 17574918..ef15022a 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -266,8 +266,9 @@ class InformedRRTStar: 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 5a0b453f..d5cda0bc 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -54,6 +54,8 @@ class LQRPlanner: # 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 2e276b9f..c1f45445 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -102,6 +102,8 @@ class LQRRRTStar(RRTStar): 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 1e918fb1..642f1cbf 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 351232d8..3fef2d0f 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 ecef7d45..b8d533d4 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 3bcc6221..9d0bc362 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -138,6 +138,8 @@ class RRT: 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 339acd2c..2a842aa4 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -106,6 +106,8 @@ class RRTDubins(RRT): 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 4d731293..d4601c0f 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -112,6 +112,8 @@ class RRTStarDubins(RRTStar): 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 5d78f98f..d37ae906 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -122,6 +122,8 @@ class RRTStarReedsShepp(RRTStar): 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 54b5adbe..b2f8d4a5 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 3ac02993..86371b20 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 74a0f9cc..1be91b2d 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 6dec12bd..0e225296 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 f977cdc2..9cc36f23 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 0f5104e9..8bff2e10 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 0814ef62..838b780f 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 38a84aff..f9624956 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 285d9404..155eeec2 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 addd5ad3..cbfa4c66 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 27e50354..33a9b1dd 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 c16407e8..ef128299 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 73624dbf..f8958b8c 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 b40ad649..e73e8d0d 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 3512ef97..4a4d29eb 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 06/19] 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 c7ab998b..51e09b4e 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 07/19] 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 89171da2..413a8625 100644 --- a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py +++ b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py @@ -23,6 +23,7 @@ class Quadrotor(): 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 51e09b4e..ee0f5e26 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 b435e62d..f3bd2557 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 5a21b806..429cd4d2 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 576bd29a..854ade90 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 @@ class NLinkArm(object): 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 134583a9..60a7cd5f 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 afa68afb..6502ce5b 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -111,6 +111,7 @@ class BipedalPlanner(object): 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 6ac6144f..244cfccd 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 029dac4b..2b057e21 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 ac07c59f..79302c8a 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 4b9abd78..3e901f8d 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 0005d12f..7bf279ce 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 27ceac21..c331d567 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 094b094f..7893d6a5 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 53458130..d658b84f 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 cae92a9d..372e31ec 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 77c66db2..1c4d4122 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 e4f7dd4d..d8ef09de 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -79,6 +79,7 @@ class AStarPlanner: 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 46ff3caf..e84f65d6 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -534,6 +534,7 @@ class BITStar(object): 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 d078338d..5833f6e9 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 8e719f50..a8f64413 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -69,6 +69,7 @@ class Dijkstra: 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 8690fbe8..de5db2a5 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 c2cc358b..b0d02030 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 51ac612c..414f6b45 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 1abda926..861260e9 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 049373ec..5c39a1ee 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 918469f0..a2729328 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 70ffcee8..8a87be85 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 ef15022a..a77b1cea 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -267,6 +267,7 @@ class InformedRRTStar: 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 d5cda0bc..ba01526a 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -54,6 +54,7 @@ class LQRPlanner: # 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 c1f45445..177131ac 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -102,6 +102,7 @@ class LQRRRTStar(RRTStar): 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 642f1cbf..31904fa4 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 3fef2d0f..0a8ea037 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 b8d533d4..406dbc1e 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 9d0bc362..f8e99082 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -138,6 +138,7 @@ class RRT: 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 2a842aa4..4599904c 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -106,6 +106,7 @@ class RRTDubins(RRT): 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 d4601c0f..65fac54c 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -112,6 +112,7 @@ class RRTStarDubins(RRTStar): 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 d37ae906..6ea66dc7 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -122,6 +122,7 @@ class RRTStarReedsShepp(RRTStar): 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 b2f8d4a5..fbf94957 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 86371b20..75176696 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 1be91b2d..98a12ab1 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 0e225296..77409c7f 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 9cc36f23..fe440a17 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 8bff2e10..85aea2ed 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 838b780f..685df197 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 f9624956..473feea5 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 155eeec2..c6a35013 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 cbfa4c66..604ed93f 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 b6698be2..4bcafef2 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 ef128299..892010c5 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 f8958b8c..25712fc0 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 e73e8d0d..2a5acfab 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 4a4d29eb..0b280260 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") From c09e20bc45b0c4408878b2d02cf17cc9d333dd16 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 14 Dec 2019 21:41:04 +0900 Subject: [PATCH 08/19] Update users_comments.md --- users_comments.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/users_comments.md b/users_comments.md index 2cfd7a4a..792f3a4d 100644 --- a/users_comments.md +++ b/users_comments.md @@ -404,6 +404,9 @@ URL: https://www.semanticscholar.org/paper/A-Review-of-Robot-Rescue-Simulation-P 7. Ghosh, Ritwika, et al. "CyPhyHouse: A Programming, Simulation, and Deployment Toolchain for Heterogeneous Distributed Coordination." arXiv preprint arXiv:1910.01557 (2019). URL: https://arxiv.org/abs/1910.01557 +8. Hahn, Carsten, et al. "Dynamic Path Planning with Stable Growing Neural Gas." (2019). +URL: https://pdfs.semanticscholar.org/5c06/f3cb9542a51e1bf1a32523c1bc7fea6cecc5.pdf + # Others - Autonomous Vehicle Readings https://richardkelley.io/readings From ca49613b53d8e45b169a3585eb7d0c8bf5057e64 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 23 Dec 2019 20:44:41 +0900 Subject: [PATCH 09/19] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index e740049b..f48e9c0e 100644 --- a/README.md +++ b/README.md @@ -605,5 +605,8 @@ This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/ - [Ryohei Sasaki](https://github.com/rsasaki0109) +- [Göktuğ Karakaşlı](https://github.com/goktug97) + + From 6132629a70b9b47ec9a00dcf338650e22368d0f6 Mon Sep 17 00:00:00 2001 From: YILUN CHEN Date: Mon, 30 Dec 2019 17:52:05 +0800 Subject: [PATCH 10/19] fix environment dependencies --- environment.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/environment.yml b/environment.yml index 28ff9f1b..6866ed93 100644 --- a/environment.yml +++ b/environment.yml @@ -1,9 +1,11 @@ name: python_robotics dependencies: - python +- pip - matplotlib - scipy - numpy==1.15 - pandas +- coverage - pip: - cvxpy From 164f82c0dc022e71625b53f8e899e7b361a50c17 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 3 Jan 2020 08:47:18 +0900 Subject: [PATCH 11/19] Delete greetings.yml --- .github/workflows/greetings.yml | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 .github/workflows/greetings.yml diff --git a/.github/workflows/greetings.yml b/.github/workflows/greetings.yml deleted file mode 100644 index 3da4a694..00000000 --- a/.github/workflows/greetings.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: Greetings - -on: [pull_request, issues] - -jobs: - greeting: - runs-on: ubuntu-latest - steps: - - uses: actions/first-interaction@v1 - with: - repo-token: ${{ secrets.GITHUB_TOKEN }} - issue-message: 'Message that will be displayed on users'' This is first issue for you on this project' - pr-message: 'Message that will be displayed on users'' This is first pr for you on this project' From 0c708766e13a167e5afe4a96ea6d4527c74a3e56 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Fri, 3 Jan 2020 14:01:47 +0100 Subject: [PATCH 12/19] :pencil: Removed unused class attribute in quintic_polynomials_planner.py --- .../quintic_polynomials_planner.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index 406dbc1e..e7c744d1 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -26,14 +26,7 @@ class QuinticPolynomial: def __init__(self, xs, vxs, axs, xe, vxe, axe, T): - # calc coefficient of quinic polynomial - self.xs = xs - self.vxs = vxs - self.axs = axs - self.xe = xe - self.vxe = vxe - self.axe = axe - + # calc coefficient of quintic polynomial self.a0 = xs self.a1 = vxs self.a2 = axs / 2.0 From 48cdd0817365ac10fc4031a39110dabf06cf4b75 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Fri, 3 Jan 2020 21:53:40 +0100 Subject: [PATCH 13/19] :pencil: Removed unused attribute in classes in frenet_optimal_trajectory.py --- .../frenet_optimal_trajectory.py | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 25fdd7db..1b62ea01 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -49,13 +49,6 @@ class quintic_polynomial: def __init__(self, xs, vxs, axs, xe, vxe, axe, T): # calc coefficient of quintic polynomial - self.xs = xs - self.vxs = vxs - self.axs = axs - self.xe = xe - self.vxe = vxe - self.axe = axe - self.a0 = xs self.a1 = vxs self.a2 = axs / 2.0 @@ -99,12 +92,7 @@ class quartic_polynomial: def __init__(self, xs, vxs, axs, vxe, axe, T): - # calc coefficient of quintic polynomial - self.xs = xs - self.vxs = vxs - self.axs = axs - self.vxe = vxe - self.axe = axe + # calc coefficient of quartic polynomial self.a0 = xs self.a1 = vxs @@ -184,7 +172,7 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t] fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t] - # Loongitudinal motion planning (Velocity keeping) + # Longitudinal motion planning (Velocity keeping) for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S): tfp = copy.deepcopy(fp) lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti) From 1ac858b075b5db19ef4c6130ecd426d42a23eb77 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 4 Jan 2020 09:43:21 +0900 Subject: [PATCH 14/19] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f48e9c0e..ec428127 100644 --- a/README.md +++ b/README.md @@ -607,6 +607,6 @@ This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/ - [Göktuğ Karakaşlı](https://github.com/goktug97) - +- [Guillaume Jacquenot](https://github.com/Gjacquenot) From 811bdc1d49ac5d04ebd05c18de906c0b3c7243dc Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Fri, 3 Jan 2020 23:13:36 +0100 Subject: [PATCH 15/19] :hammer: Refactored pure_pursuit.py - Added new method to state class - Removed global variable old_nearest_point_index - Created a class States that store the results --- PathTracking/pure_pursuit/pure_pursuit.py | 163 ++++++++++++---------- 1 file changed, 86 insertions(+), 77 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index dfe8a8e1..49e32677 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -17,7 +17,6 @@ dt = 0.1 # [s] L = 2.9 # [m] wheel base of vehicle -old_nearest_point_index = None show_animation = True @@ -31,17 +30,37 @@ class State: self.rear_x = self.x - ((L / 2) * math.cos(self.yaw)) self.rear_y = self.y - ((L / 2) * math.sin(self.yaw)) + def update(self, a, delta): -def update(state, a, delta): + self.x += self.v * math.cos(self.yaw) * dt + self.y += self.v * math.sin(self.yaw) * dt + self.yaw += self.v / L * math.tan(delta) * dt + self.v += a * dt + self.rear_x = self.x - ((L / 2) * math.cos(self.yaw)) + self.rear_y = self.y - ((L / 2) * math.sin(self.yaw)) - state.x = state.x + state.v * math.cos(state.yaw) * dt - state.y = state.y + state.v * math.sin(state.yaw) * dt - state.yaw = state.yaw + state.v / L * math.tan(delta) * dt - state.v = state.v + a * dt - state.rear_x = state.x - ((L / 2) * math.cos(state.yaw)) - state.rear_y = state.y - ((L / 2) * math.sin(state.yaw)) + def calc_distance(self, point_x, point_y): - return state + dx = self.rear_x - point_x + dy = self.rear_y - point_y + return math.hypot(dx, dy) + + +class States: + + def __init__(self): + self.x = [] + self.y = [] + self.yaw = [] + self.v = [] + self.t = [] + + def append(self, t , state): + self.x.append(state.x) + self.y.append(state.y) + self.yaw.append(state.yaw) + self.v.append(state.v) + self.t.append(t) def PIDControl(target, current): @@ -50,20 +69,59 @@ def PIDControl(target, current): return a -def pure_pursuit_control(state, cx, cy, pind): +class Trajectory: + def __init__(self, cx, cy): + self.cx = cx + self.cy = cy + self.old_nearest_point_index = None - ind = calc_target_index(state, cx, cy) + def __call__(self, state): + if self.old_nearest_point_index is None: + # search nearest point index + dx = [state.rear_x - icx for icx in self.cx] + dy = [state.rear_y - icy for icy in self.cy] + d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] + ind = d.index(min(d)) + self.old_nearest_point_index = ind + else: + ind = self.old_nearest_point_index + distance_this_index = state.calc_distance(self.cx[ind], self.cy[ind]) + while True: + ind = ind + 1 if (ind + 1) < len(self.cx) else ind + distance_next_index = state.calc_distance(self.cx[ind], self.cy[ind]) + if distance_this_index < distance_next_index: + break + distance_this_index = distance_next_index + self.old_nearest_point_index = ind + + L = 0.0 + + Lf = k * state.v + Lfc + + # search look ahead target point index + while Lf > L and (ind + 1) < len(self.cx): + dx = self.cx[ind] - state.rear_x + dy = self.cy[ind] - state.rear_y + L = math.hypot(dx, dy) + ind += 1 + + return ind + + +def pure_pursuit_control(state, trajectory, pind): + + ind = trajectory(state) if pind >= ind: ind = pind - if ind < len(cx): - tx = cx[ind] - ty = cy[ind] + if ind < len(trajectory.cx): + tx = trajectory.cx[ind] + ty = trajectory.cy[ind] else: - tx = cx[-1] - ty = cy[-1] - ind = len(cx) - 1 + tx = trajectory.cx[-1] + ty = trajectory.cy[-1] + ind = len(trajectory.cx) - 1 alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw @@ -73,48 +131,6 @@ def pure_pursuit_control(state, cx, cy, pind): return delta, ind -def calc_distance(state, point_x, point_y): - - dx = state.rear_x - point_x - dy = state.rear_y - point_y - return math.hypot(dx, dy) - - -def calc_target_index(state, cx, cy): - - global old_nearest_point_index - - if old_nearest_point_index is None: - # search nearest point index - dx = [state.rear_x - icx for icx in cx] - dy = [state.rear_y - icy for icy in cy] - d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] - ind = d.index(min(d)) - old_nearest_point_index = ind - else: - ind = old_nearest_point_index - distance_this_index = calc_distance(state, cx[ind], cy[ind]) - while True: - ind = ind + 1 if (ind + 1) < len(cx) else ind - distance_next_index = calc_distance(state, cx[ind], cy[ind]) - if distance_this_index < distance_next_index: - break - distance_this_index = distance_next_index - old_nearest_point_index = ind - - L = 0.0 - - Lf = k * state.v + Lfc - - # search look ahead target point index - while Lf > L and (ind + 1) < len(cx): - dx = cx[ind] - state.rear_x - dy = cy[ind] - state.rear_y - L = math.hypot(dx, dy) - ind += 1 - - return ind - def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): """ @@ -144,25 +160,18 @@ def main(): lastIndex = len(cx) - 1 time = 0.0 - x = [state.x] - y = [state.y] - yaw = [state.yaw] - v = [state.v] - t = [0.0] - target_ind = calc_target_index(state, cx, cy) + states = States() + states.append(time, state) + trajectory = Trajectory(cx, cy) + target_ind = trajectory(state) while T >= time and lastIndex > target_ind: ai = PIDControl(target_speed, state.v) - di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) - state = update(state, ai, di) + di, target_ind = pure_pursuit_control(state, trajectory, target_ind) + state.update(ai, di) - time = time + dt - - x.append(state.x) - y.append(state.y) - yaw.append(state.yaw) - v.append(state.v) - t.append(time) + time += dt + states.append(time, state) if show_animation: # pragma: no cover plt.cla() @@ -171,7 +180,7 @@ def main(): 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") + plt.plot(states.x, states.y, "-b", label="trajectory") plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") plt.axis("equal") plt.grid(True) @@ -184,7 +193,7 @@ def main(): if show_animation: # pragma: no cover plt.cla() plt.plot(cx, cy, ".r", label="course") - plt.plot(x, y, "-b", label="trajectory") + plt.plot(states.x, states.y, "-b", label="trajectory") plt.legend() plt.xlabel("x[m]") plt.ylabel("y[m]") @@ -192,7 +201,7 @@ def main(): plt.grid(True) plt.subplots(1) - plt.plot(t, [iv * 3.6 for iv in v], "-r") + plt.plot(states.t, [iv * 3.6 for iv in states.v], "-r") plt.xlabel("Time[s]") plt.ylabel("Speed[km/h]") plt.grid(True) From f81140e65bcf4648c8aae4bdf14e48080bfbd706 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 4 Jan 2020 09:45:05 +0100 Subject: [PATCH 16/19] :pencil: Renamed method name __call__ with search_target_index in Trajectory class Thanks to @AtsushiSakai remarks https://github.com/AtsushiSakai/PythonRobotics/pull/269#discussion_r363005681 --- PathTracking/pure_pursuit/pure_pursuit.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 49e32677..d7cf8fe9 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -75,7 +75,7 @@ class Trajectory: self.cy = cy self.old_nearest_point_index = None - def __call__(self, state): + def search_target_index(self, state): if self.old_nearest_point_index is None: # search nearest point index dx = [state.rear_x - icx for icx in self.cx] @@ -110,7 +110,7 @@ class Trajectory: def pure_pursuit_control(state, trajectory, pind): - ind = trajectory(state) + ind = trajectory.search_target_index(state) if pind >= ind: ind = pind @@ -163,7 +163,7 @@ def main(): states = States() states.append(time, state) trajectory = Trajectory(cx, cy) - target_ind = trajectory(state) + target_ind = trajectory.search_target_index(state) while T >= time and lastIndex > target_ind: ai = PIDControl(target_speed, state.v) From bfc874492390a803e803992324777376a6b12e8c Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 4 Jan 2020 09:50:41 +0100 Subject: [PATCH 17/19] :hammer: Refcatored modification in pure_pursuit.py Thanks to @AtsushiSakai remark https://github.com/AtsushiSakai/PythonRobotics/pull/269#discussion_r363005775 --- PathTracking/pure_pursuit/pure_pursuit.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index d7cf8fe9..4e8b1bed 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -80,8 +80,8 @@ class Trajectory: # search nearest point index dx = [state.rear_x - icx for icx in self.cx] dy = [state.rear_y - icy for icy in self.cy] - d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] - ind = d.index(min(d)) + d = np.hypot(dx, dy) + ind = np.argmin(d) self.old_nearest_point_index = ind else: ind = self.old_nearest_point_index @@ -100,9 +100,7 @@ class Trajectory: # search look ahead target point index while Lf > L and (ind + 1) < len(self.cx): - dx = self.cx[ind] - state.rear_x - dy = self.cy[ind] - state.rear_y - L = math.hypot(dx, dy) + L = state.calc_distance(self.cx[ind], self.cy[ind]) ind += 1 return ind From 3bdf2555345b93c89068549fc1fd87ce7a7e92dc Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 4 Jan 2020 09:57:30 +0100 Subject: [PATCH 18/19] :hammer: Removed unnecessary parenthesis in pure_pursuit.py --- PathTracking/pure_pursuit/pure_pursuit.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 4e8b1bed..bfc3c7cc 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -136,7 +136,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): """ if not isinstance(x, float): - for (ix, iy, iyaw) in zip(x, y, yaw): + for ix, iy, iyaw in zip(x, y, yaw): plot_arrow(ix, iy, iyaw) else: plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), From 08428bed153dfe49085e63becd6f399a713dd57c Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 4 Jan 2020 09:57:47 +0100 Subject: [PATCH 19/19] :pencil: Added Gjacquenot as author as suggested by @AtsushiSakai --- PathTracking/pure_pursuit/pure_pursuit.py | 1 + 1 file changed, 1 insertion(+) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index bfc3c7cc..bcaf8697 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -3,6 +3,7 @@ Path tracking simulation with pure pursuit steering control and PID speed control. author: Atsushi Sakai (@Atsushi_twi) + Guillaume Jacquenot (@Gjacquenot) """ import numpy as np