From b8afdb10d8d8f57f5c6bde9b41ab64911d01ef8e Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 7 Jun 2020 20:28:29 +0900 Subject: [PATCH] Using scipy.spatial.rotation matrix (#335) --- .travis.yml | 3 +- .../ensemble_kalman_filter.py | 40 ++- .../histogram_filter/histogram_filter.py | 119 ++++--- .../particle_filter/particle_filter.py | 16 +- .../lidar_to_grid_map/lidar_to_grid_map.py | 187 +++++----- .../rectangle_fitting/rectangle_fitting.py | 74 ++-- Mapping/rectangle_fitting/simulator.py | 81 ++--- PathPlanning/AStar/a_star.py | 130 +++---- .../bidirectional_a_star.py | 22 +- .../bidirectional_breadth_first_search.py | 20 +- .../breadth_first_search.py | 20 +- .../ClosedLoopRRTStar/pure_pursuit.py | 6 +- .../DepthFirstSearch/depth_first_search.py | 20 +- PathPlanning/Dijkstra/dijkstra.py | 4 +- .../DubinsPath/dubins_path_planning.py | 96 +++--- .../dynamic_window_approach.py | 36 +- .../greedy_best_first_search.py | 20 +- .../grid_based_sweep_coverage_path_planner.py | 187 +++++----- PathPlanning/HybridAStar/a_star_heuristic.py | 147 ++++---- PathPlanning/HybridAStar/car.py | 50 +-- PathPlanning/HybridAStar/hybrid_a_star.py | 322 +++++++++--------- .../InformedRRTStar/informed_rrt_star.py | 91 ++--- .../probabilistic_road_map.py | 128 +++---- .../quintic_polynomials_planner.py | 6 +- .../VoronoiRoadMap/dijkstra_search.py | 4 +- SLAM/GraphBasedSLAM/graph_based_slam.py | 104 +++--- 26 files changed, 1002 insertions(+), 931 deletions(-) diff --git a/.travis.yml b/.travis.yml index da82e606..99d23921 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,10 +18,9 @@ before_install: - conda update -q conda # Useful for debugging any issues with conda - conda info -a -# - conda install python==3.6.8 install: - - conda install numpy==1.15 + - conda install numpy - conda install scipy - conda install matplotlib - conda install pandas diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 58a998e2..1307b1eb 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -5,7 +5,8 @@ Ensemble Kalman Filter(EnKF) localization sample author: Ryohei Sasaki(rsasaki0109) Ref: -- [Ensemble Kalman filtering](https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135) +Ensemble Kalman filtering +(https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135) """ @@ -13,6 +14,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from scipy.spatial.transform import Rotation as Rot # Simulation parameter Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2 @@ -48,8 +50,8 @@ def observation(xTrue, xd, u, RFID): angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d <= MAX_RANGE: dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise - anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5 # add noise - zi = np.array([dn, anglen, RFID[i, 0], RFID[i, 1]]) + angle_with_noise = angle + np.random.randn() * Q_sim[1, 1] ** 0.5 + zi = np.array([dn, angle_with_noise, RFID[i, 0], RFID[i, 1]]) z = np.vstack((z, zi)) # add noise to input @@ -79,10 +81,12 @@ def motion_model(x, u): def observe_landmark_position(x, landmarks): landmarks_pos = np.zeros((2 * landmarks.shape[0], 1)) for (i, lm) in enumerate(landmarks): - landmarks_pos[2 * i] = x[0, 0] + lm[0] * math.cos(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[ - 0, 0] ** 0.5 / np.sqrt(2) - landmarks_pos[2 * i + 1] = x[1, 0] + lm[0] * math.sin(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[ - 0, 0] ** 0.5 / np.sqrt(2) + index = 2 * i + q = Q_sim[0, 0] ** 0.5 + landmarks_pos[index] = x[0, 0] + lm[0] * math.cos( + x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2) + landmarks_pos[index + 1] = x[1, 0] + lm[0] * math.sin( + x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2) return landmarks_pos @@ -148,8 +152,9 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover t = np.arange(0, 2 * math.pi + 0.1, 0.1) - # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely - # close to 0 (~10^-20), catch these cases and set the respective variable to 0 + # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative + # numbers extremely close to 0 (~10^-20), catch these cases and set + # the respective variable to 0 try: a = math.sqrt(eig_val[big_ind]) except ValueError: @@ -163,11 +168,11 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0]) - R = np.array([[math.cos(angle), math.sin(angle)], - [-math.sin(angle), math.cos(angle)]]) - fx = R.dot(np.array([[x, y]])) - px = np.array(fx[0, :] + xEst[0, 0]).flatten() - py = np.array(fx[1, :] + xEst[1, 0]).flatten() + rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2] + fx = np.stack([x, y]).T @ rot + + px = np.array(fx[:, 0] + xEst[0, 0]).flatten() + py = np.array(fx[:, 1] + xEst[1, 0]).flatten() plt.plot(px, py, "--r") @@ -214,8 +219,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.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") @@ -227,7 +233,7 @@ def main(): np.array(hxDR[1, :]).flatten(), "-k") plt.plot(np.array(hxEst[0, :]).flatten(), np.array(hxEst[1, :]).flatten(), "-r") - # plot_covariance_ellipse(xEst, PEst) + plot_covariance_ellipse(xEst, PEst) plt.axis("equal") plt.grid(True) plt.pause(0.001) diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index 135b476c..7f453d69 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -28,11 +28,11 @@ MOTION_STD = 1.0 # standard deviation for motion gaussian distribution RANGE_STD = 3.0 # standard deviation for observation gaussian distribution # grid map param -XY_RESO = 0.5 # xy grid resolution -MINX = -15.0 -MINY = -5.0 -MAXX = 15.0 -MAXY = 25.0 +XY_RESOLUTION = 0.5 # xy grid resolution +MIN_X = -15.0 +MIN_Y = -5.0 +MAX_X = 15.0 +MAX_Y = 25.0 # simulation parameters NOISE_RANGE = 2.0 # [m] 1σ range noise parameter @@ -41,17 +41,17 @@ NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter show_animation = True -class GridMap(): +class GridMap: def __init__(self): self.data = None - self.xy_reso = None - self.minx = None - self.miny = None - self.maxx = None - self.maxx = None - self.xw = None - self.yw = None + self.xy_resolution = None + self.min_x = None + self.min_y = None + self.max_x = None + self.max_y = None + self.x_w = None + self.y_w = None self.dx = 0.0 # movement distance self.dy = 0.0 # movement distance @@ -64,10 +64,10 @@ def histogram_filter_localization(grid_map, u, z, yaw): return grid_map -def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std): +def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std): # predicted range - x = ix * gmap.xy_reso + gmap.minx - y = iy * gmap.xy_reso + gmap.miny + x = ix * grid_map.xy_resolution + grid_map.min_x + y = iy * grid_map.xy_resolution + grid_map.min_y d = math.hypot(x - z[iz, 1], y - z[iz, 2]) # likelihood @@ -76,16 +76,16 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std): return pdf -def observation_update(gmap, z, std): +def observation_update(grid_map, z, std): for iz in range(z.shape[0]): - for ix in range(gmap.xw): - for iy in range(gmap.yw): - gmap.data[ix][iy] *= calc_gaussian_observation_pdf( - gmap, z, iz, ix, iy, std) + for ix in range(grid_map.x_w): + for iy in range(grid_map.y_w): + grid_map.data[ix][iy] *= calc_gaussian_observation_pdf( + grid_map, z, iz, ix, iy, std) - gmap = normalize_probability(gmap) + grid_map = normalize_probability(grid_map) - return gmap + return grid_map def calc_input(): @@ -112,8 +112,8 @@ def motion_model(x, u): def draw_heat_map(data, mx, my): - maxp = max([max(igmap) for igmap in data]) - plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues")) + max_value = max([max(i_data) for i_data in data]) + plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues")) plt.axis("equal") @@ -140,43 +140,47 @@ def observation(xTrue, u, RFID): return xTrue, z, ud -def normalize_probability(gmap): - sump = sum([sum(igmap) for igmap in gmap.data]) +def normalize_probability(grid_map): + sump = sum([sum(i_data) for i_data in grid_map.data]) - for ix in range(gmap.xw): - for iy in range(gmap.yw): - gmap.data[ix][iy] /= sump + for ix in range(grid_map.x_w): + for iy in range(grid_map.y_w): + grid_map.data[ix][iy] /= sump - return gmap + return grid_map -def init_gmap(xy_reso, minx, miny, maxx, maxy): +def init_grid_map(xy_resolution, min_x, min_y, max_x, max_y): grid_map = GridMap() - grid_map.xy_reso = xy_reso - grid_map.minx = minx - grid_map.miny = miny - grid_map.maxx = maxx - grid_map.maxy = maxy - grid_map.xw = int(round((grid_map.maxx - grid_map.minx) / grid_map.xy_reso)) - grid_map.yw = int(round((grid_map.maxy - grid_map.miny) / grid_map.xy_reso)) + grid_map.xy_resolution = xy_resolution + grid_map.min_x = min_x + grid_map.min_y = min_y + grid_map.max_x = max_x + grid_map.max_y = max_y + grid_map.x_w = int(round((grid_map.max_x - grid_map.min_x) + / grid_map.xy_resolution)) + grid_map.y_w = int(round((grid_map.max_y - grid_map.min_y) + / grid_map.xy_resolution)) - grid_map.data = [[1.0 for _ in range(grid_map.yw)] for _ in range(grid_map.xw)] + grid_map.data = [[1.0 for _ in range(grid_map.y_w)] + for _ in range(grid_map.x_w)] grid_map = normalize_probability(grid_map) return grid_map def map_shift(grid_map, x_shift, y_shift): - tgmap = copy.deepcopy(grid_map.data) + tmp_grid_map = copy.deepcopy(grid_map.data) - for ix in range(grid_map.xw): - for iy in range(grid_map.yw): + for ix in range(grid_map.x_w): + for iy in range(grid_map.y_w): nix = ix + x_shift niy = iy + y_shift - if 0 <= nix < grid_map.xw and 0 <= niy < grid_map.yw: - grid_map.data[ix + x_shift][iy + y_shift] = tgmap[ix][iy] + if 0 <= nix < grid_map.x_w and 0 <= niy < grid_map.y_w: + grid_map.data[ix + x_shift][iy + y_shift] =\ + tmp_grid_map[ix][iy] return grid_map @@ -185,22 +189,26 @@ def motion_update(grid_map, u, yaw): grid_map.dx += DT * math.cos(yaw) * u[0] grid_map.dy += DT * math.sin(yaw) * u[0] - x_shift = grid_map.dx // grid_map.xy_reso - y_shift = grid_map.dy // grid_map.xy_reso + x_shift = grid_map.dx // grid_map.xy_resolution + y_shift = grid_map.dy // grid_map.xy_resolution if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted grid_map = map_shift(grid_map, int(x_shift), int(y_shift)) - grid_map.dx -= x_shift * grid_map.xy_reso - grid_map.dy -= y_shift * grid_map.xy_reso + grid_map.dx -= x_shift * grid_map.xy_resolution + grid_map.dy -= y_shift * grid_map.xy_resolution grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD) return grid_map -def calc_grid_index(gmap): - mx, my = np.mgrid[slice(gmap.minx - gmap.xy_reso / 2.0, gmap.maxx + gmap.xy_reso / 2.0, gmap.xy_reso), - slice(gmap.miny - gmap.xy_reso / 2.0, gmap.maxy + gmap.xy_reso / 2.0, gmap.xy_reso)] +def calc_grid_index(grid_map): + mx, my = np.mgrid[slice(grid_map.min_x - grid_map.xy_resolution / 2.0, + grid_map.max_x + grid_map.xy_resolution / 2.0, + grid_map.xy_resolution), + slice(grid_map.min_y - grid_map.xy_resolution / 2.0, + grid_map.max_y + grid_map.xy_resolution / 2.0, + grid_map.xy_resolution)] return mx, my @@ -217,7 +225,7 @@ def main(): time = 0.0 xTrue = np.zeros((4, 1)) - grid_map = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY) + grid_map = init_grid_map(XY_RESOLUTION, MIN_X, MIN_Y, MAX_X, MAX_Y) mx, my = calc_grid_index(grid_map) # for grid map visualization while SIM_TIME >= time: @@ -234,8 +242,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.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 6eca4eb5..89a43ea6 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -10,6 +10,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from scipy.spatial.transform import Rotation as Rot # Estimation parameter of PF Q = np.diag([0.2]) ** 2 # range error @@ -172,8 +173,9 @@ def plot_covariance_ellipse(x_est, p_est): # pragma: no cover t = np.arange(0, 2 * math.pi + 0.1, 0.1) - # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely - # close to 0 (~10^-20), catch these cases and set the respective variable to 0 + # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative + # numbers extremely close to 0 (~10^-20), catch these cases and set the + # respective variable to 0 try: a = math.sqrt(eig_val[big_ind]) except ValueError: @@ -187,9 +189,8 @@ def plot_covariance_ellipse(x_est, p_est): # pragma: no cover x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0]) - Rot = np.array([[math.cos(angle), -math.sin(angle)], - [math.sin(angle), math.cos(angle)]]) - fx = Rot.dot(np.array([[x, y]])) + rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2] + fx = rot.dot(np.array([[x, y]])) px = np.array(fx[0, :] + x_est[0, 0]).flatten() py = np.array(fx[1, :] + x_est[1, 0]).flatten() plt.plot(px, py, "--r") @@ -235,8 +236,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.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([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k") diff --git a/Mapping/lidar_to_grid_map/lidar_to_grid_map.py b/Mapping/lidar_to_grid_map/lidar_to_grid_map.py index 60bf4db6..911a0bf0 100644 --- a/Mapping/lidar_to_grid_map/lidar_to_grid_map.py +++ b/Mapping/lidar_to_grid_map/lidar_to_grid_map.py @@ -2,15 +2,16 @@ LIDAR to 2D grid map example -author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi) +author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts """ import math -import numpy as np -import matplotlib.pyplot as plt from collections import deque +import matplotlib.pyplot as plt +import numpy as np + EXTEND_AREA = 1.0 @@ -44,47 +45,49 @@ def bresenham(start, end): x2, y2 = end dx = x2 - x1 dy = y2 - y1 - is_steep = abs(dy) > abs(dx) # determine how steep the line is - if is_steep: # rotate line + is_steep = abs(dy) > abs(dx) # determine how steep the line is + if is_steep: # rotate line x1, y1 = y1, x1 x2, y2 = y2, x2 - swapped = False # swap start and end points if necessary and store swap state + # swap start and end points if necessary and store swap state + swapped = False if x1 > x2: x1, x2 = x2, x1 y1, y2 = y2, y1 swapped = True - dx = x2 - x1 # recalculate differentials - dy = y2 - y1 # recalculate differentials - error = int(dx / 2.0) # calculate error - ystep = 1 if y1 < y2 else -1 + dx = x2 - x1 # recalculate differentials + dy = y2 - y1 # recalculate differentials + error = int(dx / 2.0) # calculate error + y_step = 1 if y1 < y2 else -1 # iterate over bounding box generating points between start and end y = y1 points = [] for x in range(x1, x2 + 1): coord = [y, x] if is_steep else (x, y) - points.append(coord) + points.append(coord) error -= abs(dy) if error < 0: - y += ystep + y += y_step error += dx - if swapped: # reverse the list if the coordinates were swapped + if swapped: # reverse the list if the coordinates were swapped points.reverse() points = np.array(points) return points -def calc_grid_map_config(ox, oy, xyreso): +def calc_grid_map_config(ox, oy, xy_resolution): """ - Calculates the size, and the maximum distances according to the the measurement center + Calculates the size, and the maximum distances according to the the + measurement center """ - minx = round(min(ox) - EXTEND_AREA / 2.0) - miny = round(min(oy) - EXTEND_AREA / 2.0) - maxx = round(max(ox) + EXTEND_AREA / 2.0) - maxy = round(max(oy) + EXTEND_AREA / 2.0) - xw = int(round((maxx - minx) / xyreso)) - yw = int(round((maxy - miny) / xyreso)) + min_x = round(min(ox) - EXTEND_AREA / 2.0) + min_y = round(min(oy) - EXTEND_AREA / 2.0) + max_x = round(max(ox) + EXTEND_AREA / 2.0) + max_y = round(max(oy) + EXTEND_AREA / 2.0) + xw = int(round((max_x - min_x) / xy_resolution)) + yw = int(round((max_y - min_y) / xy_resolution)) print("The grid map is ", xw, "x", yw, ".") - return minx, miny, maxx, maxy, xw, yw + return min_x, min_y, max_x, max_y, xw, yw def atan_zero_to_twopi(y, x): @@ -94,96 +97,110 @@ def atan_zero_to_twopi(y, x): return angle -def init_floodfill(cpoint, opoints, xypoints, mincoord, xyreso): +def init_flood_fill(center_point, obstacle_points, xy_points, min_coord, + xy_resolution): """ - cpoint: center point - opoints: detected obstacles points (x,y) - xypoints: (x,y) point pairs + center_point: center point + obstacle_points: detected obstacles points (x,y) + xy_points: (x,y) point pairs """ - centix, centiy = cpoint - prev_ix, prev_iy = centix - 1, centiy - ox, oy = opoints - xw, yw = xypoints - minx, miny = mincoord - pmap = (np.ones((xw, yw))) * 0.5 + center_x, center_y = center_point + prev_ix, prev_iy = center_x - 1, center_y + ox, oy = obstacle_points + xw, yw = xy_points + min_x, min_y = min_coord + occupancy_map = (np.ones((xw, yw))) * 0.5 for (x, y) in zip(ox, oy): - ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area - iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area + # x coordinate of the the occupied area + ix = int(round((x - min_x) / xy_resolution)) + # y coordinate of the the occupied area + iy = int(round((y - min_y) / xy_resolution)) free_area = bresenham((prev_ix, prev_iy), (ix, iy)) for fa in free_area: - pmap[fa[0]][fa[1]] = 0 # free area 0.0 + occupancy_map[fa[0]][fa[1]] = 0 # free area 0.0 prev_ix = ix prev_iy = iy - return pmap + return occupancy_map -def flood_fill(cpoint, pmap): +def flood_fill(center_point, occupancy_map): """ - cpoint: starting point (x,y) of fill - pmap: occupancy map generated from Bresenham ray-tracing + center_point: starting point (x,y) of fill + occupancy_map: occupancy map generated from Bresenham ray-tracing """ # Fill empty areas with queue method - sx, sy = pmap.shape + sx, sy = occupancy_map.shape fringe = deque() - fringe.appendleft(cpoint) + fringe.appendleft(center_point) while fringe: n = fringe.pop() nx, ny = n # West if nx > 0: - if pmap[nx - 1, ny] == 0.5: - pmap[nx - 1, ny] = 0.0 + if occupancy_map[nx - 1, ny] == 0.5: + occupancy_map[nx - 1, ny] = 0.0 fringe.appendleft((nx - 1, ny)) # East if nx < sx - 1: - if pmap[nx + 1, ny] == 0.5: - pmap[nx + 1, ny] = 0.0 + if occupancy_map[nx + 1, ny] == 0.5: + occupancy_map[nx + 1, ny] = 0.0 fringe.appendleft((nx + 1, ny)) # North if ny > 0: - if pmap[nx, ny - 1] == 0.5: - pmap[nx, ny - 1] = 0.0 + if occupancy_map[nx, ny - 1] == 0.5: + occupancy_map[nx, ny - 1] = 0.0 fringe.appendleft((nx, ny - 1)) # South if ny < sy - 1: - if pmap[nx, ny + 1] == 0.5: - pmap[nx, ny + 1] = 0.0 + if occupancy_map[nx, ny + 1] == 0.5: + occupancy_map[nx, ny + 1] = 0.0 fringe.appendleft((nx, ny + 1)) -def generate_ray_casting_grid_map(ox, oy, xyreso, breshen=True): +def generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True): """ - The breshen boolean tells if it's computed with bresenham ray casting (True) or with flood fill (False) + The breshen boolean tells if it's computed with bresenham ray casting + (True) or with flood fill (False) """ - minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso) - pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)] - centix = int(round(-minx / xyreso)) # center x coordinate of the grid map - centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map + min_x, min_y, max_x, max_y, x_w, y_w = calc_grid_map_config( + ox, oy, xy_resolution) + # default 0.5 -- [[0.5 for i in range(y_w)] for i in range(x_w)] + occupancy_map = np.ones((x_w, y_w)) / 2 + center_x = int( + round(-min_x / xy_resolution)) # center x coordinate of the grid map + center_y = int( + round(-min_y / xy_resolution)) # center y coordinate of the grid map # occupancy grid computed with bresenham ray casting if breshen: for (x, y) in zip(ox, oy): - ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area - iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area - laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point + # x coordinate of the the occupied area + ix = int(round((x - min_x) / xy_resolution)) + # y coordinate of the the occupied area + iy = int(round((y - min_y) / xy_resolution)) + laser_beams = bresenham((center_x, center_y), ( + ix, iy)) # line form the lidar to the occupied point for laser_beam in laser_beams: - pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0 - pmap[ix][iy] = 1.0 # occupied area 1.0 - pmap[ix+1][iy] = 1.0 # extend the occupied area - pmap[ix][iy+1] = 1.0 # extend the occupied area - pmap[ix+1][iy+1] = 1.0 # extend the occupied area + occupancy_map[laser_beam[0]][ + laser_beam[1]] = 0.0 # free area 0.0 + occupancy_map[ix][iy] = 1.0 # occupied area 1.0 + occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area + occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area + occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area # occupancy grid computed with with flood fill else: - pmap = init_floodfill((centix, centiy), (ox, oy), (xw, yw), (minx, miny), xyreso) - flood_fill((centix, centiy), pmap) - pmap = np.array(pmap, dtype=np.float) + occupancy_map = init_flood_fill((center_x, center_y), (ox, oy), + (x_w, y_w), + (min_x, min_y), xy_resolution) + flood_fill((center_x, center_y), occupancy_map) + occupancy_map = np.array(occupancy_map, dtype=np.float) for (x, y) in zip(ox, oy): - ix = int(round((x - minx) / xyreso)) - iy = int(round((y - miny) / xyreso)) - pmap[ix][iy] = 1.0 # occupied area 1.0 - pmap[ix+1][iy] = 1.0 # extend the occupied area - pmap[ix][iy+1] = 1.0 # extend the occupied area - pmap[ix+1][iy+1] = 1.0 # extend the occupied area - return pmap, minx, maxx, miny, maxy, xyreso + ix = int(round((x - min_x) / xy_resolution)) + iy = int(round((y - min_y) / xy_resolution)) + occupancy_map[ix][iy] = 1.0 # occupied area 1.0 + occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area + occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area + occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area + return occupancy_map, min_x, max_x, min_y, max_y, xy_resolution def main(): @@ -191,18 +208,20 @@ def main(): Example usage """ print(__file__, "start") - xyreso = 0.02 # x-y grid resolution + xy_resolution = 0.02 # x-y grid resolution ang, dist = file_read("lidar01.csv") ox = np.sin(ang) * dist oy = np.cos(ang) * dist - pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, True) - xyres = np.array(pmap).shape - plt.figure(1, figsize=(10,4)) + occupancy_map, min_x, max_x, min_y, max_y, xy_resolution = \ + generate_ray_casting_grid_map(ox, oy, xy_resolution, True) + xy_res = np.array(occupancy_map).shape + plt.figure(1, figsize=(10, 4)) plt.subplot(122) - plt.imshow(pmap, cmap="PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r" + plt.imshow(occupancy_map, cmap="PiYG_r") + # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r" plt.clim(-0.4, 1.4) - plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor=True) - plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor=True) + plt.gca().set_xticks(np.arange(-.5, xy_res[1], 1), minor=True) + plt.gca().set_yticks(np.arange(-.5, xy_res[0], 1), minor=True) plt.grid(True, which="minor", color="w", linewidth=0.6, alpha=0.5) plt.colorbar() plt.subplot(121) @@ -210,11 +229,11 @@ def main(): plt.axis("equal") plt.plot(0.0, 0.0, "ob") plt.gca().set_aspect("equal", "box") - bottom, top = plt.ylim() # return the current ylim - plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation + bottom, top = plt.ylim() # return the current y-lim + plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation plt.grid(True) plt.show() - + if __name__ == '__main__': - main() + main() diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 1a2a5340..ae41b3c5 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -5,7 +5,10 @@ Object shape recognition with L-shape fitting author: Atsushi Sakai (@Atsushi_twi) Ref: -- [Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University](https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/) +- Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners - +The Robotics Institute Carnegie Mellon University +https://www.ri.cmu.edu/publications/ +efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/ """ @@ -13,13 +16,14 @@ import matplotlib.pyplot as plt import numpy as np import itertools from enum import Enum +from scipy.spatial.transform import Rotation as Rot from simulator import VehicleSimulator, LidarSimulator show_animation = True -class LShapeFitting(): +class LShapeFitting: class Criteria(Enum): AREA = 1 @@ -29,26 +33,27 @@ class LShapeFitting(): def __init__(self): # Parameters self.criteria = self.Criteria.VARIANCE - self.min_dist_of_closeness_crit = 0.01 # [m] - self.dtheta_deg_for_serarch = 1.0 # [deg] + self.min_dist_of_closeness_criteria = 0.01 # [m] + self.d_theta_deg_for_search = 1.0 # [deg] self.R0 = 3.0 # [m] range segmentation param self.Rd = 0.001 # [m] range segmentation param def fitting(self, ox, oy): # step1: Adaptive Range Segmentation - idsets = self._adoptive_range_segmentation(ox, oy) + id_sets = self._adoptive_range_segmentation(ox, oy) # step2 Rectangle search rects = [] - for ids in idsets: # for each cluster + for ids in id_sets: # for each cluster cx = [ox[i] for i in range(len(ox)) if i in ids] cy = [oy[i] for i in range(len(oy)) if i in ids] rects.append(self._rectangle_search(cx, cy)) - return rects, idsets + return rects, id_sets - def _calc_area_criterion(self, c1, c2): + @staticmethod + def _calc_area_criterion(c1, c2): c1_max = max(c1) c2_max = max(c2) c1_min = min(c1) @@ -71,12 +76,13 @@ class LShapeFitting(): beta = 0 for i, _ in enumerate(D1): - d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_crit) + d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_criteria) beta += (1.0 / d) return beta - def _calc_variance_criterion(self, c1, c2): + @staticmethod + def _calc_variance_criterion(c1, c2): c1_max = max(c1) c2_max = max(c2) c1_min = min(c1) @@ -110,17 +116,17 @@ class LShapeFitting(): X = np.array([x, y]).T - dtheta = np.deg2rad(self.dtheta_deg_for_serarch) - minp = (-float('inf'), None) - for theta in np.arange(0.0, np.pi / 2.0 - dtheta, dtheta): + d_theta = np.deg2rad(self.d_theta_deg_for_search) + min_cost = (-float('inf'), None) + for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta): - e1 = np.array([np.cos(theta), np.sin(theta)]) - e2 = np.array([-np.sin(theta), np.cos(theta)]) - - c1 = X @ e1.T - c2 = X @ e2.T + rot = Rot.from_euler('z', theta).as_matrix()[0:2, 0:2] + c = X @ rot + c1 = c[:, 0] + c2 = c[:, 1] # Select criteria + cost = 0.0 if self.criteria == self.Criteria.AREA: cost = self._calc_area_criterion(c1, c2) elif self.criteria == self.Criteria.CLOSENESS: @@ -128,12 +134,12 @@ class LShapeFitting(): elif self.criteria == self.Criteria.VARIANCE: cost = self._calc_variance_criterion(c1, c2) - if minp[0] < cost: - minp = (cost, theta) + if min_cost[0] < cost: + min_cost = (cost, theta) # calc best rectangle - sin_s = np.sin(minp[1]) - cos_s = np.cos(minp[1]) + sin_s = np.sin(min_cost[1]) + cos_s = np.cos(min_cost[1]) c1_s = X @ np.array([cos_s, sin_s]).T c2_s = X @ np.array([-sin_s, cos_s]).T @@ -181,7 +187,7 @@ class LShapeFitting(): return S -class RectangleData(): +class RectangleData: def __init__(self): self.a = [None] * 4 @@ -207,7 +213,8 @@ class RectangleData(): [self.a[3], self.a[0]], [self.b[3], self.b[0]], [self.c[3], self.c[0]]) self.rect_c_x[4], self.rect_c_y[4] = self.rect_c_x[0], self.rect_c_y[0] - def calc_cross_point(self, a, b, c): + @staticmethod + def calc_cross_point(a, b, c): x = (b[0] * -c[1] - b[1] * -c[0]) / (a[0] * b[1] - a[1] * b[0]) y = (a[1] * -c[0] - a[0] * -c[1]) / (a[0] * b[1] - a[1] * b[0]) return x, y @@ -216,42 +223,43 @@ class RectangleData(): def main(): # simulation parameters - simtime = 30.0 # simulation time + sim_time = 30.0 # simulation time dt = 0.2 # time tick - angle_reso = np.deg2rad(3.0) # sensor angle resolution + angle_resolution = np.deg2rad(3.0) # sensor angle resolution v1 = VehicleSimulator(-10.0, 0.0, np.deg2rad(90.0), 0.0, 50.0 / 3.6, 3.0, 5.0) v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0), 0.0, 50.0 / 3.6, 4.0, 10.0) - lshapefitting = LShapeFitting() + l_shape_fitting = LShapeFitting() lidar_sim = LidarSimulator() time = 0.0 - while time <= simtime: + while time <= sim_time: time += dt v1.update(dt, 0.1, 0.0) v2.update(dt, 0.1, -0.05) - ox, oy = lidar_sim.get_observation_points([v1, v2], angle_reso) + ox, oy = lidar_sim.get_observation_points([v1, v2], angle_resolution) - rects, idsets = lshapefitting.fitting(ox, oy) + rects, id_sets = l_shape_fitting.fitting(ox, oy) 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.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() v2.plot() # Plot range observation - for ids in idsets: + for ids in id_sets: x = [ox[i] for i in range(len(ox)) if i in ids] y = [oy[i] for i in range(len(ox)) if i in ids] diff --git a/Mapping/rectangle_fitting/simulator.py b/Mapping/rectangle_fitting/simulator.py index b21d4003..c7c3141f 100644 --- a/Mapping/rectangle_fitting/simulator.py +++ b/Mapping/rectangle_fitting/simulator.py @@ -10,15 +10,16 @@ import numpy as np import matplotlib.pyplot as plt import math import random +from scipy.spatial.transform import Rotation as Rot -class VehicleSimulator(): +class VehicleSimulator: - def __init__(self, ix, iy, iyaw, iv, max_v, w, L): - self.x = ix - self.y = iy - self.yaw = iyaw - self.v = iv + def __init__(self, i_x, i_y, i_yaw, i_v, max_v, w, L): + self.x = i_x + self.y = i_y + self.yaw = i_yaw + self.v = i_v self.max_v = max_v self.W = w self.L = L @@ -40,10 +41,10 @@ class VehicleSimulator(): plt.plot(gx, gy, "--b") def calc_global_contour(self): - gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) + - self.x for (ix, iy) in zip(self.vc_x, self.vc_y)] - gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) + - self.y for (ix, iy) in zip(self.vc_x, self.vc_y)] + rot = Rot.from_euler('z', self.yaw).as_matrix()[0:2, 0:2] + gxy = np.stack([self.vc_x, self.vc_y]).T @ rot + gx = gxy[:, 0] + self.x + gy = gxy[:, 1] + self.y return gx, gy @@ -67,69 +68,71 @@ class VehicleSimulator(): self.vc_x.append(self.L / 2.0) self.vc_y.append(self.W / 2.0) - self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y) + self.vc_x, self.vc_y = self._interpolate(self.vc_x, self.vc_y) - def _interporate(self, x, y): + @staticmethod + def _interpolate(x, y): rx, ry = [], [] - dtheta = 0.05 + d_theta = 0.05 for i in range(len(x) - 1): - rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1] - for θ in np.arange(0.0, 1.0, dtheta)]) - ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1] - for θ in np.arange(0.0, 1.0, dtheta)]) + rx.extend([(1.0 - theta) * x[i] + theta * x[i + 1] + for theta in np.arange(0.0, 1.0, d_theta)]) + ry.extend([(1.0 - theta) * y[i] + theta * y[i + 1] + for theta in np.arange(0.0, 1.0, d_theta)]) - rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1] - for θ in np.arange(0.0, 1.0, dtheta)]) - ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1] - for θ in np.arange(0.0, 1.0, dtheta)]) + rx.extend([(1.0 - theta) * x[len(x) - 1] + theta * x[1] + for theta in np.arange(0.0, 1.0, d_theta)]) + ry.extend([(1.0 - theta) * y[len(y) - 1] + theta * y[1] + for theta in np.arange(0.0, 1.0, d_theta)]) return rx, ry -class LidarSimulator(): +class LidarSimulator: def __init__(self): self.range_noise = 0.01 - def get_observation_points(self, vlist, angle_reso): + def get_observation_points(self, v_list, angle_resolution): x, y, angle, r = [], [], [], [] # store all points - for v in vlist: + for v in v_list: gx, gy = v.calc_global_contour() for vx, vy in zip(gx, gy): - vangle = math.atan2(vy, vx) + v_angle = math.atan2(vy, vx) vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise, 1.0 + self.range_noise) x.append(vx) y.append(vy) - angle.append(vangle) + angle.append(v_angle) r.append(vr) # ray casting filter - rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso) + rx, ry = self.ray_casting_filter(angle, r, angle_resolution) return rx, ry - def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso): + @staticmethod + def ray_casting_filter(theta_l, range_l, angle_resolution): rx, ry = [], [] - rangedb = [float("inf") for _ in range( - int(np.floor((np.pi * 2.0) / angle_reso)) + 1)] + range_db = [float("inf") for _ in range( + int(np.floor((np.pi * 2.0) / angle_resolution)) + 1)] - for i in range(len(thetal)): - angleid = int(round(thetal[i] / angle_reso)) + for i in range(len(theta_l)): + angle_id = int(round(theta_l[i] / angle_resolution)) - if rangedb[angleid] > rangel[i]: - rangedb[angleid] = rangel[i] + if range_db[angle_id] > range_l[i]: + range_db[angle_id] = range_l[i] - for i in range(len(rangedb)): - t = i * angle_reso - if rangedb[i] != float("inf"): - rx.append(rangedb[i] * np.cos(t)) - ry.append(rangedb[i] * np.sin(t)) + for i in range(len(range_db)): + t = i * angle_resolution + if range_db[i] != float("inf"): + rx.append(range_db[i] * np.cos(t)) + ry.append(range_db[i] * np.sin(t)) return rx, ry diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 14c51ad9..eb2672a9 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -18,39 +18,43 @@ show_animation = True class AStarPlanner: - def __init__(self, ox, oy, reso, rr): + def __init__(self, ox, oy, resolution, rr): """ Initialize grid map for a star planning ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ - self.reso = reso + self.resolution = resolution self.rr = rr - self.calc_obstacle_map(ox, oy) + self.min_x, self.min_y = 0, 0 + self.max_x, self.max_y = 0, 0 + self.obstacle_map = None + self.x_width, self.y_width = 0, 0 self.motion = self.get_motion_model() + self.calc_obstacle_map(ox, oy) class Node: - def __init__(self, x, y, cost, pind): + def __init__(self, x, y, cost, parent_index): self.x = x # index of grid self.y = y # index of grid self.cost = cost - self.pind = pind + self.parent_index = parent_index def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( - self.cost) + "," + str(self.pind) + self.cost) + "," + str(self.parent_index) def planning(self, sx, sy, gx, gy): """ A star path search input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] @@ -59,13 +63,13 @@ class AStarPlanner: ry: y position list of the final path """ - nstart = self.Node(self.calc_xyindex(sx, self.minx), - self.calc_xyindex(sy, self.miny), 0.0, -1) - ngoal = self.Node(self.calc_xyindex(gx, self.minx), - self.calc_xyindex(gy, self.miny), 0.0, -1) + start_node = self.Node(self.calc_xy_index(sx, self.min_x), + self.calc_xy_index(sy, self.min_y), 0.0, -1) + goal_node = self.Node(self.calc_xy_index(gx, self.min_x), + self.calc_xy_index(gy, self.min_y), 0.0, -1) open_set, closed_set = dict(), dict() - open_set[self.calc_grid_index(nstart)] = nstart + open_set[self.calc_grid_index(start_node)] = start_node while 1: if len(open_set) == 0: @@ -74,15 +78,15 @@ class AStarPlanner: c_id = min( open_set, - key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, + key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[ o])) current = open_set[c_id] # show graph 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.plot(self.calc_grid_position(current.x, self.min_x), + self.calc_grid_position(current.y, self.min_y), "xc") # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit( @@ -90,10 +94,10 @@ class AStarPlanner: if len(closed_set.keys()) % 10 == 0: plt.pause(0.001) - if current.x == ngoal.x and current.y == ngoal.y: + if current.x == goal_node.x and current.y == goal_node.y: print("Find goal") - ngoal.pind = current.pind - ngoal.cost = current.cost + goal_node.parent_index = current.parent_index + goal_node.cost = current.cost break # Remove the item from the open set @@ -123,20 +127,20 @@ class AStarPlanner: # This path is the best until now. record it open_set[n_id] = node - rx, ry = self.calc_final_path(ngoal, closed_set) + rx, ry = self.calc_final_path(goal_node, closed_set) return rx, ry - def calc_final_path(self, ngoal, closedset): + def calc_final_path(self, goal_node, closed_set): # generate final course - rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ - self.calc_grid_position(ngoal.y, self.miny)] - pind = ngoal.pind - while pind != -1: - n = closedset[pind] - rx.append(self.calc_grid_position(n.x, self.minx)) - ry.append(self.calc_grid_position(n.y, self.miny)) - pind = n.pind + rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ + self.calc_grid_position(goal_node.y, self.min_y)] + parent_index = goal_node.parent_index + while parent_index != -1: + n = closed_set[parent_index] + rx.append(self.calc_grid_position(n.x, self.min_x)) + ry.append(self.calc_grid_position(n.y, self.min_y)) + parent_index = n.parent_index return rx, ry @@ -146,69 +150,69 @@ class AStarPlanner: d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) return d - def calc_grid_position(self, index, minp): + def calc_grid_position(self, index, min_position): """ calc grid position :param index: - :param minp: + :param min_position: :return: """ - pos = index * self.reso + minp + pos = index * self.resolution + min_position return pos - def calc_xyindex(self, position, min_pos): - return round((position - min_pos) / self.reso) + def calc_xy_index(self, position, min_pos): + return round((position - min_pos) / self.resolution) def calc_grid_index(self, node): - return (node.y - self.miny) * self.xwidth + (node.x - self.minx) + return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) def verify_node(self, node): - px = self.calc_grid_position(node.x, self.minx) - py = self.calc_grid_position(node.y, self.miny) + px = self.calc_grid_position(node.x, self.min_x) + py = self.calc_grid_position(node.y, self.min_y) - if px < self.minx: + if px < self.min_x: return False - elif py < self.miny: + elif py < self.min_y: return False - elif px >= self.maxx: + elif px >= self.max_x: return False - elif py >= self.maxy: + elif py >= self.max_y: return False # collision check - if self.obmap[node.x][node.y]: + if self.obstacle_map[node.x][node.y]: return False return True def calc_obstacle_map(self, ox, oy): - self.minx = round(min(ox)) - self.miny = round(min(oy)) - self.maxx = round(max(ox)) - self.maxy = round(max(oy)) - print("minx:", self.minx) - print("miny:", self.miny) - print("maxx:", self.maxx) - print("maxy:", self.maxy) + self.min_x = round(min(ox)) + self.min_y = round(min(oy)) + self.max_x = round(max(ox)) + self.max_y = round(max(oy)) + print("min_x:", self.min_x) + print("min_y:", self.min_y) + print("max_x:", self.max_x) + print("max_y:", self.max_y) - self.xwidth = round((self.maxx - self.minx) / self.reso) - self.ywidth = round((self.maxy - self.miny) / self.reso) - print("xwidth:", self.xwidth) - print("ywidth:", self.ywidth) + self.x_width = round((self.max_x - self.min_x) / self.resolution) + self.y_width = round((self.max_y - self.min_y) / self.resolution) + print("x_width:", self.x_width) + print("y_width:", self.y_width) # obstacle map generation - self.obmap = [[False for i in range(self.ywidth)] - for i in range(self.xwidth)] - for ix in range(self.xwidth): - x = self.calc_grid_position(ix, self.minx) - for iy in range(self.ywidth): - y = self.calc_grid_position(iy, self.miny) + self.obstacle_map = [[False for _ in range(self.y_width)] + for _ in range(self.x_width)] + for ix in range(self.x_width): + x = self.calc_grid_position(ix, self.min_x) + for iy in range(self.y_width): + y = self.calc_grid_position(iy, self.min_y) for iox, ioy in zip(ox, oy): d = math.hypot(iox - x, ioy - y) if d <= self.rr: - self.obmap[ix][iy] = True + self.obstacle_map[ix][iy] = True break @staticmethod @@ -270,8 +274,8 @@ def main(): if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") - plt.show() plt.pause(0.001) + plt.show() if __name__ == '__main__': diff --git a/PathPlanning/BidirectionalAStar/bidirectional_a_star.py b/PathPlanning/BidirectionalAStar/bidirectional_a_star.py index 65d1fc31..cf76ab67 100644 --- a/PathPlanning/BidirectionalAStar/bidirectional_a_star.py +++ b/PathPlanning/BidirectionalAStar/bidirectional_a_star.py @@ -23,7 +23,7 @@ class BidirectionalAStarPlanner: ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ @@ -48,8 +48,8 @@ class BidirectionalAStarPlanner: Bidirectional A star path search input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] @@ -179,12 +179,12 @@ class BidirectionalAStarPlanner: # generate final course rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ self.calc_grid_position(ngoal.y, self.miny)] - pind = ngoal.pind + pind = ngoal.parent_index while pind != -1: n = closedset[pind] rx.append(self.calc_grid_position(n.x, self.minx)) ry.append(self.calc_grid_position(n.y, self.miny)) - pind = n.pind + pind = n.parent_index return rx, ry @@ -252,15 +252,15 @@ class BidirectionalAStarPlanner: self.miny = round(min(oy)) self.maxx = round(max(ox)) self.maxy = round(max(oy)) - print("minx:", self.minx) - print("miny:", self.miny) - print("maxx:", self.maxx) - print("maxy:", self.maxy) + print("min_x:", self.minx) + print("min_y:", self.miny) + print("max_x:", self.maxx) + print("max_y:", self.maxy) self.xwidth = round((self.maxx - self.minx) / self.reso) self.ywidth = round((self.maxy - self.miny) / self.reso) - print("xwidth:", self.xwidth) - print("ywidth:", self.ywidth) + print("x_width:", self.xwidth) + print("y_width:", self.ywidth) # obstacle map generation self.obmap = [[False for _ in range(self.ywidth)] diff --git a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py index d29112eb..328898db 100644 --- a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py +++ b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py @@ -23,7 +23,7 @@ class BidirectionalBreadthFirstSearchPlanner: ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ @@ -49,8 +49,8 @@ class BidirectionalBreadthFirstSearchPlanner: Bidirectional Breadth First search based planning input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] @@ -170,7 +170,7 @@ class BidirectionalBreadthFirstSearchPlanner: # generate final course rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ self.calc_grid_position(ngoal.y, self.miny)] - n = closedset[ngoal.pind] + n = closedset[ngoal.parent_index] while n is not None: rx.append(self.calc_grid_position(n.x, self.minx)) ry.append(self.calc_grid_position(n.y, self.miny)) @@ -220,15 +220,15 @@ class BidirectionalBreadthFirstSearchPlanner: self.miny = round(min(oy)) self.maxx = round(max(ox)) self.maxy = round(max(oy)) - print("minx:", self.minx) - print("miny:", self.miny) - print("maxx:", self.maxx) - print("maxy:", self.maxy) + print("min_x:", self.minx) + print("min_y:", self.miny) + print("max_x:", self.maxx) + print("max_y:", self.maxy) self.xwidth = round((self.maxx - self.minx) / self.reso) self.ywidth = round((self.maxy - self.miny) / self.reso) - print("xwidth:", self.xwidth) - print("ywidth:", self.ywidth) + print("x_width:", self.xwidth) + print("y_width:", self.ywidth) # obstacle map generation self.obmap = [[False for _ in range(self.ywidth)] diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index 198ddd2e..01f1a31d 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -23,7 +23,7 @@ class BreadthFirstSearchPlanner: ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ @@ -49,8 +49,8 @@ class BreadthFirstSearchPlanner: Breadth First search based planning input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] @@ -118,7 +118,7 @@ class BreadthFirstSearchPlanner: # generate final course rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ self.calc_grid_position(ngoal.y, self.miny)] - n = closedset[ngoal.pind] + n = closedset[ngoal.parent_index] while n is not None: rx.append(self.calc_grid_position(n.x, self.minx)) ry.append(self.calc_grid_position(n.y, self.miny)) @@ -168,15 +168,15 @@ class BreadthFirstSearchPlanner: self.miny = round(min(oy)) self.maxx = round(max(ox)) self.maxy = round(max(oy)) - print("minx:", self.minx) - print("miny:", self.miny) - print("maxx:", self.maxx) - print("maxy:", self.maxy) + print("min_x:", self.minx) + print("min_y:", self.miny) + print("max_x:", self.maxx) + print("max_y:", self.maxy) self.xwidth = round((self.maxx - self.minx) / self.reso) self.ywidth = round((self.maxy - self.miny) / self.reso) - print("xwidth:", self.xwidth) - print("ywidth:", self.ywidth) + print("x_width:", self.xwidth) + print("y_width:", self.ywidth) # obstacle map generation self.obmap = [[False for _ in range(self.ywidth)] diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index a413a05d..f3984f87 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -40,7 +40,7 @@ def pure_pursuit_control(state, cx, cy, pind): if pind >= ind: ind = pind - # print(pind, ind) + # print(parent_index, ind) if ind < len(cx): tx = cx[ind] ty = cy[ind] @@ -181,12 +181,12 @@ def set_stop_point(target_speed, cx, cy, cyaw): speed_profile[i] = 0.0 forward = False # plt.plot(cx[i], cy[i], "xb") - # print(iyaw, move_direction, dx, dy) + # print(i_yaw, move_direction, dx, dy) elif not is_back and not forward: speed_profile[i] = 0.0 forward = True # plt.plot(cx[i], cy[i], "xb") - # print(iyaw, move_direction, dx, dy) + # print(i_yaw, move_direction, dx, dy) speed_profile[0] = 0.0 if is_back: speed_profile[-1] = -stop_speed diff --git a/PathPlanning/DepthFirstSearch/depth_first_search.py b/PathPlanning/DepthFirstSearch/depth_first_search.py index 229eb70c..c8d2d5ea 100644 --- a/PathPlanning/DepthFirstSearch/depth_first_search.py +++ b/PathPlanning/DepthFirstSearch/depth_first_search.py @@ -23,7 +23,7 @@ class DepthFirstSearchPlanner: ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ @@ -49,8 +49,8 @@ class DepthFirstSearchPlanner: Depth First search input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] @@ -115,7 +115,7 @@ class DepthFirstSearchPlanner: # generate final course rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ self.calc_grid_position(ngoal.y, self.miny)] - n = closedset[ngoal.pind] + n = closedset[ngoal.parent_index] while n is not None: rx.append(self.calc_grid_position(n.x, self.minx)) ry.append(self.calc_grid_position(n.y, self.miny)) @@ -165,15 +165,15 @@ class DepthFirstSearchPlanner: self.miny = round(min(oy)) self.maxx = round(max(ox)) self.maxy = round(max(oy)) - print("minx:", self.minx) - print("miny:", self.miny) - print("maxx:", self.maxx) - print("maxy:", self.maxy) + print("min_x:", self.minx) + print("min_y:", self.miny) + print("max_x:", self.maxx) + print("max_y:", self.maxy) self.xwidth = round((self.maxx - self.minx) / self.reso) self.ywidth = round((self.maxy - self.miny) / self.reso) - print("xwidth:", self.xwidth) - print("ywidth:", self.ywidth) + print("x_width:", self.xwidth) + print("y_width:", self.ywidth) # obstacle map generation self.obmap = [[False for _ in range(self.ywidth)] diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index 0ece712a..b744bc6f 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -53,8 +53,8 @@ class Dijkstra: dijkstra path search input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gx: goal x position [m] diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index b72801a6..9c13198a 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -9,6 +9,7 @@ import math import matplotlib.pyplot as plt import numpy as np +from scipy.spatial.transform import Rotation as Rot show_animation = True @@ -136,7 +137,8 @@ def left_right_left(alpha, beta, d): return t, p, q, mode -def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size): +def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, + step_size): dx = end_x dy = end_y D = math.hypot(dx, dy) @@ -146,7 +148,8 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size alpha = mod2pi(- theta) beta = mod2pi(end_yaw - theta) - planners = [left_straight_left, right_straight_right, left_straight_right, right_straight_left, right_left_right, + planners = [left_straight_left, right_straight_right, left_straight_right, + right_straight_left, right_left_right, left_right_left] best_cost = float("inf") @@ -163,13 +166,14 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size best_cost = cost lengths = [bt, bp, bq] - px, py, pyaw, directions = generate_local_course( + x_list, y_list, yaw_list, directions = generate_local_course( sum(lengths), lengths, best_mode, curvature, step_size) - return px, py, pyaw, best_mode, best_cost + return x_list, y_list, yaw_list, best_mode, best_cost -def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions): +def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, + origin_yaw, path_x, path_y, path_yaw, directions): if mode == "S": path_x[ind] = origin_x + length / max_curvature * math.cos(origin_yaw) path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw) @@ -199,54 +203,49 @@ def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw return path_x, path_y, path_yaw, directions -def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, step_size=0.1): +def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, c, step_size=0.1): """ - Dubins path plannner + Dubins path planner input: - sx x position of start point [m] - sy y position of start point [m] - syaw yaw angle of start point [rad] - ex x position of end point [m] - ey y position of end point [m] - eyaw yaw angle of end point [rad] + s_x x position of start point [m] + s_y y position of start point [m] + s_yaw yaw angle of start point [rad] + g_x x position of end point [m] + g_y y position of end point [m] + g_yaw yaw angle of end point [rad] c curvature [1/m] - output: - px - py - pyaw - mode - """ - ex = ex - sx - ey = ey - sy + g_x = g_x - s_x + g_y = g_y - s_y - lex = math.cos(syaw) * ex + math.sin(syaw) * ey - ley = - math.sin(syaw) * ex + math.cos(syaw) * ey - leyaw = eyaw - syaw + l_rot = Rot.from_euler('z', s_yaw).as_matrix()[0:2, 0:2] + le_xy = np.stack([g_x, g_y]).T @ l_rot + le_yaw = g_yaw - s_yaw - lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( - lex, ley, leyaw, c, step_size) + lp_x, lp_y, lp_yaw, mode, lengths = dubins_path_planning_from_origin( + le_xy[0], le_xy[1], le_yaw, c, step_size) - px = [math.cos(-syaw) * x + math.sin(-syaw) - * y + sx for x, y in zip(lpx, lpy)] - py = [- math.sin(-syaw) * x + math.cos(-syaw) - * y + sy for x, y in zip(lpx, lpy)] - pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] + rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2] + converted_xy = np.stack([lp_x, lp_y]).T @ rot + x_list = converted_xy[:, 0] + s_x + y_list = converted_xy[:, 1] + s_y + yaw_list = [pi_2_pi(i_yaw + s_yaw) for i_yaw in lp_yaw] - return px, py, pyaw, mode, clen + return x_list, y_list, yaw_list, mode, lengths -def generate_local_course(total_length, lengths, mode, max_curvature, step_size): +def generate_local_course(total_length, lengths, mode, max_curvature, + step_size): n_point = math.trunc(total_length / step_size) + len(lengths) + 4 path_x = [0.0 for _ in range(n_point)] path_y = [0.0 for _ in range(n_point)] path_yaw = [0.0 for _ in range(n_point)] directions = [0.0 for _ in range(n_point)] - ind = 1 + index = 1 if lengths[0] > 0.0: directions[0] = 1 @@ -262,25 +261,28 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size) d = -step_size # set origin state - origin_x, origin_y, origin_yaw = path_x[ind], path_y[ind], path_yaw[ind] + origin_x, origin_y, origin_yaw = \ + path_x[index], path_y[index], path_yaw[index] - ind -= 1 + index -= 1 if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: pd = - d - ll else: pd = d - ll while abs(pd) <= abs(l): - ind += 1 + index += 1 path_x, path_y, path_yaw, directions = interpolate( - ind, pd, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) + index, pd, m, max_curvature, origin_x, origin_y, origin_yaw, + path_x, path_y, path_yaw, directions) pd += d ll = l - pd - d # calc remain length - ind += 1 + index += 1 path_x, path_y, path_yaw, directions = interpolate( - ind, l, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) + index, l, m, max_curvature, origin_x, origin_y, origin_yaw, + path_x, path_y, path_yaw, directions) if len(path_x) <= 1: return [], [], [], [] @@ -295,14 +297,15 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size) return path_x, path_y, path_yaw, directions -def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", + ec="k"): # pragma: no cover """ Plot arrow """ if not isinstance(x, float): - for (ix, iy, iyaw) in zip(x, y, yaw): - plot_arrow(ix, iy, iyaw) + for (i_x, i_y, i_yaw) in zip(x, y, yaw): + plot_arrow(i_x, i_y, i_yaw) else: plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc, ec=ec, head_width=width, head_length=width) @@ -322,11 +325,12 @@ def main(): curvature = 1.0 - px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw, - end_x, end_y, end_yaw, curvature) + path_x, path_y, path_yaw, mode, path_length = dubins_path_planning( + start_x, start_y, start_yaw, + end_x, end_y, end_yaw, curvature) if show_animation: - plt.plot(px, py, label="final course " + "".join(mode)) + plt.plot(path_x, path_y, label="final course " + "".join(mode)) # plotting plot_arrow(start_x, start_y, start_yaw) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index b8395e5f..0df40410 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -41,11 +41,11 @@ class Config: # robot parameter self.max_speed = 1.0 # [m/s] self.min_speed = -0.5 # [m/s] - self.max_yawrate = 40.0 * math.pi / 180.0 # [rad/s] + self.max_yaw_rate = 40.0 * math.pi / 180.0 # [rad/s] self.max_accel = 0.2 # [m/ss] - self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss] - self.v_reso = 0.01 # [m/s] - self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s] + self.max_delta_yaw_rate = 40.0 * math.pi / 180.0 # [rad/ss] + self.v_resolution = 0.01 # [m/s] + self.yaw_rate_resolution = 0.1 * math.pi / 180.0 # [rad/s] self.dt = 0.1 # [s] Time tick for motion prediction self.predict_time = 3.0 # [s] self.to_goal_cost_gain = 0.15 @@ -93,15 +93,15 @@ def calc_dynamic_window(x, config): # Dynamic window from robot specification Vs = [config.min_speed, config.max_speed, - -config.max_yawrate, config.max_yawrate] + -config.max_yaw_rate, config.max_yaw_rate] # Dynamic window from motion model Vd = [x[3] - config.max_accel * config.dt, x[3] + config.max_accel * config.dt, - x[4] - config.max_dyawrate * config.dt, - x[4] + config.max_dyawrate * config.dt] + x[4] - config.max_delta_yaw_rate * config.dt, + x[4] + config.max_delta_yaw_rate * config.dt] - # [vmin, vmax, yaw_rate min, yaw_rate max] + # [v_min, v_max, yaw_rate_min, yaw_rate_max] dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]), max(Vs[2], Vd[2]), min(Vs[3], Vd[3])] @@ -114,14 +114,14 @@ def predict_trajectory(x_init, v, y, config): """ x = np.array(x_init) - traj = np.array(x) + trajectory = np.array(x) time = 0 while time <= config.predict_time: x = motion(x, [v, y], config.dt) - traj = np.vstack((traj, x)) + trajectory = np.vstack((trajectory, x)) time += config.dt - return traj + return trajectory def calc_control_and_trajectory(x, dw, config, goal, ob): @@ -135,8 +135,8 @@ def calc_control_and_trajectory(x, dw, config, goal, ob): best_trajectory = np.array([x]) # evaluate all trajectory with sampled input in dynamic window - for v in np.arange(dw[0], dw[1], config.v_reso): - for y in np.arange(dw[2], dw[3], config.yawrate_reso): + for v in np.arange(dw[0], dw[1], config.v_resolution): + for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution): trajectory = predict_trajectory(x_init, v, y, config) @@ -182,7 +182,7 @@ def calc_obstacle_cost(trajectory, ob, config): np.logical_and(bottom_check, left_check))).any(): return float("Inf") elif config.robot_type == RobotType.circle: - if (r <= config.robot_radius).any(): + if np.array(r <= config.robot_radius).any(): return float("Inf") min_r = np.min(r) @@ -269,8 +269,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.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") @@ -296,4 +297,5 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle): if __name__ == '__main__': - main(robot_type=RobotType.circle) + main(robot_type=RobotType.rectangle) + # main(robot_type=RobotType.circle) diff --git a/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py b/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py index c29e37e8..7100b0e6 100644 --- a/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py +++ b/PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py @@ -23,7 +23,7 @@ class BestFirstSearchPlanner: ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ @@ -49,8 +49,8 @@ class BestFirstSearchPlanner: Greedy Best-First search input: - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] @@ -132,7 +132,7 @@ class BestFirstSearchPlanner: # generate final course rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ self.calc_grid_position(ngoal.y, self.miny)] - n = closedset[ngoal.pind] + n = closedset[ngoal.parent_index] while n is not None: rx.append(self.calc_grid_position(n.x, self.minx)) ry.append(self.calc_grid_position(n.y, self.miny)) @@ -188,15 +188,15 @@ class BestFirstSearchPlanner: self.miny = round(min(oy)) self.maxx = round(max(ox)) self.maxy = round(max(oy)) - print("minx:", self.minx) - print("miny:", self.miny) - print("maxx:", self.maxx) - print("maxy:", self.maxy) + print("min_x:", self.minx) + print("min_y:", self.miny) + print("max_x:", self.maxx) + print("max_y:", self.maxy) self.xwidth = round((self.maxx - self.minx) / self.reso) self.ywidth = round((self.maxy - self.miny) / self.reso) - print("xwidth:", self.xwidth) - print("ywidth:", self.ywidth) + print("x_width:", self.xwidth) + print("y_width:", self.ywidth) # obstacle map generation self.obmap = [[False for _ in range(self.ywidth)] diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index 8d9ccd2e..09df21c4 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -11,6 +11,7 @@ from enum import IntEnum import matplotlib.pyplot as plt import numpy as np +from scipy.spatial.transform import Rotation as Rot sys.path.append(os.path.relpath("../../Mapping/grid_map_lib/")) try: @@ -36,52 +37,55 @@ class SweepSearcher: self.sweep_direction = sweep_direction self.turing_window = [] self.update_turning_window() - self.xinds_goaly = x_inds_goal_y - self.goaly = goal_y + self.x_indexes_goal_y = x_inds_goal_y + self.goal_y = goal_y - def move_target_grid(self, cxind, cyind, gmap): - nxind = self.moving_direction + cxind - nyind = cyind + def move_target_grid(self, c_x_index, c_y_index, grid_map): + n_x_index = self.moving_direction + c_x_index + n_y_index = c_y_index # found safe grid - if not gmap.check_occupied_from_xy_index(nxind, nyind, - occupied_val=0.5): - return nxind, nyind + if not grid_map.check_occupied_from_xy_index(n_x_index, n_y_index, + occupied_val=0.5): + return n_x_index, n_y_index else: # occupied - ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap) - if (ncxind is None) and (ncyind is None): + next_c_x_index, next_c_y_index = self.find_safe_turning_grid( + c_x_index, c_y_index, grid_map) + if (next_c_x_index is None) and (next_c_y_index is None): # moving backward - ncxind = -self.moving_direction + cxind - ncyind = cyind - if gmap.check_occupied_from_xy_index(ncxind, ncyind): + next_c_x_index = -self.moving_direction + c_x_index + next_c_y_index = c_y_index + if grid_map.check_occupied_from_xy_index(next_c_x_index, + next_c_y_index): # moved backward, but the grid is occupied by obstacle return None, None else: # keep moving until end - while not gmap.check_occupied_from_xy_index( - ncxind + self.moving_direction, ncyind, - occupied_val=0.5): - ncxind += self.moving_direction + while not grid_map.check_occupied_from_xy_index( + next_c_x_index + self.moving_direction, + next_c_y_index, occupied_val=0.5): + next_c_x_index += self.moving_direction self.swap_moving_direction() - return ncxind, ncyind + return next_c_x_index, next_c_y_index - def find_safe_turning_grid(self, cxind, cyind, gmap): + def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map): for (d_x_ind, d_y_ind) in self.turing_window: - next_x_ind = d_x_ind + cxind - next_y_ind = d_y_ind + cyind + next_x_ind = d_x_ind + c_x_index + next_y_ind = d_y_ind + c_y_index # found safe grid - if not gmap.check_occupied_from_xy_index(next_x_ind, next_y_ind, - occupied_val=0.5): + if not grid_map.check_occupied_from_xy_index(next_x_ind, + next_y_ind, + occupied_val=0.5): return next_x_ind, next_y_ind return None, None def is_search_done(self, grid_map): - for ix in self.xinds_goaly: - if not grid_map.check_occupied_from_xy_index(ix, self.goaly, + for ix in self.x_indexes_goal_y: + if not grid_map.check_occupied_from_xy_index(ix, self.goal_y, occupied_val=0.5): return False @@ -138,64 +142,54 @@ def find_sweep_direction_and_start_position(ox, oy): return vec, sweep_start_pos -def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi): - tx = [ix - sweep_start_posi[0] for ix in ox] - ty = [iy - sweep_start_posi[1] for iy in oy] - +def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position): + tx = [ix - sweep_start_position[0] for ix in ox] + ty = [iy - sweep_start_position[1] for iy in oy] th = math.atan2(sweep_vec[1], sweep_vec[0]) + rot = Rot.from_euler('z', th).as_matrix()[0:2, 0:2] + converted_xy = np.stack([tx, ty]).T @ rot - c = np.cos(-th) - s = np.sin(-th) - - rx = [ix * c - iy * s for (ix, iy) in zip(tx, ty)] - ry = [ix * s + iy * c for (ix, iy) in zip(tx, ty)] - - return rx, ry + return converted_xy[:, 0], converted_xy[:, 1] -def convert_global_coordinate(x, y, sweep_vec, sweep_start_posi): +def convert_global_coordinate(x, y, sweep_vec, sweep_start_position): th = math.atan2(sweep_vec[1], sweep_vec[0]) - c = np.cos(th) - s = np.sin(th) - - tx = [ix * c - iy * s for (ix, iy) in zip(x, y)] - ty = [ix * s + iy * c for (ix, iy) in zip(x, y)] - - rx = [ix + sweep_start_posi[0] for ix in tx] - ry = [iy + sweep_start_posi[1] for iy in ty] - + rot = Rot.from_euler('z', -th).as_matrix()[0:2, 0:2] + converted_xy = np.stack([x, y]).T @ rot + rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]] + ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]] return rx, ry def search_free_grid_index_at_edge_y(grid_map, from_upper=False): - yind = None - xinds = [] + y_index = None + x_indexes = [] if from_upper: - xrange = range(grid_map.height)[::-1] - yrange = range(grid_map.width)[::-1] + x_range = range(grid_map.height)[::-1] + y_range = range(grid_map.width)[::-1] else: - xrange = range(grid_map.height) - yrange = range(grid_map.width) + x_range = range(grid_map.height) + y_range = range(grid_map.width) - for iy in xrange: - for ix in yrange: + for iy in x_range: + for ix in y_range: if not grid_map.check_occupied_from_xy_index(ix, iy): - yind = iy - xinds.append(ix) - if yind: + y_index = iy + x_indexes.append(ix) + if y_index: break - return xinds, yind + return x_indexes, y_index -def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10): - width = math.ceil((max(ox) - min(ox)) / reso) + offset_grid - height = math.ceil((max(oy) - min(oy)) / reso) + offset_grid - center_x = (np.max(ox)+np.min(ox))/2.0 - center_y = (np.max(oy)+np.min(oy))/2.0 +def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10): + width = math.ceil((max(ox) - min(ox)) / resolution) + offset_grid + height = math.ceil((max(oy) - min(oy)) / resolution) + offset_grid + center_x = (np.max(ox) + np.min(ox)) / 2.0 + center_y = (np.max(oy) + np.min(oy)) / 2.0 - grid_map = GridMap(width, height, reso, center_x, center_y) + grid_map = GridMap(width, height, resolution, center_x, center_y) grid_map.print_grid_map_info() grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False) grid_map.expand_grid() @@ -203,48 +197,51 @@ def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10): x_inds_goal_y = [] goal_y = 0 if sweep_direction == SweepSearcher.SweepDirection.UP: - x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map, - from_upper=True) + x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y( + grid_map, from_upper=True) elif sweep_direction == SweepSearcher.SweepDirection.DOWN: - x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map, - from_upper=False) + x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y( + grid_map, from_upper=False) return grid_map, x_inds_goal_y, goal_y def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False): # search start grid - cxind, cyind = sweep_searcher.search_start_grid(grid_map) - if not grid_map.set_value_from_xy_index(cxind, cyind, 0.5): + c_x_index, c_y_index = sweep_searcher.search_start_grid(grid_map) + if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5): print("Cannot find start grid") return [], [] - x, y = grid_map.calc_grid_central_xy_position_from_xy_index(cxind, cyind) + x, y = grid_map.calc_grid_central_xy_position_from_xy_index(c_x_index, + c_y_index) px, py = [x], [y] fig, ax = None, None 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]) + 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, grid_map) + c_x_index, c_y_index = sweep_searcher.move_target_grid(c_x_index, + c_y_index, + grid_map) if sweep_searcher.is_search_done(grid_map) or ( - cxind is None or cyind is None): + c_x_index is None or c_y_index is None): print("Done") break x, y = grid_map.calc_grid_central_xy_position_from_xy_index( - cxind, cyind) + c_x_index, c_y_index) px.append(x) py.append(y) - grid_map.set_value_from_xy_index(cxind, cyind, 0.5) + grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5) if grid_search_animation: grid_map.plot_grid_map(ax=ax) @@ -255,32 +252,34 @@ def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False): return px, py -def planning(ox, oy, reso, +def planning(ox, oy, resolution, moving_direction=SweepSearcher.MovingDirection.RIGHT, sweeping_direction=SweepSearcher.SweepDirection.UP, ): - sweep_vec, sweep_start_posi = find_sweep_direction_and_start_position( + sweep_vec, sweep_start_position = find_sweep_direction_and_start_position( ox, oy) - rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi) + rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, + sweep_start_position) - gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso, - sweeping_direction) + grid_map, x_inds_goal_y, goal_y = setup_grid_map(rox, roy, resolution, + sweeping_direction) sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, - xinds_goaly, goaly) + x_inds_goal_y, goal_y) - px, py = sweep_path_search(sweep_searcher, gmap) + px, py = sweep_path_search(sweep_searcher, grid_map) - rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi) + rx, ry = convert_global_coordinate(px, py, sweep_vec, + sweep_start_position) print("Path length:", len(rx)) return rx, ry -def planning_animation(ox, oy, reso): # pragma: no cover - px, py = planning(ox, oy, reso) +def planning_animation(ox, oy, resolution): # pragma: no cover + px, py = planning(ox, oy, resolution) # animation if do_animation: @@ -311,18 +310,18 @@ def main(): # pragma: no cover ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0] oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0] - reso = 5.0 - planning_animation(ox, oy, reso) + resolution = 5.0 + planning_animation(ox, oy, resolution) ox = [0.0, 50.0, 50.0, 0.0, 0.0] oy = [0.0, 0.0, 30.0, 30.0, 0.0] - reso = 1.3 - planning_animation(ox, oy, reso) + resolution = 1.3 + planning_animation(ox, oy, resolution) ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0] oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0] - reso = 5.0 - planning_animation(ox, oy, reso) + resolution = 5.0 + planning_animation(ox, oy, resolution) plt.show() print("done!!") diff --git a/PathPlanning/HybridAStar/a_star_heuristic.py b/PathPlanning/HybridAStar/a_star_heuristic.py index a2729328..36c33424 100644 --- a/PathPlanning/HybridAStar/a_star_heuristic.py +++ b/PathPlanning/HybridAStar/a_star_heuristic.py @@ -8,8 +8,9 @@ See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) """ -import math import heapq +import math + import matplotlib.pyplot as plt show_animation = False @@ -17,71 +18,73 @@ show_animation = False class Node: - def __init__(self, x, y, cost, pind): + def __init__(self, x, y, cost, parent_index): self.x = x self.y = y self.cost = cost - self.pind = pind + self.parent_index = parent_index def __str__(self): - return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) + return str(self.x) + "," + str(self.y) + "," + str( + self.cost) + "," + str(self.parent_index) -def calc_final_path(ngoal, closedset, reso): +def calc_final_path(goal_node, closed_node_set, resolution): # generate final course - rx, ry = [ngoal.x * reso], [ngoal.y * reso] - pind = ngoal.pind - while pind != -1: - n = closedset[pind] - rx.append(n.x * reso) - ry.append(n.y * reso) - pind = n.pind + rx, ry = [goal_node.x * resolution], [goal_node.y * resolution] + parent_index = goal_node.parent_index + while parent_index != -1: + n = closed_node_set[parent_index] + rx.append(n.x * resolution) + ry.append(n.y * resolution) + parent_index = n.parent_index return rx, ry -def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): +def dp_planning(sx, sy, gx, gy, ox, oy, resolution, rr): """ gx: goal x position [m] gx: goal x position [m] ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - reso: grid resolution [m] + resolution: grid resolution [m] rr: robot radius[m] """ - nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1) - ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) - ox = [iox / reso for iox in ox] - oy = [ioy / reso for ioy in oy] + start_node = Node(round(sx / resolution), round(sy / resolution), 0.0, -1) + goal_node = Node(round(gx / resolution), round(gy / resolution), 0.0, -1) + ox = [iox / resolution for iox in ox] + oy = [ioy / resolution for ioy in oy] - obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr) + obstacle_map, min_x, min_y, max_x, max_y, x_w, y_w = calc_obstacle_map( + ox, oy, resolution, rr) motion = get_motion_model() - openset, closedset = dict(), dict() - openset[calc_index(ngoal, xw, minx, miny)] = ngoal - pq = [] - pq.append((0, calc_index(ngoal, xw, minx, miny))) + open_set, closed_set = dict(), dict() + open_set[calc_index(goal_node, x_w, min_x, min_y)] = goal_node + priority_queue = [(0, calc_index(goal_node, x_w, min_x, min_y))] while 1: - if not pq: + if not priority_queue: break - cost, c_id = heapq.heappop(pq) - if c_id in openset: - current = openset[c_id] - closedset[c_id] = current - openset.pop(c_id) + cost, c_id = heapq.heappop(priority_queue) + if c_id in open_set: + current = open_set[c_id] + closed_set[c_id] = current + open_set.pop(c_id) else: continue # show graph if show_animation: # pragma: no cover - plt.plot(current.x * reso, current.y * reso, "xc") + plt.plot(current.x * resolution, current.y * resolution, "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.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) # Remove the item from the open set @@ -91,82 +94,82 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): node = Node(current.x + motion[i][0], current.y + motion[i][1], current.cost + motion[i][2], c_id) - n_id = calc_index(node, xw, minx, miny) + n_id = calc_index(node, x_w, min_x, min_y) - if n_id in closedset: + if n_id in closed_set: continue - if not verify_node(node, obmap, minx, miny, maxx, maxy): + if not verify_node(node, obstacle_map, min_x, min_y, max_x, max_y): continue - if n_id not in openset: - openset[n_id] = node # Discover a new node + if n_id not in open_set: + open_set[n_id] = node # Discover a new node heapq.heappush( - pq, (node.cost, calc_index(node, xw, minx, miny))) + priority_queue, + (node.cost, calc_index(node, x_w, min_x, min_y))) else: - if openset[n_id].cost >= node.cost: + if open_set[n_id].cost >= node.cost: # This path is the best until now. record it! - openset[n_id] = node + open_set[n_id] = node heapq.heappush( - pq, (node.cost, calc_index(node, xw, minx, miny))) + priority_queue, + (node.cost, calc_index(node, x_w, min_x, min_y))) - rx, ry = calc_final_path(closedset[calc_index( - nstart, xw, minx, miny)], closedset, reso) + rx, ry = calc_final_path(closed_set[calc_index( + start_node, x_w, min_x, min_y)], closed_set, resolution) - return rx, ry, closedset + return rx, ry, closed_set def calc_heuristic(n1, n2): w = 1.0 # weight of heuristic - d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2) + d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2) return d -def verify_node(node, obmap, minx, miny, maxx, maxy): - - if node.x < minx: +def verify_node(node, obstacle_map, min_x, min_y, max_x, max_y): + if node.x < min_x: return False - elif node.y < miny: + elif node.y < min_y: return False - elif node.x >= maxx: + elif node.x >= max_x: return False - elif node.y >= maxy: + elif node.y >= max_y: return False - if obmap[node.x][node.y]: + if obstacle_map[node.x][node.y]: return False return True -def calc_obstacle_map(ox, oy, reso, vr): +def calc_obstacle_map(ox, oy, resolution, vr): + min_x = round(min(ox)) + min_y = round(min(oy)) + max_x = round(max(ox)) + max_y = round(max(oy)) - minx = round(min(ox)) - miny = round(min(oy)) - maxx = round(max(ox)) - maxy = round(max(oy)) - - xwidth = round(maxx - minx) - ywidth = round(maxy - miny) + x_width = round(max_x - min_x) + y_width = round(max_y - min_y) # obstacle map generation - obmap = [[False for i in range(ywidth)] for i in range(xwidth)] - for ix in range(xwidth): - x = ix + minx - for iy in range(ywidth): - y = iy + miny + obstacle_map = [[False for _ in range(y_width)] for _ in range(x_width)] + for ix in range(x_width): + x = ix + min_x + for iy in range(y_width): + y = iy + min_y # print(x, y) for iox, ioy in zip(ox, oy): - d = math.sqrt((iox - x)**2 + (ioy - y)**2) - if d <= vr / reso: - obmap[ix][iy] = True + d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2) + if d <= vr / resolution: + obstacle_map[ix][iy] = True break - return obmap, minx, miny, maxx, maxy, xwidth, ywidth + return obstacle_map, min_x, min_y, max_x, max_y, x_width, y_width -def calc_index(node, xwidth, xmin, ymin): - return (node.y - ymin) * xwidth + (node.x - xmin) +def calc_index(node, x_width, x_min, y_min): + return (node.y - y_min) * x_width + (node.x - x_min) def get_motion_model(): diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py index 13e50b29..9b956a08 100644 --- a/PathPlanning/HybridAStar/car.py +++ b/PathPlanning/HybridAStar/car.py @@ -6,9 +6,11 @@ author: Zheng Zh (@Zhengzh) """ +from math import sqrt, cos, sin, tan, pi import matplotlib.pyplot as plt -from math import sqrt, cos, sin, tan, pi +import numpy as np +from scipy.spatial.transform import Rotation as Rot WB = 3. # rear to front wheel W = 2. # width of car @@ -16,25 +18,25 @@ LF = 3.3 # distance from rear to vehicle front end LB = 1.0 # distance from rear to vehicle back end MAX_STEER = 0.6 # [rad] maximum steering angle -WBUBBLE_DIST = (LF - LB) / 2.0 -WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1) +W_BUBBLE_DIST = (LF - LB) / 2.0 +W_BUBBLE_R = sqrt(((LF + LB) / 2.0) ** 2 + 1) -# vehicle rectangle verticles +# vehicle rectangle vertices VRX = [LF, LF, -LB, -LB, LF] VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2] -def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): - for x, y, yaw in zip(xlist, ylist, yawlist): - cx = x + WBUBBLE_DIST * cos(yaw) - cy = y + WBUBBLE_DIST * sin(yaw) +def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): + for i_x, i_y, i_yaw in zip(x_list, y_list, yaw_list): + cx = i_x + W_BUBBLE_DIST * cos(i_yaw) + cy = i_y + W_BUBBLE_DIST * sin(i_yaw) - ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R) + ids = kd_tree.search_in_distance([cx, cy], W_BUBBLE_R) if not ids: continue - if not rectangle_check(x, y, yaw, + if not rectangle_check(i_x, i_y, i_yaw, [ox[i] for i in ids], [oy[i] for i in ids]): return False # collision @@ -43,12 +45,12 @@ def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): def rectangle_check(x, y, yaw, ox, oy): # transform obstacles to base link frame - c, s = cos(-yaw), sin(-yaw) + rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2] for iox, ioy in zip(ox, oy): tx = iox - x ty = ioy - y - rx = c * tx - s * ty - ry = s * tx + c * ty + converted_xy = np.stack([tx, ty]).T @ rot + rx, ry = converted_xy[0], converted_xy[1] if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0): return False # no collision @@ -59,24 +61,22 @@ def rectangle_check(x, y, yaw, ox, oy): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): """Plot arrow.""" if not isinstance(x, float): - for (ix, iy, iyaw) in zip(x, y, yaw): - plot_arrow(ix, iy, iyaw) + for (i_x, i_y, i_yaw) in zip(x, y, yaw): + plot_arrow(i_x, i_y, i_yaw) else: plt.arrow(x, y, length * cos(yaw), length * sin(yaw), fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4) - # plt.plot(x, y) def plot_car(x, y, yaw): car_color = '-k' c, s = cos(yaw), sin(yaw) - + rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2] car_outline_x, car_outline_y = [], [] for rx, ry in zip(VRX, VRY): - tx = c * rx - s * ry + x - ty = s * rx + c * ry + y - car_outline_x.append(tx) - car_outline_y.append(ty) + converted_xy = np.stack([rx, ry]).T @ rot + car_outline_x.append(converted_xy[0]+x) + car_outline_y.append(converted_xy[1]+y) arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw plot_arrow(arrow_x, arrow_y, arrow_yaw) @@ -96,8 +96,12 @@ def move(x, y, yaw, distance, steer, L=WB): return x, y, yaw -if __name__ == '__main__': +def main(): x, y, yaw = 0., 0., 1. plt.axis('equal') plot_car(x, y, yaw) - plt.show() \ No newline at end of file + plt.show() + + +if __name__ == '__main__': + main() diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 14e4ac5a..ff9b3fe4 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -7,28 +7,27 @@ author: Zheng Zh (@Zhengzh) """ import heapq -import scipy.spatial -import numpy as np import math -import matplotlib.pyplot as plt -import sys import os +import sys + +import matplotlib.pyplot as plt +import numpy as np +import scipy.spatial sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../ReedsSheppPath") try: - from a_star_heuristic import dp_planning # , calc_obstacle_map + from a_star_heuristic import dp_planning import reeds_shepp_path_planning as rs from car import move, check_car_collision, MAX_STEER, WB, plot_car except Exception: raise - XY_GRID_RESOLUTION = 2.0 # [m] YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad] -MOTION_RESOLUTION = 0.1 # [m] path interporate resolution +MOTION_RESOLUTION = 0.1 # [m] path interpolate resolution N_STEER = 20 # number of steer command -H_COST = 1.0 VR = 1.0 # robot radius SB_COST = 100.0 # switch back penalty cost @@ -42,29 +41,29 @@ show_animation = True class Node: - def __init__(self, xind, yind, yawind, direction, - xlist, ylist, yawlist, directions, - steer=0.0, pind=None, cost=None): - self.xind = xind - self.yind = yind - self.yawind = yawind + def __init__(self, x_ind, y_ind, yaw_ind, direction, + x_list, y_list, yaw_list, directions, + steer=0.0, parent_index=None, cost=None): + self.x_index = x_ind + self.y_index = y_ind + self.yaw_index = yaw_ind self.direction = direction - self.xlist = xlist - self.ylist = ylist - self.yawlist = yawlist + self.x_list = x_list + self.y_list = y_list + self.yaw_list = yaw_list self.directions = directions self.steer = steer - self.pind = pind + self.parent_index = parent_index self.cost = cost class Path: - def __init__(self, xlist, ylist, yawlist, directionlist, cost): - self.xlist = xlist - self.ylist = ylist - self.yawlist = yawlist - self.directionlist = directionlist + def __init__(self, x_list, y_list, yaw_list, direction_list, cost): + self.x_list = x_list + self.y_list = y_list + self.yaw_list = yaw_list + self.direction_list = direction_list self.cost = cost @@ -88,9 +87,9 @@ class KDTree: dist = [] for i in inp.T: - idist, iindex = self.tree.query(i, k=k) - index.append(iindex) - dist.append(idist) + i_dist, i_index = self.tree.query(i, k=k) + index.append(i_index) + dist.append(i_dist) return index, dist @@ -108,7 +107,7 @@ class KDTree: class Config: - def __init__(self, ox, oy, xyreso, yawreso): + def __init__(self, ox, oy, xy_resolution, yaw_resolution): min_x_m = min(ox) min_y_m = min(oy) max_x_m = max(ox) @@ -119,94 +118,93 @@ class Config: ox.append(max_x_m) oy.append(max_y_m) - self.minx = round(min_x_m / xyreso) - self.miny = round(min_y_m / xyreso) - self.maxx = round(max_x_m / xyreso) - self.maxy = round(max_y_m / xyreso) + self.min_x = round(min_x_m / xy_resolution) + self.min_y = round(min_y_m / xy_resolution) + self.max_x = round(max_x_m / xy_resolution) + self.max_y = round(max_y_m / xy_resolution) - self.xw = round(self.maxx - self.minx) - self.yw = round(self.maxy - self.miny) + self.x_w = round(self.max_x - self.min_x) + self.y_w = round(self.max_y - self.min_y) - self.minyaw = round(- math.pi / yawreso) - 1 - self.maxyaw = round(math.pi / yawreso) - self.yaww = round(self.maxyaw - self.minyaw) + self.min_yaw = round(- math.pi / yaw_resolution) - 1 + self.max_yaw = round(math.pi / yaw_resolution) + self.yaw_w = round(self.max_yaw - self.min_yaw) def calc_motion_inputs(): - for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER, N_STEER), [0.0])): for d in [1, -1]: yield [steer, d] -def get_neighbors(current, config, ox, oy, kdtree): - +def get_neighbors(current, config, ox, oy, kd_tree): for steer, d in calc_motion_inputs(): - node = calc_next_node(current, steer, d, config, ox, oy, kdtree) + node = calc_next_node(current, steer, d, config, ox, oy, kd_tree) if node and verify_index(node, config): yield node -def calc_next_node(current, steer, direction, config, ox, oy, kdtree): - - x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1] +def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): + x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] arc_l = XY_GRID_RESOLUTION * 1.5 - xlist, ylist, yawlist = [], [], [] + x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) - xlist.append(x) - ylist.append(y) - yawlist.append(yaw) + x_list.append(x) + y_list.append(y) + yaw_list.append(yaw) - if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): + if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 - xind = round(x / XY_GRID_RESOLUTION) - yind = round(y / XY_GRID_RESOLUTION) - yawind = round(yaw / YAW_GRID_RESOLUTION) + x_ind = round(x / XY_GRID_RESOLUTION) + y_ind = round(y / XY_GRID_RESOLUTION) + yaw_ind = round(yaw / YAW_GRID_RESOLUTION) - addedcost = 0.0 + added_cost = 0.0 if d != current.direction: - addedcost += SB_COST + added_cost += SB_COST # steer penalty - addedcost += STEER_COST * abs(steer) + added_cost += STEER_COST * abs(steer) # steer change penalty - addedcost += STEER_CHANGE_COST * abs(current.steer - steer) + added_cost += STEER_CHANGE_COST * abs(current.steer - steer) - cost = current.cost + addedcost + arc_l + cost = current.cost + added_cost + arc_l - node = Node(xind, yind, yawind, d, xlist, - ylist, yawlist, [d], - pind=calc_index(current, config), + node = Node(x_ind, y_ind, yaw_ind, d, x_list, + y_list, yaw_list, [d], + parent_index=calc_index(current, config), cost=cost, steer=steer) return node def is_same_grid(n1, n2): - if n1.xind == n2.xind and n1.yind == n2.yind and n1.yawind == n2.yawind: + if n1.x_index == n2.x_index \ + and n1.y_index == n2.y_index \ + and n1.yaw_index == n2.yaw_index: return True return False -def analytic_expantion(current, goal, c, ox, oy, kdtree): +def analytic_expansion(current, goal, ox, oy, kd_tree): + start_x = current.x_list[-1] + start_y = current.y_list[-1] + start_yaw = current.yaw_list[-1] - sx = current.xlist[-1] - sy = current.ylist[-1] - syaw = current.yawlist[-1] - - gx = goal.xlist[-1] - gy = goal.ylist[-1] - gyaw = goal.yawlist[-1] + goal_x = goal.x_list[-1] + goal_y = goal.y_list[-1] + goal_yaw = goal.yaw_list[-1] max_curvature = math.tan(MAX_STEER) / WB - paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw, + paths = rs.calc_paths(start_x, start_y, start_yaw, + goal_x, goal_y, goal_yaw, max_curvature, step_size=MOTION_RESOLUTION) if not paths: @@ -215,7 +213,7 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree): best_path, best = None, None for path in paths: - if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree): + if check_car_collision(path.x, path.y, path.yaw, ox, oy, kd_tree): cost = calc_rs_path_cost(path) if not best or best > cost: best = cost @@ -224,99 +222,105 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree): return best_path -def update_node_with_analystic_expantion(current, goal, - c, ox, oy, kdtree): - apath = analytic_expantion(current, goal, c, ox, oy, kdtree) +def update_node_with_analytic_expansion(current, goal, + c, ox, oy, kd_tree): + path = analytic_expansion(current, goal, ox, oy, kd_tree) - if apath: + if path: if show_animation: - plt.plot(apath.x, apath.y) - fx = apath.x[1:] - fy = apath.y[1:] - fyaw = apath.yaw[1:] + plt.plot(path.x, path.y) + f_x = path.x[1:] + f_y = path.y[1:] + f_yaw = path.yaw[1:] - fcost = current.cost + calc_rs_path_cost(apath) - fpind = calc_index(current, c) + f_cost = current.cost + calc_rs_path_cost(path) + f_parent_index = calc_index(current, c) fd = [] - for d in apath.directions[1:]: + for d in path.directions[1:]: fd.append(d >= 0) - fsteer = 0.0 - fpath = Node(current.xind, current.yind, current.yawind, - current.direction, fx, fy, fyaw, fd, - cost=fcost, pind=fpind, steer=fsteer) - return True, fpath + f_steer = 0.0 + f_path = Node(current.x_index, current.y_index, current.yaw_index, + current.direction, f_x, f_y, f_yaw, fd, + cost=f_cost, parent_index=f_parent_index, steer=f_steer) + return True, f_path return False, None -def calc_rs_path_cost(rspath): - +def calc_rs_path_cost(reed_shepp_path): cost = 0.0 - for l in rspath.lengths: - if l >= 0: # forward - cost += l + for length in reed_shepp_path.lengths: + if length >= 0: # forward + cost += length else: # back - cost += abs(l) * BACK_COST + cost += abs(length) * BACK_COST - # swich back penalty - for i in range(len(rspath.lengths) - 1): - if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: # switch back + # switch back penalty + for i in range(len(reed_shepp_path.lengths) - 1): + # switch back + if reed_shepp_path.lengths[i] * reed_shepp_path.lengths[i + 1] < 0.0: cost += SB_COST - # steer penalyty - for ctype in rspath.ctypes: - if ctype != "S": # curve + # steer penalty + for course_type in reed_shepp_path.ctypes: + if course_type != "S": # curve cost += STEER_COST * abs(MAX_STEER) # ==steer change penalty # calc steer profile - nctypes = len(rspath.ctypes) - ulist = [0.0] * nctypes - for i in range(nctypes): - if rspath.ctypes[i] == "R": - ulist[i] = - MAX_STEER - elif rspath.ctypes[i] == "L": - ulist[i] = MAX_STEER + n_ctypes = len(reed_shepp_path.ctypes) + u_list = [0.0] * n_ctypes + for i in range(n_ctypes): + if reed_shepp_path.ctypes[i] == "R": + u_list[i] = - MAX_STEER + elif reed_shepp_path.ctypes[i] == "L": + u_list[i] = MAX_STEER - for i in range(len(rspath.ctypes) - 1): - cost += STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i]) + for i in range(len(reed_shepp_path.ctypes) - 1): + cost += STEER_CHANGE_COST * abs(u_list[i + 1] - u_list[i]) return cost -def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): +def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution): """ - start - goal + start: start node + goal: goal node ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] - xyreso: grid resolution [m] - yawreso: yaw angle resolution [rad] + xy_resolution: grid resolution [m] + yaw_resolution: yaw angle resolution [rad] """ start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2]) tox, toy = ox[:], oy[:] - obkdtree = KDTree(np.vstack((tox, toy)).T) + obstacle_kd_tree = KDTree(np.vstack((tox, toy)).T) - config = Config(tox, toy, xyreso, yawreso) + config = Config(tox, toy, xy_resolution, yaw_resolution) - nstart = Node(round(start[0] / xyreso), round(start[1] / xyreso), round(start[2] / yawreso), - True, [start[0]], [start[1]], [start[2]], [True], cost=0) - ngoal = Node(round(goal[0] / xyreso), round(goal[1] / xyreso), round(goal[2] / yawreso), - True, [goal[0]], [goal[1]], [goal[2]], [True]) + start_node = Node(round(start[0] / xy_resolution), + round(start[1] / xy_resolution), + round(start[2] / yaw_resolution), True, + [start[0]], [start[1]], [start[2]], [True], cost=0) + goal_node = Node(round(goal[0] / xy_resolution), + round(goal[1] / xy_resolution), + round(goal[2] / yaw_resolution), True, + [goal[0]], [goal[1]], [goal[2]], [True]) openList, closedList = {}, {} - _, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1], - ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR) + _, _, h_dp = dp_planning(start_node.x_list[-1], start_node.y_list[-1], + goal_node.x_list[-1], goal_node.y_list[-1], + ox, oy, xy_resolution, VR) pq = [] - openList[calc_index(nstart, config)] = nstart - heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config), - calc_index(nstart, config))) + openList[calc_index(start_node, config)] = start_node + heapq.heappush(pq, (calc_cost(start_node, h_dp, config), + calc_index(start_node, config))) + final_path = None while True: if not openList: @@ -331,83 +335,85 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): continue if show_animation: # pragma: no cover - plt.plot(current.xlist[-1], current.ylist[-1], "xc") + plt.plot(current.x_list[-1], current.y_list[-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]) + 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) - isupdated, fpath = update_node_with_analystic_expantion( - current, ngoal, config, ox, oy, obkdtree) + is_updated, final_path = update_node_with_analytic_expansion( + current, goal_node, config, ox, oy, obstacle_kd_tree) - if isupdated: + if is_updated: print("path found") break - for neighbor in get_neighbors(current, config, ox, oy, obkdtree): + for neighbor in get_neighbors(current, config, ox, oy, + obstacle_kd_tree): neighbor_index = calc_index(neighbor, config) if neighbor_index in closedList: continue if neighbor not in openList \ or openList[neighbor_index].cost > neighbor.cost: heapq.heappush( - pq, (calc_cost(neighbor, h_dp, ngoal, config), + pq, (calc_cost(neighbor, h_dp, config), neighbor_index)) openList[neighbor_index] = neighbor - path = get_final_path(closedList, fpath, nstart, config) + path = get_final_path(closedList, final_path) return path -def calc_cost(n, h_dp, goal, c): - ind = (n.yind - c.miny) * c.xw + (n.xind - c.minx) +def calc_cost(n, h_dp, c): + ind = (n.y_index - c.min_y) * c.x_w + (n.x_index - c.min_x) if ind not in h_dp: return n.cost + 999999999 # collision cost return n.cost + H_COST * h_dp[ind].cost -def get_final_path(closed, ngoal, nstart, config): - rx, ry, ryaw = list(reversed(ngoal.xlist)), list( - reversed(ngoal.ylist)), list(reversed(ngoal.yawlist)) - direction = list(reversed(ngoal.directions)) - nid = ngoal.pind - finalcost = ngoal.cost +def get_final_path(closed, goal_node): + reversed_x, reversed_y, reversed_yaw = \ + list(reversed(goal_node.x_list)), list(reversed(goal_node.y_list)), \ + list(reversed(goal_node.yaw_list)) + direction = list(reversed(goal_node.directions)) + nid = goal_node.parent_index + final_cost = goal_node.cost while nid: n = closed[nid] - rx.extend(list(reversed(n.xlist))) - ry.extend(list(reversed(n.ylist))) - ryaw.extend(list(reversed(n.yawlist))) + reversed_x.extend(list(reversed(n.x_list))) + reversed_y.extend(list(reversed(n.y_list))) + reversed_yaw.extend(list(reversed(n.yaw_list))) direction.extend(list(reversed(n.directions))) - nid = n.pind + nid = n.parent_index - rx = list(reversed(rx)) - ry = list(reversed(ry)) - ryaw = list(reversed(ryaw)) + reversed_x = list(reversed(reversed_x)) + reversed_y = list(reversed(reversed_y)) + reversed_yaw = list(reversed(reversed_yaw)) direction = list(reversed(direction)) # adjust first direction direction[0] = direction[1] - path = Path(rx, ry, ryaw, direction, finalcost) + path = Path(reversed_x, reversed_y, reversed_yaw, direction, final_cost) return path def verify_index(node, c): - xind, yind = node.xind, node.yind - if xind >= c.minx and xind <= c.maxx and yind >= c.miny \ - and yind <= c.maxy: + x_ind, y_ind = node.x_index, node.y_index + if c.min_x <= x_ind <= c.max_x and c.min_y <= y_ind <= c.max_y: return True return False def calc_index(node, c): - ind = (node.yawind - c.minyaw) * c.xw * c.yw + \ - (node.yind - c.miny) * c.xw + (node.xind - c.minx) + ind = (node.yaw_index - c.min_yaw) * c.x_w * c.y_w + \ + (node.y_index - c.min_y) * c.x_w + (node.x_index - c.min_x) if ind <= 0: print("Error(calc_index):", ind) @@ -457,18 +463,18 @@ def main(): path = hybrid_a_star_planning( start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION) - x = path.xlist - y = path.ylist - yaw = path.yawlist + x = path.x_list + y = path.y_list + yaw = path.yaw_list if show_animation: - for ix, iy, iyaw in zip(x, y, yaw): + for i_x, i_y, i_yaw in zip(x, y, yaw): plt.cla() plt.plot(ox, oy, ".k") plt.plot(x, y, "-r", label="Hybrid A* path") plt.grid(True) plt.axis("equal") - plot_car(ix, iy, iyaw) + plot_car(i_x, i_y, i_yaw) plt.pause(0.0001) print(__file__ + " done!!") diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 912e56fa..baf7dcce 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -5,7 +5,8 @@ author: Karan Chawla Atsushi Sakai(@Atsushi_twi) Reference: Informed RRT*: Optimal Sampling-based Path planning Focused via -Direct Sampling of an Admissible Ellipsoidal Heuristichttps://arxiv.org/pdf/1404.2334.pdf +Direct Sampling of an Admissible Ellipsoidal Heuristic +https://arxiv.org/pdf/1404.2334.pdf """ @@ -14,6 +15,7 @@ import math import random import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation as Rot import numpy as np show_animation = True @@ -38,7 +40,8 @@ class InformedRRTStar: def informed_rrt_star_search(self, animation=True): self.node_list = [self.start] - # max length we expect to find in our 'informed' sample space, starts as infinite + # max length we expect to find in our 'informed' sample space, + # starts as infinite cBest = float('inf') solutionSet = set() path = None @@ -51,13 +54,14 @@ class InformedRRTStar: a1 = np.array([[(self.goal.x - self.start.x) / cMin], [(self.goal.y - self.start.y) / cMin], [0]]) - etheta = math.atan2(a1[1], a1[0]) - # first column of idenity matrix transposed + e_theta = math.atan2(a1[1], a1[0]) + # first column of identity matrix transposed id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3) M = a1 @ id1_t U, S, Vh = np.linalg.svd(M, True, True) C = np.dot(np.dot(U, np.diag( - [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) + [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), + Vh) for i in range(self.max_iter): # Sample space is defined by cBest @@ -66,11 +70,11 @@ class InformedRRTStar: # cBest changes when a new path is found rnd = self.informed_sample(cBest, cMin, xCenter, C) - nind = self.get_nearest_list_index(self.node_list, rnd) - nearestNode = self.node_list[nind] + n_ind = self.get_nearest_list_index(self.node_list, rnd) + nearestNode = self.node_list[n_ind] # steer theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) - newNode = self.get_new_node(theta, nind, nearestNode) + newNode = self.get_new_node(theta, n_ind, nearestNode) d = self.line_cost(nearestNode, newNode) noCollision = self.check_collision(nearestNode, theta, d) @@ -83,7 +87,8 @@ class InformedRRTStar: self.rewire(newNode, nearInds) if self.is_near_goal(newNode): - if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y): + if self.check_segment_collision(newNode.x, newNode.y, + self.goal.x, self.goal.y): solutionSet.add(newNode) lastIndex = len(self.node_list) - 1 tempPath = self.get_final_course(lastIndex) @@ -94,7 +99,7 @@ class InformedRRTStar: if animation: self.draw_graph(xCenter=xCenter, cBest=cBest, cMin=cMin, - etheta=etheta, rnd=rnd) + e_theta=e_theta, rnd=rnd) return path @@ -117,7 +122,7 @@ class InformedRRTStar: minInd = nearInds[dList.index(minCost)] if minCost == float('inf'): - print("mincost is inf") + print("min cost is inf") return newNode newNode.cost = minCost @@ -126,12 +131,12 @@ class InformedRRTStar: return newNode def find_near_nodes(self, newNode): - nnode = len(self.node_list) - r = 50.0 * math.sqrt((math.log(nnode) / nnode)) - dlist = [(node.x - newNode.x) ** 2 - + (node.y - newNode.y) ** 2 for node in self.node_list] - nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] - return nearinds + n_node = len(self.node_list) + r = 50.0 * math.sqrt((math.log(n_node) / n_node)) + d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 + for node in self.node_list] + near_inds = [d_list.index(i) for i in d_list if i <= r ** 2] + return near_inds def informed_sample(self, cMax, cMin, xCenter, C): if cMax < float('inf'): @@ -192,14 +197,14 @@ class InformedRRTStar: minIndex = dList.index(min(dList)) return minIndex - def get_new_node(self, theta, nind, nearestNode): + def get_new_node(self, theta, n_ind, nearestNode): newNode = copy.deepcopy(nearestNode) newNode.x += self.expand_dis * math.cos(theta) newNode.y += self.expand_dis * math.sin(theta) newNode.cost += self.expand_dis - newNode.parent = nind + newNode.parent = n_ind return newNode def is_near_goal(self, node): @@ -216,28 +221,29 @@ class InformedRRTStar: d = math.sqrt((nearNode.x - newNode.x) ** 2 + (nearNode.y - newNode.y) ** 2) - scost = newNode.cost + d + s_cost = newNode.cost + d - if nearNode.cost > scost: + if nearNode.cost > s_cost: theta = math.atan2(newNode.y - nearNode.y, newNode.x - nearNode.x) if self.check_collision(nearNode, theta, d): nearNode.parent = n_node - 1 - nearNode.cost = scost + nearNode.cost = s_cost @staticmethod def distance_squared_point_to_segment(v, w, p): # Return minimum distance between line segment vw and point p - if (np.array_equal(v, w)): - return (p-v).dot(p-v) # v == w case - l2 = (w-v).dot(w-v) # i.e. |w-v|^2 - avoid a sqrt - # Consider the line extending the segment, parameterized as v + t (w - v). + if np.array_equal(v, w): + return (p - v).dot(p - v) # v == w case + l2 = (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt + # Consider the line extending the segment, + # parameterized as v + t (w - v). # We find projection of point p onto the line. # It falls where t = [(p-v) . (w-v)] / |w-v|^2 # We clamp t from [0,1] to handle points outside the segment vw. t = max(0, min(1, (p - v).dot(w - v) / l2)) - projection = v + t * (w - v) # Projection falls on the segment - return (p-projection).dot(p-projection) + projection = v + t * (w - v) # Projection falls on the segment + return (p - projection).dot(p - projection) def check_segment_collision(self, x1, y1, x2, y2): for (ox, oy, size) in self.obstacle_list: @@ -245,16 +251,15 @@ class InformedRRTStar: np.array([x1, y1]), np.array([x2, y2]), np.array([ox, oy])) - if dd <= size**2: + if dd <= size ** 2: return False # collision return True - def check_collision(self, nearNode, theta, d): tmpNode = copy.deepcopy(nearNode) - endx = tmpNode.x + math.cos(theta)*d - endy = tmpNode.y + math.sin(theta)*d - return self.check_segment_collision(tmpNode.x, tmpNode.y, endx, endy) + end_x = tmpNode.x + math.cos(theta) * d + end_y = tmpNode.y + math.sin(theta) * d + return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y) def get_final_course(self, lastIndex): path = [[self.goal.x, self.goal.y]] @@ -265,15 +270,17 @@ class InformedRRTStar: path.append([self.start.x, self.start.y]) return path - def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None): + def draw_graph(self, xCenter=None, cBest=None, cMin=None, e_theta=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]) + 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'): - self.plot_ellipse(xCenter, cBest, cMin, etheta) + self.plot_ellipse(xCenter, cBest, cMin, e_theta) for node in self.node_list: if node.parent is not None: @@ -291,20 +298,18 @@ class InformedRRTStar: plt.pause(0.01) @staticmethod - def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover + def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0 b = cBest / 2.0 - angle = math.pi / 2.0 - etheta + angle = math.pi / 2.0 - e_theta cx = xCenter[0] cy = xCenter[1] - t = np.arange(0, 2 * math.pi + 0.1, 0.1) x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] - R = np.array([[math.cos(angle), math.sin(angle)], - [-math.sin(angle), math.cos(angle)]]) - fx = R @ np.array([x, y]) + rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2] + fx = rot @ np.array([x, y]) px = np.array(fx[0, :] + cx).flatten() py = np.array(fx[1, :] + cy).flatten() plt.plot(cx, cy, "xc") diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 7715174e..790db074 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -1,6 +1,6 @@ """ -Probablistic Road Map (PRM) Planner +Probabilistic Road Map (PRM) Planner author: Atsushi Sakai (@Atsushi_twi) @@ -25,14 +25,15 @@ class Node: Node class for dijkstra search """ - def __init__(self, x, y, cost, pind): + def __init__(self, x, y, cost, parent_index): self.x = x self.y = y self.cost = cost - self.pind = pind + self.parent_index = parent_index def __str__(self): - return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) + return str(self.x) + "," + str(self.y) + "," +\ + str(self.cost) + "," + str(self.parent_index) class KDTree: @@ -57,9 +58,9 @@ class KDTree: dist = [] for i in inp.T: - idist, iindex = self.tree.query(i, k=k) - index.append(iindex) - dist.append(idist) + i_dist, i_index = self.tree.query(i, k=k) + index.append(i_index) + dist.append(i_dist) return index, dist @@ -75,23 +76,24 @@ class KDTree: return index -def PRM_planning(sx, sy, gx, gy, ox, oy, rr): +def prm_planning(sx, sy, gx, gy, ox, oy, rr): - obkdtree = KDTree(np.vstack((ox, oy)).T) + obstacle_kd_tree = KDTree(np.vstack((ox, oy)).T) - sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree) + sample_x, sample_y = sample_points(sx, sy, gx, gy, + rr, ox, oy, obstacle_kd_tree) if show_animation: plt.plot(sample_x, sample_y, ".b") - road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree) + road_map = generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree) rx, ry = dijkstra_planning( - sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y) + sx, sy, gx, gy, road_map, sample_x, sample_y) return rx, ry -def is_collision(sx, sy, gx, gy, rr, okdtree): +def is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree): x = sx y = sy dx = gx - sx @@ -103,41 +105,41 @@ def is_collision(sx, sy, gx, gy, rr, okdtree): return True D = rr - nstep = round(d / D) + n_step = round(d / D) - for i in range(nstep): - idxs, dist = okdtree.search(np.array([x, y]).reshape(2, 1)) + for i in range(n_step): + _, dist = obstacle_kd_tree.search(np.array([x, y]).reshape(2, 1)) if dist[0] <= rr: return True # collision x += D * math.cos(yaw) y += D * math.sin(yaw) # goal point check - idxs, dist = okdtree.search(np.array([gx, gy]).reshape(2, 1)) + _, dist = obstacle_kd_tree.search(np.array([gx, gy]).reshape(2, 1)) if dist[0] <= rr: return True # collision return False # OK -def generate_roadmap(sample_x, sample_y, rr, obkdtree): +def generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree): """ Road map generation sample_x: [m] x positions of sampled points sample_y: [m] y positions of sampled points rr: Robot Radius[m] - obkdtree: KDTree object of obstacles + obstacle_kd_tree: KDTree object of obstacles """ road_map = [] - nsample = len(sample_x) - skdtree = KDTree(np.vstack((sample_x, sample_y)).T) + n_sample = len(sample_x) + sample_kd_tree = KDTree(np.vstack((sample_x, sample_y)).T) - for (i, ix, iy) in zip(range(nsample), sample_x, sample_y): + for (i, ix, iy) in zip(range(n_sample), sample_x, sample_y): - index, dists = skdtree.search( - np.array([ix, iy]).reshape(2, 1), k=nsample) + index, dists = sample_kd_tree.search( + np.array([ix, iy]).reshape(2, 1), k=n_sample) inds = index[0] edge_id = [] # print(index) @@ -146,7 +148,7 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree): nx = sample_x[inds[ii]] ny = sample_y[inds[ii]] - if not is_collision(ix, iy, nx, ny, rr, obkdtree): + if not is_collision(ix, iy, nx, ny, rr, obstacle_kd_tree): edge_id.append(inds[ii]) if len(edge_id) >= N_KNN: @@ -159,10 +161,10 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree): return road_map -def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): +def dijkstra_planning(sx, sy, gx, gy, road_map, sample_x, sample_y): """ - sx: start x position [m] - sy: start y position [m] + s_x: start x position [m] + s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] ox: x position list of Obstacles [m] @@ -175,41 +177,42 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): @return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found """ - nstart = Node(sx, sy, 0.0, -1) - ngoal = Node(gx, gy, 0.0, -1) + start_node = Node(sx, sy, 0.0, -1) + goal_node = Node(gx, gy, 0.0, -1) - openset, closedset = dict(), dict() - openset[len(road_map) - 2] = nstart + open_set, closed_set = dict(), dict() + open_set[len(road_map) - 2] = start_node path_found = True while True: - if not openset: + if not open_set: print("Cannot find path") path_found = False break - c_id = min(openset, key=lambda o: openset[o].cost) - current = openset[c_id] + c_id = min(open_set, key=lambda o: open_set[o].cost) + current = open_set[c_id] # show graph - if show_animation and len(closedset.keys()) % 2 == 0: + if show_animation and len(closed_set.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.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) if c_id == (len(road_map) - 1): print("goal is found!") - ngoal.pind = current.pind - ngoal.cost = current.cost + goal_node.parent_index = current.parent_index + goal_node.cost = current.cost break # Remove the item from the open set - del openset[c_id] + del open_set[c_id] # Add it to the closed set - closedset[c_id] = current + closed_set[c_id] = current # expand search grid based on motion model for i in range(len(road_map[c_id])): @@ -220,27 +223,27 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): node = Node(sample_x[n_id], sample_y[n_id], current.cost + d, c_id) - if n_id in closedset: + if n_id in closed_set: continue # Otherwise if it is already in the open set - if n_id in openset: - if openset[n_id].cost > node.cost: - openset[n_id].cost = node.cost - openset[n_id].pind = c_id + if n_id in open_set: + if open_set[n_id].cost > node.cost: + open_set[n_id].cost = node.cost + open_set[n_id].parent_index = c_id else: - openset[n_id] = node + open_set[n_id] = node if path_found is False: return [], [] # generate final course - rx, ry = [ngoal.x], [ngoal.y] - pind = ngoal.pind - while pind != -1: - n = closedset[pind] + rx, ry = [goal_node.x], [goal_node.y] + parent_index = goal_node.parent_index + while parent_index != -1: + n = closed_set[parent_index] rx.append(n.x) ry.append(n.y) - pind = n.pind + parent_index = n.parent_index return rx, ry @@ -255,19 +258,19 @@ def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover [sample_y[i], sample_y[ind]], "-k") -def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree): - maxx = max(ox) - maxy = max(oy) - minx = min(ox) - miny = min(oy) +def sample_points(sx, sy, gx, gy, rr, ox, oy, obstacle_kd_tree): + max_x = max(ox) + max_y = max(oy) + min_x = min(ox) + min_y = min(oy) sample_x, sample_y = [], [] while len(sample_x) <= N_SAMPLE: - tx = (random.random() * (maxx - minx)) + minx - ty = (random.random() * (maxy - miny)) + miny + tx = (random.random() * (max_x - min_x)) + min_x + ty = (random.random() * (max_y - min_y)) + min_y - index, dist = obkdtree.search(np.array([tx, ty]).reshape(2, 1)) + index, dist = obstacle_kd_tree.search(np.array([tx, ty]).reshape(2, 1)) if dist[0] >= rr: sample_x.append(tx) @@ -320,12 +323,13 @@ def main(): plt.grid(True) plt.axis("equal") - rx, ry = PRM_planning(sx, sy, gx, gy, ox, oy, robot_size) + rx, ry = prm_planning(sx, sy, gx, gy, ox, oy, robot_size) assert rx, 'Cannot found path' if show_animation: plt.plot(rx, ry, "-r") + plt.pause(0.001) plt.show() diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index 5538731d..8ba11a23 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -71,9 +71,9 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_ quintic polynomial planner input - sx: start x position [m] - sy: start y position [m] - syaw: start yaw angle [rad] + s_x: start x position [m] + s_y: start y position [m] + s_yaw: start yaw angle [rad] sa: start accel [m/ss] gx: goal x position [m] gy: goal y position [m] diff --git a/PathPlanning/VoronoiRoadMap/dijkstra_search.py b/PathPlanning/VoronoiRoadMap/dijkstra_search.py index 7bf5425e..54d15ea9 100644 --- a/PathPlanning/VoronoiRoadMap/dijkstra_search.py +++ b/PathPlanning/VoronoiRoadMap/dijkstra_search.py @@ -35,8 +35,8 @@ class DijkstraSearch: """ Search shortest path - sx: start x positions [m] - sy: start y positions [m] + s_x: start x positions [m] + s_y: start y positions [m] gx: goal x position [m] gx: goal x position [m] node_x: node x position diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index 9bb68c28..9335dbe6 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -6,20 +6,22 @@ author: Atsushi Sakai (@Atsushi_twi) Ref -[A Tutorial on Graph-Based SLAM](http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf) +[A Tutorial on Graph-Based SLAM] +(http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf) """ -import numpy as np -import math import copy import itertools -import matplotlib.pyplot as plt +import math +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial.transform import Rotation as Rot # Simulation parameter -Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 -Rsim = np.diag([0.1, np.deg2rad(10.0)])**2 +Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2 +R_sim = np.diag([0.1, np.deg2rad(10.0)]) ** 2 DT = 2.0 # time tick [s] SIM_TIME = 100.0 # simulation time [s] @@ -33,11 +35,11 @@ C_SIGMA3 = np.deg2rad(1.0) MAX_ITR = 20 # Maximum iteration -show_graph_dtime = 20.0 # [s] +show_graph_d_time = 20.0 # [s] show_animation = True -class Edge(): +class Edge: def __init__(self): self.e = np.zeros((3, 1)) @@ -52,26 +54,21 @@ class Edge(): self.id2 = 0 -def cal_observation_sigma(d): - +def cal_observation_sigma(): sigma = np.zeros((3, 3)) - sigma[0, 0] = C_SIGMA1**2 - sigma[1, 1] = C_SIGMA2**2 - sigma[2, 2] = C_SIGMA3**2 + sigma[0, 0] = C_SIGMA1 ** 2 + sigma[1, 1] = C_SIGMA2 ** 2 + sigma[2, 2] = C_SIGMA3 ** 2 return sigma def calc_rotational_matrix(angle): - - Rt = np.array([[math.cos(angle), -math.sin(angle), 0], - [math.sin(angle), math.cos(angle), 0], - [0, 0, 1.0]]) - return Rt + return Rot.from_euler('z', angle).as_matrix() def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, - angle1, phi1, d2, angle2, phi2, t1, t2): + angle1, d2, angle2, t1, t2): edge = Edge() tangle1 = pi_2_pi(yaw1 + angle1) @@ -88,8 +85,8 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, Rt1 = calc_rotational_matrix(tangle1) Rt2 = calc_rotational_matrix(tangle2) - sig1 = cal_observation_sigma(d1) - sig2 = cal_observation_sigma(d2) + sig1 = cal_observation_sigma() + sig2 = cal_observation_sigma() edge.omega = np.linalg.inv(Rt1 @ sig1 @ Rt1.T + Rt2 @ sig2 @ Rt2.T) @@ -101,34 +98,33 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, return edge -def calc_edges(xlist, zlist): - +def calc_edges(x_list, z_list): edges = [] cost = 0.0 - zids = list(itertools.combinations(range(len(zlist)), 2)) + z_ids = list(itertools.combinations(range(len(z_list)), 2)) - for (t1, t2) in zids: - x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1] - x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2] + for (t1, t2) in z_ids: + x1, y1, yaw1 = x_list[0, t1], x_list[1, t1], x_list[2, t1] + x2, y2, yaw2 = x_list[0, t2], x_list[1, t2], x_list[2, t2] - if zlist[t1] is None or zlist[t2] is None: + if z_list[t1] is None or z_list[t2] is None: continue # No observation - for iz1 in range(len(zlist[t1][:, 0])): - for iz2 in range(len(zlist[t2][:, 0])): - if zlist[t1][iz1, 3] == zlist[t2][iz2, 3]: - d1 = zlist[t1][iz1, 0] - angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2] - d2 = zlist[t2][iz2, 0] - angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2] + for iz1 in range(len(z_list[t1][:, 0])): + for iz2 in range(len(z_list[t2][:, 0])): + if z_list[t1][iz1, 3] == z_list[t2][iz2, 3]: + d1 = z_list[t1][iz1, 0] + angle1, phi1 = z_list[t1][iz1, 1], z_list[t1][iz1, 2] + d2 = z_list[t2][iz2, 0] + angle2, phi2 = z_list[t2][iz2, 1], z_list[t2][iz2, 2] edge = calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, - angle1, phi1, d2, angle2, phi2, t1, t2) + angle1, d2, angle2, t1, t2) edges.append(edge) - cost += (edge.e.T @ (edge.omega) @ edge.e)[0, 0] + cost += (edge.e.T @ edge.omega @ edge.e)[0, 0] - print("cost:", cost, ",nedge:", len(edges)) + print("cost:", cost, ",n_edge:", len(edges)) return edges @@ -147,7 +143,6 @@ def calc_jacobian(edge): def fill_H_and_b(H, b, edge): - A, B = calc_jacobian(edge) id1 = edge.id1 * STATE_SIZE @@ -167,14 +162,14 @@ def fill_H_and_b(H, b, edge): def graph_based_slam(x_init, hz): print("start graph based slam") - zlist = copy.deepcopy(hz) + z_list = copy.deepcopy(hz) x_opt = copy.deepcopy(x_init) nt = x_opt.shape[1] n = nt * STATE_SIZE for itr in range(MAX_ITR): - edges = calc_edges(x_opt, zlist) + edges = calc_edges(x_opt, z_list) H = np.zeros((n, n)) b = np.zeros((n, 1)) @@ -200,13 +195,12 @@ def graph_based_slam(x_init, hz): def calc_input(): v = 1.0 # [m/s] - yawrate = 0.1 # [rad/s] - u = np.array([[v, yawrate]]).T + yaw_rate = 0.1 # [rad/s] + u = np.array([[v, yaw_rate]]).T return u def observation(xTrue, xd, u, RFID): - xTrue = motion_model(xTrue, u) # add noise to gps x-y @@ -220,16 +214,16 @@ def observation(xTrue, xd, u, RFID): angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0] phi = pi_2_pi(math.atan2(dy, dx)) if d <= MAX_RANGE: - dn = d + np.random.randn() * Qsim[0, 0] # add noise - angle_noise = np.random.randn() * Qsim[1, 1] + dn = d + np.random.randn() * Q_sim[0, 0] # add noise + angle_noise = np.random.randn() * Q_sim[1, 1] angle += angle_noise phi += angle_noise zi = np.array([dn, angle, phi, i]) z = np.vstack((z, zi)) # add noise to input - ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] - ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] + ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] + ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ud = np.array([[ud1, ud2]]).T xd = motion_model(xd, ud) @@ -238,7 +232,6 @@ def observation(xTrue, xd, u, RFID): def motion_model(x, u): - F = np.array([[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]) @@ -277,7 +270,7 @@ def main(): hxTrue = [] hxDR = [] hz = [] - dtime = 0.0 + d_time = 0.0 init = False while SIM_TIME >= time: @@ -290,22 +283,23 @@ def main(): hxTrue = np.hstack((hxTrue, xTrue)) time += DT - dtime += DT + d_time += DT u = calc_input() xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) hz.append(z) - if dtime >= show_graph_dtime: + if d_time >= show_graph_d_time: x_opt = graph_based_slam(hxDR, hz) - dtime = 0.0 + d_time = 0.0 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.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(),