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' diff --git a/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py b/AerialNavigation/drone_3d_trajectory_following/Quadrotor.py index 9b38677c..413a8625 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,10 @@ 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]) + self.ax = fig.add_subplot(111, projection='3d') self.update_pose(x, y, z, roll, pitch, yaw) @@ -81,4 +84,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 dde76f19..eb9d7ba4 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -567,6 +567,9 @@ 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]) 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..f3bd2557 100644 --- a/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py +++ b/ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py @@ -162,6 +162,9 @@ 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) plt.show() plt.pause(1e-2) @@ -262,4 +265,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..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,9 @@ 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) 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..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,9 @@ 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]) 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 0e91be0f..75e7cf30 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,9 @@ 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 3cf2936e..6502ce5b 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -111,6 +111,9 @@ 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) 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 3e2af749..58a998e2 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -213,6 +213,9 @@ 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]) 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..2b057e21 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -191,6 +191,9 @@ 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") 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 db715a6d..135b476c 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -233,6 +233,9 @@ 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) 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 d14b7c06..bfef3ed9 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -230,6 +230,9 @@ 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]) 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..7bf279ce 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -240,6 +240,9 @@ 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") 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..c331d567 100644 --- a/Mapping/circle_fitting/circle_fitting.py +++ b/Mapping/circle_fitting/circle_fitting.py @@ -124,6 +124,9 @@ 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") 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 9c1aa273..b7664f1a 100644 --- a/Mapping/gaussian_grid_map/gaussian_grid_map.py +++ b/Mapping/gaussian_grid_map/gaussian_grid_map.py @@ -73,6 +73,9 @@ 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) 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 c2518b80..e18960e9 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -133,8 +133,10 @@ 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() - plt.plot(cx, cy, "or") plt.xlim(-2.0, 10.0) plt.ylim(-2.0, 10.0) diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/raycasting_grid_map/raycasting_grid_map.py index a0d90609..8ce37b92 100644 --- a/Mapping/raycasting_grid_map/raycasting_grid_map.py +++ b/Mapping/raycasting_grid_map/raycasting_grid_map.py @@ -124,6 +124,9 @@ 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) 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 2018f025..1a2a5340 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -242,6 +242,9 @@ 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") plt.plot(0.0, 0.0, "*r") v1.plot() diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 061aafbe..b0c718c9 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -79,6 +79,9 @@ 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: plt.pause(0.001) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 8e7af39f..227b3f9d 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -534,6 +534,9 @@ 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: 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 be1d2238..a413a05d 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -135,6 +135,9 @@ 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") 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 5c06f53a..62da6d76 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -69,6 +69,9 @@ 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: plt.pause(0.001) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 96c9c8e4..410e8db0 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -318,6 +318,9 @@ 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)) # plotting diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 2dab2f30..92fd08bb 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -268,6 +268,9 @@ 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") 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..414f6b45 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -213,6 +213,9 @@ 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) if show_animation: diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index f3ae2a43..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) @@ -347,6 +335,9 @@ 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) 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 9d7e1616..0d2239a9 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -213,6 +213,9 @@ 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]) while True: cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap) @@ -266,6 +269,9 @@ 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") 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..a2729328 100644 --- a/PathPlanning/HybridAStar/a_star.py +++ b/PathPlanning/HybridAStar/a_star.py @@ -78,6 +78,9 @@ 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: plt.pause(0.001) @@ -228,4 +231,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 ea6a4661..725976dc 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -327,6 +327,9 @@ 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: plt.pause(0.001) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 065868f2..912e56fa 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -266,8 +266,10 @@ class InformedRRTStar: return path 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: 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..ba01526a 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -54,6 +54,9 @@ 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") 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..177131ac 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -102,6 +102,9 @@ 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: 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..31904fa4 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py @@ -98,6 +98,9 @@ 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") plt.plot(gix, giy, "*m") diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 8090e336..7715174e 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -194,6 +194,9 @@ 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") plt.pause(0.001) diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index ecef7d45..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 @@ -151,6 +144,9 @@ 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) plt.axis("equal") plot_arrow(sx, sy, syaw) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 5b6f3252..b590d7b1 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -138,6 +138,9 @@ 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: 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..4599904c 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -106,6 +106,9 @@ 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: 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..65fac54c 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -112,6 +112,9 @@ 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: 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..6ea66dc7 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -122,6 +122,9 @@ 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: 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..fbf94957 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -395,6 +395,9 @@ 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)) # plotting diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index c73d6509..6db4bb26 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -185,6 +185,9 @@ 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) 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..98a12ab1 100644 --- a/PathTracking/cgmres_nmpc/cgmres_nmpc.py +++ b/PathTracking/cgmres_nmpc/cgmres_nmpc.py @@ -546,6 +546,9 @@ 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") 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 c7c8e0dd..bd2e54bf 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,9 @@ 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") 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 b69577d3..6f01e5a8 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -203,6 +203,9 @@ 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") 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 a16768e3..26e8e74f 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,9 @@ 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: 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 e161b461..2ae0090d 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -93,6 +93,10 @@ 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]) + 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 370cac96..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 @@ -17,7 +18,6 @@ dt = 0.1 # [s] L = 2.9 # [m] wheel base of vehicle -old_nearest_point_index = None show_animation = True @@ -31,17 +31,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 +70,57 @@ 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 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] + dy = [state.rear_y - icy for icy in self.cy] + d = np.hypot(dx, dy) + ind = np.argmin(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): + L = state.calc_distance(self.cx[ind], self.cy[ind]) + ind += 1 + + return ind + + +def pure_pursuit_control(state, trajectory, pind): + + ind = trajectory.search_target_index(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 +130,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"): """ @@ -122,7 +137,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), @@ -144,31 +159,27 @@ 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.search_target_index(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() + # 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) 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) @@ -181,7 +192,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]") @@ -189,7 +200,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) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index bf4f31b3..758e9ad9 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -149,6 +149,9 @@ 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") 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 9b808dd9..2af3989f 100644 --- a/PathTracking/stanley_controller/stanley_controller.py +++ b/PathTracking/stanley_controller/stanley_controller.py @@ -185,6 +185,9 @@ 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") plt.plot(x, y, "-b", label="trajectory") plt.plot(cx[target_idx], cy[target_idx], "xg", label="target") diff --git a/README.md b/README.md index e740049b..ec428127 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) + +- [Guillaume Jacquenot](https://github.com/Gjacquenot) diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 66234b57..3038e40a 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -235,6 +235,9 @@ 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") plt.plot(xEst[0], xEst[1], ".r") diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index 298cc036..2d7c0037 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -365,6 +365,9 @@ 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") for i in range(N_PARTICLE): diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index b9df6948..86881222 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -390,6 +390,9 @@ 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") for iz in range(len(z[:, 0])): diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index 41b9d525..9bb68c28 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -303,7 +303,9 @@ 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") plt.plot(hxTrue[0, :].flatten(), @@ -319,4 +321,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..0b280260 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -36,6 +36,9 @@ 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") plt.plot(current_points[0, :], current_points[1, :], ".b") plt.plot(0.0, 0.0, "xr") 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 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