├── .gitignore ├── Pipfile ├── Presentations ├── OGM_ERL.pptx ├── Path_Planning_ERL.pptx └── Python Simulation Environment_ERL.pptx ├── README.md ├── localization_and_mapping ├── data │ ├── angles.npy │ ├── angular_velocities.npy │ ├── linear_velocities.npy │ ├── occ_map.png │ ├── poses.npy │ └── ranges.npy ├── scripts │ ├── __init__.py │ ├── dead_reckoning.py │ ├── lidar_point_cloud.py │ ├── log_odds_mapping.py │ └── pf_localization.py └── utils │ ├── __init__.py │ ├── util.py │ └── visualization.py ├── motion_planning ├── data │ └── grid_1.npy ├── results_pictures │ ├── a_star.png │ ├── dijkstra.png │ └── path_planning.png └── scripts │ ├── a_star.py │ ├── dijkstra.py │ ├── path_planning.py │ └── window.py └── tutorial_pictures ├── cd_pycharmdir.png ├── clone.png ├── deadsnakes_1.png ├── deadsnakes_2.png ├── extract_pycharm.png ├── pipenv_install.png ├── privacy_policy.png ├── pycharm_download.png ├── python_install_1.png ├── python_install_2.png ├── python_vcheck.png ├── update_install.png └── welcome_window.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | .idea 29 | MANIFEST 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .nox/ 45 | .coverage 46 | .coverage.* 47 | .cache 48 | nosetests.xml 49 | coverage.xml 50 | *.cover 51 | *.py,cover 52 | .hypothesis/ 53 | .pytest_cache/ 54 | 55 | # Translations 56 | *.mo 57 | *.pot 58 | 59 | # Django stuff: 60 | *.log 61 | local_settings.py 62 | db.sqlite3 63 | db.sqlite3-journal 64 | 65 | # Flask stuff: 66 | instance/ 67 | .webassets-cache 68 | 69 | # Scrapy stuff: 70 | .scrapy 71 | 72 | # Sphinx documentation 73 | docs/_build/ 74 | 75 | # PyBuilder 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | .python-version 87 | 88 | # pipenv 89 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 90 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 91 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 92 | # install all needed dependencies. 93 | #Pipfile.lock 94 | 95 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 96 | __pypackages__/ 97 | 98 | # Celery stuff 99 | celerybeat-schedule 100 | celerybeat.pid 101 | 102 | # SageMath parsed files 103 | *.sage.py 104 | 105 | # Environments 106 | .env 107 | .venv 108 | env/ 109 | venv/ 110 | ENV/ 111 | env.bak/ 112 | venv.bak/ 113 | 114 | # Spyder project settings 115 | .spyderproject 116 | .spyproject 117 | 118 | # Rope project settings 119 | .ropeproject 120 | 121 | # mkdocs documentation 122 | /site 123 | 124 | # mypy 125 | .mypy_cache/ 126 | .dmypy.json 127 | dmypy.json 128 | 129 | # Pyre type checker 130 | .pyre/ 131 | -------------------------------------------------------------------------------- /Pipfile: -------------------------------------------------------------------------------- 1 | [[source]] 2 | name = "pypi" 3 | url = "https://pypi.org/simple" 4 | verify_ssl = true 5 | 6 | [dev-packages] 7 | 8 | [packages] 9 | numpy = "*" 10 | matplotlib = "*" 11 | opencv-python = "*" 12 | networkx = "*" 13 | 14 | [requires] 15 | python_version = "3.7" 16 | -------------------------------------------------------------------------------- /Presentations/OGM_ERL.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/Presentations/OGM_ERL.pptx -------------------------------------------------------------------------------- /Presentations/Path_Planning_ERL.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/Presentations/Path_Planning_ERL.pptx -------------------------------------------------------------------------------- /Presentations/Python Simulation Environment_ERL.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/Presentations/Python Simulation Environment_ERL.pptx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotics Workshop # 2 | 3 | This repository contains the logistics for the Robotics Workshop. 4 | 5 | ### Installation and Demos 6 | 7 | Please visit the [wiki page](https://github.com/ExistentialRobotics/robotics-workshop/wiki/Getting-started) for installation instructions and the list of demos. 8 | 9 | ### Slides and Recordings 10 | 11 | * Localization and Mapping: 12 | + [Wiki](https://github.com/ExistentialRobotics/robotics-workshop/wiki/Localization-and-Mapping) 13 | + [Slides](https://drive.google.com/file/d/1SNbLO_JxYmhhlZDoNOztgVAga11boOpv/view?usp=sharing) 14 | + [Recording](https://drive.google.com/file/d/1Q6VUDrLEF6tXTbQSwidgjfnYQPERNu_U/view?usp=sharing) 15 | 16 | * Planning: 17 | + [Wiki](https://github.com/ExistentialRobotics/robotics-workshop/wiki/Motion-Planning) 18 | + [Slides](https://docs.google.com/presentation/d/1suhyZSuhL6OzJimihyCJ607Ac8g7yJq8QE-nNGlhYZw/edit?usp=sharing) 19 | + [Recording](https://drive.google.com/file/d/1fga8nBZLUtpLyZFWVMjM2UhNx8IrsWk9/view?usp=sharing) 20 | -------------------------------------------------------------------------------- /localization_and_mapping/data/angles.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/data/angles.npy -------------------------------------------------------------------------------- /localization_and_mapping/data/angular_velocities.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/data/angular_velocities.npy -------------------------------------------------------------------------------- /localization_and_mapping/data/linear_velocities.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/data/linear_velocities.npy -------------------------------------------------------------------------------- /localization_and_mapping/data/occ_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/data/occ_map.png -------------------------------------------------------------------------------- /localization_and_mapping/data/poses.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/data/poses.npy -------------------------------------------------------------------------------- /localization_and_mapping/data/ranges.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/data/ranges.npy -------------------------------------------------------------------------------- /localization_and_mapping/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/scripts/__init__.py -------------------------------------------------------------------------------- /localization_and_mapping/scripts/dead_reckoning.py: -------------------------------------------------------------------------------- 1 | from robotics_workshop.utils.util import read_data, read_velocity_data, load_map, diff_drive, xy_to_rc 2 | from robotics_workshop.utils.visualization import visualize_dead_reckoning 3 | 4 | 5 | if __name__ == '__main__': 6 | _, _, poses = read_data() 7 | v, omega = read_velocity_data() 8 | MAP = load_map(xmin=-7.5, xmax=17.5, ymin=-15, ymax=7.5, resolution=0.15) 9 | 10 | render_interval = 5 11 | tau = 0.1 12 | 13 | robot_pose = poses[0, :] 14 | pred_pose_rc = xy_to_rc(robot_pose, MAP) 15 | gt_pose_rc = xy_to_rc(poses[0, :], MAP) 16 | visualize_dead_reckoning(MAP, pred_pose_rc, gt_pose_rc) 17 | 18 | for i in range(poses.shape[0] - 1): 19 | robot_pose = diff_drive(robot_pose, v[i], omega[i], tau) 20 | 21 | if i % render_interval == 0: 22 | pred_pose_rc = xy_to_rc(robot_pose, MAP) 23 | gt_pose_rc = xy_to_rc(poses[i + 1, :], MAP) 24 | visualize_dead_reckoning(MAP, pred_pose_rc, gt_pose_rc) 25 | -------------------------------------------------------------------------------- /localization_and_mapping/scripts/lidar_point_cloud.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from robotics_workshop.utils.util import read_data 4 | from robotics_workshop.utils.visualization import visualize_lidar_pc 5 | 6 | 7 | if __name__ == '__main__': 8 | ranges, angles, poses = read_data() 9 | 10 | render_interval = 15 11 | 12 | for i in range(ranges.shape[0]): 13 | 14 | angles_i = poses[i, 2] + angles[i, :] 15 | point_cloud_i = poses[i, :2][None].T + ranges[i, :] * np.vstack((np.cos(angles_i), np.sin(angles_i))) 16 | 17 | if i % render_interval == 0: 18 | visualize_lidar_pc(point_cloud_i, poses[i, :], i) -------------------------------------------------------------------------------- /localization_and_mapping/scripts/log_odds_mapping.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from robotics_workshop.utils.util import read_data, xy_to_rc 4 | from robotics_workshop.utils.visualization import visualize_log_odds_map 5 | 6 | 7 | def define_map(xmin, xmax, ymin, ymax, resolution): 8 | MAP = {} 9 | MAP['res'] = resolution # meters 10 | MAP['xmin'] = xmin # meters 11 | MAP['ymin'] = ymin 12 | MAP['xmax'] = xmax 13 | MAP['ymax'] = ymax 14 | MAP['sizex'] = int(np.floor((MAP['xmax'] - MAP['xmin']) / MAP['res'] + 1)) # cells 15 | MAP['sizey'] = int(np.floor((MAP['ymax'] - MAP['ymin']) / MAP['res'] + 1)) 16 | MAP['map'] = np.zeros((MAP['sizex'], MAP['sizey'])) 17 | 18 | return MAP 19 | 20 | 21 | def bresenham2D(sx, sy, ex, ey): 22 | sx = int(round(sx)) 23 | sy = int(round(sy)) 24 | ex = int(round(ex)) 25 | ey = int(round(ey)) 26 | dx = abs(ex-sx) 27 | dy = abs(ey-sy) 28 | steep = abs(dy)>abs(dx) 29 | if steep: 30 | dx,dy = dy,dx # swap 31 | 32 | if dy == 0: 33 | q = np.zeros((dx+1,1)) 34 | else: 35 | q = np.append(0,np.greater_equal(np.diff(np.mod(np.arange( np.floor(dx/2), -dy*dx+np.floor(dx/2)-1,-dy),dx)),0)) 36 | if steep: 37 | if sy <= ey: 38 | y = np.arange(sy,ey+1) 39 | else: 40 | y = np.arange(sy,ey-1,-1) 41 | if sx <= ex: 42 | x = sx + np.cumsum(q) 43 | else: 44 | x = sx - np.cumsum(q) 45 | else: 46 | if sx <= ex: 47 | x = np.arange(sx,ex+1) 48 | else: 49 | x = np.arange(sx,ex-1,-1) 50 | if sy <= ey: 51 | y = sy + np.cumsum(q) 52 | else: 53 | y = sy - np.cumsum(q) 54 | return np.vstack((x,y)) 55 | 56 | 57 | if __name__ == '__main__': 58 | ranges, angles, poses = read_data() 59 | 60 | render_interval = 15 61 | delta_lambda = 1 62 | lambda_max = 10 63 | max_range = 10 64 | 65 | MAP = define_map(xmin=-7.5, xmax=17.5, ymin=-15, ymax=7.5, resolution=0.15) 66 | 67 | for i in range(ranges.shape[0]): 68 | 69 | angles_i = poses[i, 2] + angles[i, :] 70 | point_cloud_i = poses[i, :2][None].T + ranges[i, :] * np.vstack((np.cos(angles_i), np.sin(angles_i))) 71 | 72 | pose_rc = xy_to_rc(poses[i, :], MAP) 73 | for ray_ind in range(0, ranges.shape[1], 10): 74 | if np.isnan(point_cloud_i[0, ray_ind]): 75 | v_point = poses[i, :2][None].T + max_range *\ 76 | np.vstack((np.cos(angles_i[ray_ind]), np.sin(angles_i[ray_ind]))) 77 | point_rc = xy_to_rc(v_point, MAP) 78 | ray_rc = bresenham2D(pose_rc[0], pose_rc[1], point_rc[0], point_rc[1]).astype(int) 79 | MAP['map'][ray_rc[0, :], ray_rc[1, :]] = np.clip(MAP['map'][ray_rc[0, :], ray_rc[1, :]] - 80 | delta_lambda, -lambda_max, lambda_max) 81 | else: 82 | point_rc = xy_to_rc(point_cloud_i[:, ray_ind], MAP) 83 | ray_rc = bresenham2D(pose_rc[0], pose_rc[1], point_rc[0], point_rc[1]).astype(int) 84 | MAP['map'][ray_rc[0, :-1], ray_rc[1, :-1]] = np.clip(MAP['map'][ray_rc[0, :-1], ray_rc[1, :-1]] - 85 | delta_lambda, -lambda_max, lambda_max) 86 | MAP['map'][ray_rc[0, -1], ray_rc[1, -1]] = np.clip(MAP['map'][ray_rc[0, -1], ray_rc[1, -1]] + 87 | delta_lambda, -lambda_max, lambda_max) 88 | 89 | if i % render_interval == 0: 90 | visualize_log_odds_map(MAP, pose_rc) -------------------------------------------------------------------------------- /localization_and_mapping/scripts/pf_localization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from robotics_workshop.utils.util import read_data, read_velocity_data, load_map, diff_drive, \ 4 | xy_to_rc, xy_to_rc_multiple 5 | from robotics_workshop.utils.visualization import visualize_pf_localization 6 | 7 | 8 | def prediction(particles, v, omega, std_v, std_omega, tau): 9 | N = particles.shape[0] 10 | n_v = np.random.normal(0, std_v, size=N) 11 | n_omega = np.random.normal(0, std_omega, size=N) 12 | 13 | for k in range(N): 14 | particles[k, :3] = diff_drive(particles[k, :3], v + n_v[k], omega + n_omega[k], tau) 15 | 16 | return particles 17 | 18 | 19 | def update(particles, ranges, angles, MAP): 20 | occ_map = np.zeros((MAP['sizex'], MAP['sizey'])) 21 | np.place(occ_map, MAP['map'] == 0, 1) 22 | corr_list = [] 23 | for k in range(particles.shape[0]): 24 | angles_k = particles[k, 2] + angles 25 | point_cloud_k = particles[k, :2][None].T + ranges * np.vstack((np.cos(angles_k), np.sin(angles_k))) 26 | point_cloud_k_rc = xy_to_rc_multiple(point_cloud_k.T, MAP) 27 | good_inds = np.logical_and(np.logical_and(point_cloud_k_rc[:, 0] >= 0, 28 | point_cloud_k_rc[:, 0] < MAP['sizex']), np.logical_and(point_cloud_k_rc[:, 1] >= 0, 29 | point_cloud_k_rc[:, 1] < MAP['sizey'])) 30 | corr = np.sum(occ_map[point_cloud_k_rc[good_inds, 0], point_cloud_k_rc[good_inds, 1]]) / 5 31 | corr_list.append(corr) 32 | particles[k, 3] = np.exp(corr) * particles[k, 3] 33 | 34 | particles[:, 3] = particles[:, 3] / np.sum(particles[:, 3]) 35 | 36 | return particles 37 | 38 | 39 | def resample(particles): 40 | rand_choice = np.random.choice(np.arange(particles.shape[0]), size=particles.shape[0], p=particles[:, 3]) 41 | particles[:, :3] = particles[rand_choice, :3] 42 | particles[:, 3] = 1 / particles.shape[0] 43 | return particles 44 | 45 | 46 | if __name__ == '__main__': 47 | np.random.seed(3) 48 | 49 | ranges, angles, poses = read_data() 50 | v, omega = read_velocity_data() 51 | MAP = load_map(xmin=-7.5, xmax=17.5, ymin=-15, ymax=7.5, resolution=0.15) 52 | 53 | render_interval = 5 54 | tau = 0.1 55 | std_v = 3 56 | std_omega = 0.03 57 | N = 100 58 | N_eff = 2 59 | 60 | particles = np.empty((N, 4)) 61 | particles[:, 0] = poses[0, 0] 62 | particles[:, 1] = poses[0, 1] 63 | particles[:, 2] = poses[0, 2] 64 | particles[:, 3] = 1 / N 65 | 66 | particles_rc = xy_to_rc_multiple(particles[:, :3], MAP) 67 | gt_pose_rc = xy_to_rc(poses[0, :], MAP) 68 | best_ind = np.argmax(particles[:, 3]) 69 | visualize_pf_localization(MAP, particles_rc[:, :2], gt_pose_rc, best_ind) 70 | 71 | for i in range(poses.shape[0] - 1): 72 | particles = prediction(particles, v[i], omega[i], std_v, std_omega, tau) 73 | particles = update(particles, ranges[i], angles[i], MAP) 74 | 75 | if (1 / np.sum(particles[:, 3]**2)) < N_eff: 76 | particles = resample(particles) 77 | print('Resampling...') 78 | 79 | if i % render_interval == 0: 80 | 81 | particles_rc = xy_to_rc_multiple(particles[:, :3], MAP) 82 | gt_pose_rc = xy_to_rc(poses[i + 1, :], MAP) 83 | best_ind = np.argmax(particles[:, 3]) 84 | visualize_pf_localization(MAP, particles_rc[:, :2], gt_pose_rc, best_ind) -------------------------------------------------------------------------------- /localization_and_mapping/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/localization_and_mapping/utils/__init__.py -------------------------------------------------------------------------------- /localization_and_mapping/utils/util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os, cv2 3 | 4 | 5 | def read_data(): 6 | file_address = os.path.abspath(os.path.join("", "robotics_workshop")) + "/data/" 7 | ranges = np.load(file_address + 'ranges.npy', allow_pickle=True) 8 | angles = np.load(file_address + 'angles.npy', allow_pickle=True) 9 | poses = np.load(file_address + 'poses.npy', allow_pickle=True) 10 | return ranges, angles, poses 11 | 12 | 13 | def read_velocity_data(): 14 | file_address = os.path.abspath(os.path.join("", "robotics_workshop")) + "/data/" 15 | v = np.load(file_address + 'linear_velocities.npy', allow_pickle=True) 16 | omega = np.load(file_address + 'angular_velocities.npy', allow_pickle=True) 17 | return v, omega 18 | 19 | 20 | def load_map(xmin, xmax, ymin, ymax, resolution): 21 | file_address = os.path.abspath(os.path.join("", "robotics_workshop")) + "/data/" 22 | occ_map = cv2.imread(file_address + 'occ_map.png', 0) 23 | 24 | MAP = {} 25 | MAP['res'] = resolution # meters 26 | MAP['xmin'] = xmin # meters 27 | MAP['ymin'] = ymin 28 | MAP['xmax'] = xmax 29 | MAP['ymax'] = ymax 30 | MAP['sizex'] = int(np.floor((MAP['xmax'] - MAP['xmin']) / MAP['res'] + 1)) # cells 31 | MAP['sizey'] = int(np.floor((MAP['ymax'] - MAP['ymin']) / MAP['res'] + 1)) 32 | MAP['map'] = occ_map 33 | 34 | return MAP 35 | 36 | 37 | def obtain_velocities(tau): 38 | _, _, poses = read_data() 39 | omega = (poses[1:, 2] - poses[:-1, 2]) / tau 40 | v = np.diag((poses[1:, :2] - poses[:-1, :2]) @ np.array([np.cos(poses[:-1, 2]), np.sin(poses[:-1, 2])])) / tau 41 | 42 | np.save('linear_velocities.npy', v) 43 | np.save('angular_velocities.npy', omega) 44 | 45 | 46 | def diff_drive(pose, v, omega, tau): 47 | new_x = pose[0] + tau * v * np.cos(pose[2]) 48 | new_y = pose[1] + tau * v * np.sin(pose[2]) 49 | new_theta = pose[2] + tau * omega 50 | return np.array([new_x, new_y, new_theta]) 51 | 52 | 53 | def xy_to_rc(pose, MAP): 54 | r = int(np.floor((pose[0] - MAP['xmin']) / MAP['res'] + 1)) 55 | c = int(np.floor((pose[1] - MAP['ymin']) / MAP['res'] + 1)) 56 | if pose.shape[0] > 2: 57 | return np.array([r, c, pose[2]]) 58 | else: 59 | return np.array([r, c]) 60 | 61 | 62 | def xy_to_rc_multiple(poses, MAP): 63 | r = np.floor((poses[:, 0] - MAP['xmin']) / MAP['res'] + 1).astype(int) 64 | c = np.floor((poses[:, 1] - MAP['ymin']) / MAP['res'] + 1).astype(int) 65 | if poses.shape[1] == 3: 66 | return np.vstack((r, c, poses[:, 2])).T 67 | else: 68 | return np.vstack((r, c)).T 69 | 70 | 71 | if __name__ == '__main__': 72 | obtain_velocities(0.1) 73 | -------------------------------------------------------------------------------- /localization_and_mapping/utils/visualization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | plt.ion() 6 | fig, ax = plt.subplots(figsize=(7, 7)) 7 | 8 | UNEXPLORED = 127 9 | OCCUPIED = 0 10 | FREE = 255 11 | 12 | def visualize_lidar_pc(point_cloud, pose, i): 13 | good_inds = np.logical_not(np.isnan(point_cloud[0, :])) 14 | point_cloud = point_cloud[:, good_inds] 15 | if i == 0: 16 | ax.scatter(point_cloud[0, :], point_cloud[1, :], c='blue', s=0.1) 17 | 18 | ax.scatter(pose[0], pose[1], c='red', s=10, alpha=0.25) 19 | ax.plot(pose[0] + np.array([0, 0.5 * np.cos(pose[2])]), 20 | pose[1] + np.array([0, 0.5 * np.sin(pose[2])]), c='red', alpha=0.25) 21 | 22 | plt.xlim(-7.5, 17.5) 23 | plt.ylim(-15, 7.5) 24 | 25 | plt.draw() 26 | else: 27 | ax.scatter(point_cloud[0, :], point_cloud[1, :], c='blue', s=0.1) 28 | 29 | ax.scatter(pose[0], pose[1], c='red', s=10, alpha=0.25) 30 | ax.plot(pose[0] + np.array([0, 0.5 * np.cos(pose[2])]), 31 | pose[1] + np.array([0, 0.5 * np.sin(pose[2])]), c='red', alpha=0.25) 32 | 33 | fig.canvas.draw_idle() 34 | plt.pause(0.1) 35 | 36 | 37 | def visualize_log_odds_map(MAP, pose_rc): 38 | occ_map = UNEXPLORED * np.ones((MAP['sizex'], MAP['sizey']), dtype=np.uint8) 39 | np.place(occ_map, MAP['map'] > 0, OCCUPIED) 40 | np.place(occ_map, MAP['map'] < 0, FREE) 41 | 42 | ax.imshow(np.flipud(occ_map.T), cmap='gray') 43 | 44 | ax.scatter(pose_rc[0], MAP['sizey'] - pose_rc[1], c='red', s=10, alpha=0.25) 45 | ax.plot(pose_rc[0] + np.array([0, 3.3 * np.cos(pose_rc[2])]), 46 | MAP['sizey'] - pose_rc[1] - np.array([0, 3.3 * np.sin(pose_rc[2])]), c='red', alpha=0.25) 47 | 48 | plt.show() 49 | plt.pause(0.1) 50 | 51 | 52 | def visualize_dead_reckoning(MAP, pred_pose_rc, gt_pose_rc): 53 | ax.clear() 54 | ax.imshow(np.flipud(MAP['map'].T), cmap='gray') 55 | 56 | ax.scatter(gt_pose_rc[0], MAP['sizey'] - gt_pose_rc[1], c='red', s=10, alpha=0.25) 57 | ax.plot(gt_pose_rc[0] + np.array([0, 3.3 * np.cos(gt_pose_rc[2])]), 58 | MAP['sizey'] - gt_pose_rc[1] - np.array([0, 3.3 * np.sin(gt_pose_rc[2])]), c='red', alpha=0.25) 59 | 60 | ax.scatter(pred_pose_rc[0], MAP['sizey'] - pred_pose_rc[1], c='blue', s=10, alpha=0.25) 61 | ax.plot(pred_pose_rc[0] + np.array([0, 3.3 * np.cos(pred_pose_rc[2])]), 62 | MAP['sizey'] - pred_pose_rc[1] - np.array([0, 3.3 * np.sin(pred_pose_rc[2])]), c='blue', alpha=0.25) 63 | 64 | plt.show() 65 | plt.pause(0.1) 66 | 67 | 68 | def visualize_pf_localization(MAP, particles_rc, gt_pose_rc, best_ind): 69 | ax.clear() 70 | ax.imshow(np.flipud(MAP['map'].T), cmap='gray') 71 | 72 | ax.scatter(gt_pose_rc[0], MAP['sizey'] - gt_pose_rc[1], c='red', s=10, alpha=0.25) 73 | ax.plot(gt_pose_rc[0] + np.array([0, 3.3 * np.cos(gt_pose_rc[2])]), 74 | MAP['sizey'] - gt_pose_rc[1] - np.array([0, 3.3 * np.sin(gt_pose_rc[2])]), c='red', alpha=0.25) 75 | 76 | ax.scatter(particles_rc[:, 0], MAP['sizey'] - particles_rc[:, 1], c='blue', s=0.5, alpha=0.25) 77 | ax.scatter(particles_rc[best_ind, 0], MAP['sizey'] - particles_rc[best_ind, 1], c='green', s=10, alpha=1) 78 | 79 | plt.show() 80 | plt.pause(0.1) -------------------------------------------------------------------------------- /motion_planning/data/grid_1.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/motion_planning/data/grid_1.npy -------------------------------------------------------------------------------- /motion_planning/results_pictures/a_star.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/motion_planning/results_pictures/a_star.png -------------------------------------------------------------------------------- /motion_planning/results_pictures/dijkstra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/motion_planning/results_pictures/dijkstra.png -------------------------------------------------------------------------------- /motion_planning/results_pictures/path_planning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/motion_planning/results_pictures/path_planning.png -------------------------------------------------------------------------------- /motion_planning/scripts/a_star.py: -------------------------------------------------------------------------------- 1 | import heapq 2 | import argparse 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from window import WindowBase 6 | 7 | IDX_TO_COLOR = { 8 | 0: np.array([255, 255, 255]), # unexplored 9 | 1: np.array([0, 0, 0]), # wall 10 | 2: np.array([0, 0, 255]), # explored 11 | 3: np.array([0, 255, 0]), # exploring 12 | 4: np.array([255, 0, 0]), # current node / shortest path 13 | 5: np.array([112, 39, 195]), # start / goal 14 | } 15 | 16 | class Window(WindowBase): 17 | """ 18 | Window to visualize A* 19 | """ 20 | def __init__(self, title): 21 | super().__init__(title) 22 | 23 | def show_img(self, img): 24 | """ 25 | Show an image or update the image being shown 26 | """ 27 | 28 | # Show the first image of the environment 29 | if self.imshow_obj is None: 30 | self.imshow_obj = self.ax.imshow(img) 31 | 32 | self.imshow_obj.set_data(img) 33 | self.fig.canvas.draw() 34 | 35 | # Let matplotlib process UI events 36 | # This is needed for interactive mode to work properly 37 | plt.pause(0.01) 38 | 39 | 40 | def process_grid(grid, CLOSED, OPEN, dist, current, start, goal, path=None): 41 | img = [[IDX_TO_COLOR[0] for _ in range(grid.height)] for _ in range(grid.width)] 42 | 43 | for x, y in grid.walls: 44 | img[x][y] = IDX_TO_COLOR[1] 45 | for x, y in OPEN: 46 | img[x][y] = IDX_TO_COLOR[3] 47 | for x, y in CLOSED: 48 | img[x][y] = IDX_TO_COLOR[2] 49 | img[current[0]][current[1]] = IDX_TO_COLOR[4] 50 | if path is not None: 51 | for x, y in path: 52 | img[x][y] = IDX_TO_COLOR[4] 53 | img[start[0]][start[1]] = IDX_TO_COLOR[5] 54 | img[goal[0]][goal[1]] = IDX_TO_COLOR[5] 55 | 56 | img = np.array(img).transpose(1, 0, 2) 57 | return img 58 | 59 | class Grid: 60 | def __init__(self, width, height): 61 | self.width = width 62 | self.height = height 63 | 64 | def add_walls(self, walls): 65 | self.walls = walls 66 | 67 | def neighbors(self, node): 68 | """Cost to each neighbor is 1""" 69 | neighbors = [] 70 | x, y = node 71 | for nx, ny in [[x, y+1], [x, y-1], [x+1, y], [x-1, y]]: 72 | if 0 <= nx < self.width and 0 <= ny < self.height and (nx, ny) not in self.walls: 73 | neighbors.append([(nx, ny), 1]) 74 | return neighbors 75 | 76 | def heuristic(a, b): 77 | """L1 norm (Manhattan distance) between a and b""" 78 | return abs(a[0] - b[0]) + abs(a[1] - b[1]) 79 | 80 | def astar(grid, start, goal, eps=1): 81 | 82 | # Initialize set for explored nodes 83 | CLOSED = set() 84 | 85 | # Initialize min heap for exploring nodes 86 | OPEN = [(0, start)] 87 | heapq.heapify(OPEN) 88 | 89 | # Initialize distance from source to each node 90 | dist = {start: 0} 91 | 92 | # Initialize parent of each node to retrieve shortest path 93 | parent = {start: None} 94 | 95 | # Initialize window for visualization 96 | window = Window('A* algorithm visualization') 97 | window.show(block=False) 98 | 99 | while OPEN: 100 | # pop node with minimum distance from start to current node 101 | d, current = heapq.heappop(OPEN) 102 | CLOSED.add(current) 103 | 104 | # terminate when goal is found 105 | if current == goal: 106 | break 107 | 108 | for nei, w in grid.neighbors(current): 109 | # Do not update node if it is already explored 110 | if nei not in CLOSED: 111 | # update distance to neighbor node through current node 112 | if nei not in dist or dist[nei] > dist[current] + w: 113 | dist[nei] = dist[current] + w 114 | heapq.heappush(OPEN, (dist[nei] + eps * heuristic(nei, goal), nei)) 115 | parent[nei] = current 116 | img = process_grid(grid, CLOSED, [node for _, node in OPEN], dist, current, start, goal) 117 | window.show_img(img) 118 | 119 | path = reconstruct_path(parent, current) 120 | img = process_grid(grid, CLOSED, [node for _, node in OPEN], dist, current, start, goal, path) 121 | window.show_img(img) 122 | plt.pause(5) 123 | window.close() 124 | 125 | def reconstruct_path(parent, node): 126 | """Reconstruct the shortest path from the parent list""" 127 | path = [] 128 | while parent[node] is not None: 129 | path.append(node) 130 | node = parent[node] 131 | path.append(node) 132 | path.reverse() 133 | return path 134 | 135 | 136 | def main(): 137 | 138 | parser = argparse.ArgumentParser() 139 | parser.add_argument('--eps', type=float, default=1.0, 140 | help="Multiplier for heuristic function") 141 | args = parser.parse_args() 142 | 143 | grid = Grid(30, 20) 144 | walls = [(x, 5) for x in range(15, 20)] + [(x, 15) for x in range(15, 20)] + [(20, y) for y in range(5, 16)] 145 | grid.add_walls(walls) 146 | 147 | start = (5, 10) 148 | goal = (25, 10) 149 | 150 | astar(grid, start, goal, args.eps) 151 | 152 | if __name__ == '__main__': 153 | main() -------------------------------------------------------------------------------- /motion_planning/scripts/dijkstra.py: -------------------------------------------------------------------------------- 1 | import heapq 2 | import matplotlib.pyplot as plt 3 | import networkx as nx 4 | from window import WindowBase 5 | 6 | 7 | # node positions for visualization 8 | position = { 9 | 0: (0, 0), 10 | 1: (1, 1), 11 | 2: (1, -1), 12 | 3: (2, 1), 13 | 4: (2, -1), 14 | 5: (3, 0), 15 | 6: (4, -1) 16 | } 17 | 18 | class Window(WindowBase): 19 | def __init__(self, title): 20 | super().__init__(title) 21 | 22 | # Set window margin to see all nodes 23 | self.ax.set_xlim(-0.5, 4.5) 24 | self.ax.set_ylim(-1.5, 1.5) 25 | 26 | def show_graph(self, G): 27 | """ 28 | Show an image or update the image being shown 29 | """ 30 | 31 | pos = nx.get_node_attributes(G, 'pos') 32 | color = nx.get_node_attributes(G, 'color').values() 33 | nx.draw(G, pos, ax=self.ax, with_labels=True, font_weight='bold', node_color=color, node_size=2000) 34 | weights = nx.get_edge_attributes(G, 'weight') 35 | nx.draw_networkx_edge_labels(G, pos, edge_labels=weights) 36 | 37 | self.fig.canvas.draw() 38 | 39 | # Let matplotlib process UI events 40 | # This is needed for interactive mode to work properly 41 | plt.pause(1.0) 42 | 43 | 44 | def process_graph(edges, CLOSED, OPEN, dist, u): 45 | """Builds a networkx graph for visualization""" 46 | G = nx.Graph() 47 | for node in OPEN: 48 | G.add_node(node, pos=position[node], color='green') 49 | for node in CLOSED: 50 | G.add_node(node, pos=position[node], color='blue') 51 | G.add_node(u, pos=position[u], color='red') 52 | for node in edges.keys(): 53 | if node not in CLOSED and node not in OPEN: 54 | G.add_node(node, pos=position[node], color='grey') 55 | for u, neighbors in edges.items(): 56 | for v, w in neighbors: 57 | G.add_edge(u, v, weight=w) 58 | return G 59 | 60 | def dijkstra(edges, start, goal): 61 | # Initialize set for explored nodes 62 | CLOSED = set() 63 | 64 | # Initialize min heap for exploring nodes 65 | OPEN = [(0, start)] 66 | heapq.heapify(OPEN) 67 | 68 | # Initialize distance from source to each node 69 | dist = {start: 0} 70 | 71 | # Initialize parent of each node to retrieve shortest path 72 | parent = {start: None} 73 | 74 | window = Window("Dijkstra's algorithm visualization") 75 | 76 | while OPEN: 77 | # pop node with minimum distance from start to current node 78 | d, u = heapq.heappop(OPEN) 79 | CLOSED.add(u) 80 | 81 | # visualize graph 82 | G = process_graph(edges, CLOSED, [v for _, v in OPEN], dist, u) 83 | window.show_graph(G) 84 | 85 | # terminate when goal is found 86 | if u == goal: 87 | break 88 | 89 | for v, w in edges[u]: 90 | # Do not update node if it is already explored 91 | if v not in CLOSED: 92 | # update distance to neighbor node through current node 93 | if v not in dist or dist[v] > dist[u] + w: 94 | dist[v] = dist[u] + w 95 | heapq.heappush(OPEN, (dist[v], v)) 96 | parent[v] = u 97 | 98 | # visualize graph 99 | G = process_graph(edges, CLOSED, [v for _, v in OPEN], dist, u) 100 | window.show_graph(G) 101 | 102 | 103 | 104 | def main(): 105 | # Initialize graph with adjacency list representation 106 | # [v, w] in edges[u] means a directed edge from u to v with weight w 107 | edges = { 108 | 0: [[1, 4], [2, 2]], 109 | 1: [[1, 1], [2, 1], [3, 1], [5, 10]], 110 | 2: [[4, 3]], 111 | 3: [[5, 2]], 112 | 4: [[5, 2], [6, 10]], 113 | 5: [[6, 4]], 114 | 6: [] 115 | } 116 | 117 | # Specify start and goal nodes 118 | start, goal = 0, 6 119 | 120 | # run 121 | dijkstra(edges, start, goal) 122 | 123 | if __name__ == '__main__': 124 | main() 125 | -------------------------------------------------------------------------------- /motion_planning/scripts/path_planning.py: -------------------------------------------------------------------------------- 1 | import heapq 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from window import WindowBase 5 | 6 | class Window(WindowBase): 7 | """ 8 | Window to visualize path planning 9 | """ 10 | def __init__(self, title): 11 | super().__init__(title) 12 | 13 | # Create plot objects 14 | self.prev_path_obj, = self.ax.plot([], [], 'r.') 15 | self.path_obj, = self.ax.plot([], [], 'b.') 16 | self.pos_obj, = self.ax.plot([], [], 'gs') 17 | self.goal_obj, = self.ax.plot([], [], 'ms') 18 | 19 | def show_img(self, img, prev_path, path, pos, goal): 20 | """ 21 | Show an image or update the image being shown 22 | """ 23 | 24 | # Show the first image of the environment 25 | if self.imshow_obj is None: 26 | self.imshow_obj = self.ax.imshow(img, cmap='gray_r') 27 | 28 | self.imshow_obj.set_data(img) 29 | if len(prev_path) == 0: 30 | self.prev_path_obj.set_data([], []) 31 | else: 32 | prev_path_y, prev_path_x = zip(*prev_path) 33 | self.prev_path_obj.set_data(prev_path_x, prev_path_y) 34 | if len(path) == 0: 35 | self.path_obj.set_data([], []) 36 | else: 37 | path_y, path_x = zip(*path) 38 | self.path_obj.set_data(path_x, path_y) 39 | self.pos_obj.set_data(pos[1], pos[0]) 40 | self.goal_obj.set_data(goal[1], goal[0]) 41 | self.fig.canvas.draw() 42 | 43 | # Let matplotlib process UI events 44 | # This is needed for interactive mode to work properly 45 | plt.pause(0.01) 46 | 47 | 48 | class Grid: 49 | # using rc coord, not cartesian xy 50 | # coordinates define the center of each pixel 51 | # physical size of each pixel is 1, need to add scale of each pixel 52 | # free = -1, occupied = 1 53 | def __init__(self, grid): 54 | self.origin = (0, 0) 55 | self.res = np.array([1, 1]) 56 | self.min = np.array([-0.5, -0.5]) 57 | self.max = grid.shape - 0.5 * self.res 58 | self.true_grid = grid 59 | self.observed_grid = np.zeros_like(grid) 60 | self.frontier = set() 61 | self.observed = set() 62 | 63 | def update_observed_grid(self, free, occupied, frontier, observed): 64 | for x, y in free: 65 | self.observed_grid[x, y] = -1 66 | for x, y in occupied: 67 | self.observed_grid[x, y] = 1 68 | 69 | self.observed = self.observed.union(observed) 70 | self.frontier = self.frontier.union(frontier) 71 | self.frontier = self.frontier.difference(self.observed) 72 | 73 | def get_frontier_and_observed(self): 74 | return self.frontier, self.observed 75 | 76 | def get_observed_grid(self): 77 | return self.observed_grid 78 | 79 | def neighbors(self, pos): 80 | """Returns the neighbors of current position""" 81 | neighbors = [] 82 | x, y = pos 83 | # direct neighbors 84 | for nx, ny in [[x, y+1], [x, y-1], [x+1, y], [x-1, y]]: 85 | if 0 <= nx < self.max[0] and 0 <= ny < self.max[1] and self.observed_grid[nx, ny] == -1: 86 | neighbors.append([(nx, ny), 1]) 87 | # diagonal neighbors 88 | for nx, ny in [[x+1, y+1], [x-1, y-1], [x+1, y-1], [x-1, y+1]]: 89 | if 0 <= nx < self.max[0] and 0 <= ny < self.max[1] and self.observed_grid[nx, ny] == -1: 90 | neighbors.append([(nx, ny), np.sqrt(2)]) 91 | return neighbors 92 | 93 | def heuristic(a, b): 94 | """L2 norm (Euclidean distance) between a and b""" 95 | return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2) 96 | 97 | def meters2cells(X, map_min, res): 98 | return np.floor((X - map_min) / res).astype(int) 99 | 100 | class Lidar: 101 | def __init__(self, lidar_specs, noise=False): 102 | 103 | # lidar name 104 | self.name = lidar_specs['name']; 105 | 106 | # range in meters 107 | self.min_range = lidar_specs['min_range'] 108 | self.max_range = lidar_specs['max_range'] 109 | self.res = lidar_specs['resolution'] 110 | 111 | # angles in radian 112 | self.fov = lidar_specs['fov'] # default 360 degrees field of view, -180 to +180 113 | self.ang_res = lidar_specs['angular_resolution'] # default 5 degrees angular resolution 114 | self.num_scans = self.fov // self.ang_res + 1 # fov / ang_res + 1 115 | 116 | # detected range 117 | self.ranges = None 118 | 119 | def get_scan(self, grid, pos): 120 | ''' 121 | Get lidar scan 122 | Input: 123 | grid - object instance for binary grid map 124 | pos - (row,col) 2D position of the lidar sensor 125 | Output: 126 | 127 | ''' 128 | r = np.arange(self.min_range, self.max_range, self.res) # be careful about the endpoint of the interval 129 | theta = np.arange(-self.fov/2, self.fov/2+1e-6, self.ang_res) 130 | rr, tt = np.meshgrid(r, theta) 131 | xx_w = rr * np.cos(tt) + pos[0] 132 | yy_w = rr * np.sin(tt) + pos[1] 133 | 134 | x_invalid = np.logical_or(xx_w >= grid.max[0], xx_w <= grid.min[0]) 135 | y_invalid = np.logical_or(yy_w >= grid.max[1], yy_w <= grid.min[1]) 136 | invalid = np.logical_or(x_invalid, y_invalid) 137 | 138 | # convert out of bounds scans to origin 139 | xx_w[invalid] = grid.origin[0] 140 | yy_w[invalid] = grid.origin[1] 141 | 142 | x_idx = meters2cells(xx_w, grid.min[0], grid.res[0]) 143 | y_idx = meters2cells(yy_w, grid.min[1], grid.res[1]) 144 | 145 | is_obs = grid.true_grid[x_idx, y_idx] # -1 or 1 146 | is_obs = is_obs / 2 + 0.5 # convert to 0, 1 147 | 148 | good = np.logical_and(np.logical_not(invalid), np.logical_not(is_obs)) 149 | ranges = np.copy(rr) 150 | ranges[good] = self.max_range 151 | 152 | detected_obs = np.argmin(ranges, axis=1) 153 | ranges = ranges[np.arange(ranges.shape[0]), detected_obs] 154 | 155 | observed = set() 156 | frontier = set() 157 | free = set() 158 | occupied = set() 159 | for i in range(ranges.shape[0]): 160 | if detected_obs[i] == 0: # no obstacle along this ray 161 | frontier.add((x_idx[i,-1], y_idx[i,-1])) 162 | free.update([(x_idx[i,j], y_idx[i,j]) for j in range(x_idx.shape[1])]) 163 | observed.update([ 164 | (x_idx[i,j], y_idx[i,j]) for j in range(x_idx.shape[1]) 165 | if (x_idx[i,j], y_idx[i,j]) != (x_idx[i,-1], y_idx[i,-1]) 166 | ]) 167 | else: 168 | occupied.add((x_idx[i, detected_obs[i]], y_idx[i, detected_obs[i]])) 169 | free.update([(x_idx[i,j], y_idx[i,j]) for j in range(detected_obs[i])]) 170 | observed.update([(x_idx[i,j], y_idx[i,j]) for j in range(detected_obs[i]+1)]) 171 | 172 | return free, occupied, frontier, observed 173 | 174 | def reconstruct_path(parent, node): 175 | """Reconstruct the shortest path from the parent list""" 176 | path = [] 177 | while parent[node] is not None: 178 | path.append(node) 179 | node = parent[node] 180 | path.append(node) 181 | path.reverse() 182 | return path 183 | 184 | def a_star(grid, start, goal, eps=1): 185 | 186 | # Initialize set for explored nodes 187 | CLOSED = set() 188 | 189 | # Initialize min heap for exploring nodes 190 | OPEN = [(0, start)] 191 | heapq.heapify(OPEN) 192 | 193 | # Initialize distance from source to each node 194 | dist = {start: 0} 195 | 196 | # Initialize parent of each node to retrieve shortest path 197 | parent = {start: None} 198 | 199 | while OPEN: 200 | # pop node with minimum distance from start to current node 201 | d, current = heapq.heappop(OPEN) 202 | CLOSED.add(current) 203 | 204 | # terminate when goal is found 205 | if current == goal: 206 | break 207 | 208 | for nei, w in grid.neighbors(current): 209 | # Do not update node if it is already explored 210 | if nei not in CLOSED: 211 | # update distance to neighbor node through current node 212 | if nei not in dist or dist[nei] > dist[current] + w: 213 | dist[nei] = dist[current] + w 214 | heapq.heappush(OPEN, (dist[nei] + eps * heuristic(nei, goal), nei)) 215 | parent[nei] = current 216 | 217 | path = reconstruct_path(parent, current) 218 | return path 219 | 220 | def find_temp_goal(frontier, observed, goal): 221 | """ 222 | Return goal if it is observed 223 | Otherwise find a temporary goal within the frontier of explored area if goal is not seen 224 | """ 225 | if goal in observed: 226 | return goal 227 | 228 | dist = [(heuristic(pos, goal), pos) for pos in frontier] 229 | dist.sort() 230 | d, pos = dist[0] 231 | return pos 232 | 233 | def path_planning(grid, lidar, start, goal): 234 | """ 235 | Select a position in the current exploration frontier that is 236 | closest to the goal as temporary goal until the goal is observed 237 | """ 238 | window = Window('Path Planning Visualization') 239 | window.show(block=False) 240 | 241 | pos = start 242 | prev_path = [start] 243 | 244 | # Collect initial lidar observation 245 | free, occupied, frontier, observed = lidar.get_scan(grid, pos) 246 | grid.update_observed_grid(free, occupied, frontier, observed) 247 | 248 | # Calculate temporary goal within the frontier 249 | temp_goal = find_temp_goal(*grid.get_frontier_and_observed(), goal) 250 | 251 | while pos != goal: 252 | 253 | # Use A* to plan a path from current position to the temporary goal 254 | path = a_star(grid, pos, temp_goal) 255 | 256 | # visualize path 257 | window.show_img(grid.get_observed_grid(), prev_path, path, pos, temp_goal) 258 | plt.pause(2) 259 | 260 | # Follow the path and update observation along the way 261 | for i, pos in enumerate(path): 262 | free, occupied, frontier, observed = lidar.get_scan(grid, pos) 263 | prev_path.append(pos) 264 | grid.update_observed_grid(free, occupied, frontier, observed) 265 | 266 | window.show_img(grid.get_observed_grid(), prev_path, path[i+1:], pos, temp_goal) 267 | 268 | # Calculate the next temporary goal 269 | temp_goal = find_temp_goal(*grid.get_frontier_and_observed(), goal) 270 | 271 | plt.pause(3) 272 | window.close() 273 | 274 | if __name__ == '__main__': 275 | 276 | # Load grid, -1: free, 1: occupied 277 | grid = Grid(np.load('../data/grid_1.npy')) 278 | start = (17, 20) 279 | goal = (100, 125) 280 | 281 | lidar_specs = { 282 | 'name': 'Unknown', 283 | 'min_range': 0.1, 284 | 'max_range': 40.0, 285 | 'resolution': 0.1, 286 | 'fov': 360, 287 | 'angular_resolution': 1, 288 | } 289 | lidar = Lidar(lidar_specs) 290 | 291 | path_planning(grid, lidar, start, goal) 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | -------------------------------------------------------------------------------- /motion_planning/scripts/window.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | 3 | class WindowBase: 4 | """ 5 | Window for visualization 6 | """ 7 | def __init__(self, title): 8 | self.fig = None 9 | self.imshow_obj = None 10 | 11 | # Create the figure and axes 12 | self.fig, self.ax = plt.subplots(figsize=(10, 10)) 13 | 14 | # Show the env name in the window title 15 | self.fig.canvas.set_window_title(title) 16 | 17 | # Flag indicating the window was closed 18 | self.closed = False 19 | 20 | def close_handler(evt): 21 | self.closed = True 22 | 23 | self.fig.canvas.mpl_connect('close_event', close_handler) 24 | 25 | 26 | def show(self, block=True): 27 | """ 28 | Show the window, and start an event loop 29 | """ 30 | 31 | # If not blocking, trigger interactive mode 32 | if not block: 33 | plt.ion() 34 | 35 | # Show the plot 36 | # In non-interative mode, this enters the matplotlib event loop 37 | # In interactive mode, this call does not block 38 | plt.show() 39 | 40 | def close(self): 41 | """ 42 | Close the window 43 | """ 44 | 45 | plt.close() 46 | self.closed = True -------------------------------------------------------------------------------- /tutorial_pictures/cd_pycharmdir.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/cd_pycharmdir.png -------------------------------------------------------------------------------- /tutorial_pictures/clone.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/clone.png -------------------------------------------------------------------------------- /tutorial_pictures/deadsnakes_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/deadsnakes_1.png -------------------------------------------------------------------------------- /tutorial_pictures/deadsnakes_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/deadsnakes_2.png -------------------------------------------------------------------------------- /tutorial_pictures/extract_pycharm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/extract_pycharm.png -------------------------------------------------------------------------------- /tutorial_pictures/pipenv_install.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/pipenv_install.png -------------------------------------------------------------------------------- /tutorial_pictures/privacy_policy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/privacy_policy.png -------------------------------------------------------------------------------- /tutorial_pictures/pycharm_download.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/pycharm_download.png -------------------------------------------------------------------------------- /tutorial_pictures/python_install_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/python_install_1.png -------------------------------------------------------------------------------- /tutorial_pictures/python_install_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/python_install_2.png -------------------------------------------------------------------------------- /tutorial_pictures/python_vcheck.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/python_vcheck.png -------------------------------------------------------------------------------- /tutorial_pictures/update_install.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/update_install.png -------------------------------------------------------------------------------- /tutorial_pictures/welcome_window.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ExistentialRobotics/robotics-workshop/64193dd76cf0a69941d95a8affbb9e7be16ddb81/tutorial_pictures/welcome_window.png --------------------------------------------------------------------------------