├── .gitignore ├── lidar-sensor-simulation ├── env.py ├── main.py ├── map.png ├── scan.jpg └── sensors.py ├── obstacle-avoidance-simulation ├── graphics.py ├── main.py ├── map.png ├── robot-simulation.gif ├── robot-vacuum.png ├── robot.py └── ultrasonic.py └── readme.md /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.toptal.com/developers/gitignore/api/python 3 | # Edit at https://www.toptal.com/developers/gitignore?templates=python 4 | 5 | ### Python ### 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | build/ 17 | develop-eggs/ 18 | dist/ 19 | downloads/ 20 | eggs/ 21 | .eggs/ 22 | lib/ 23 | lib64/ 24 | parts/ 25 | sdist/ 26 | var/ 27 | wheels/ 28 | share/python-wheels/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | MANIFEST 33 | 34 | # PyInstaller 35 | # Usually these files are written by a python script from a template 36 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 37 | *.manifest 38 | *.spec 39 | 40 | # Installer logs 41 | pip-log.txt 42 | pip-delete-this-directory.txt 43 | 44 | # Unit test / coverage reports 45 | htmlcov/ 46 | .tox/ 47 | .nox/ 48 | .coverage 49 | .coverage.* 50 | .cache 51 | nosetests.xml 52 | coverage.xml 53 | *.cover 54 | *.py,cover 55 | .hypothesis/ 56 | .pytest_cache/ 57 | cover/ 58 | 59 | # Translations 60 | *.mo 61 | *.pot 62 | 63 | # Django stuff: 64 | *.log 65 | local_settings.py 66 | db.sqlite3 67 | db.sqlite3-journal 68 | 69 | # Flask stuff: 70 | instance/ 71 | .webassets-cache 72 | 73 | # Scrapy stuff: 74 | .scrapy 75 | 76 | # Sphinx documentation 77 | docs/_build/ 78 | 79 | # PyBuilder 80 | .pybuilder/ 81 | target/ 82 | 83 | # Jupyter Notebook 84 | .ipynb_checkpoints 85 | 86 | # IPython 87 | profile_default/ 88 | ipython_config.py 89 | 90 | # pyenv 91 | # For a library or package, you might want to ignore these files since the code is 92 | # intended to run in multiple environments; otherwise, check them in: 93 | # .python-version 94 | 95 | # pipenv 96 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 97 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 98 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 99 | # install all needed dependencies. 100 | #Pipfile.lock 101 | 102 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 103 | __pypackages__/ 104 | 105 | # Celery stuff 106 | celerybeat-schedule 107 | celerybeat.pid 108 | 109 | # SageMath parsed files 110 | *.sage.py 111 | 112 | # Environments 113 | .env 114 | .venv 115 | env/ 116 | venv/ 117 | ENV/ 118 | env.bak/ 119 | venv.bak/ 120 | 121 | # Spyder project settings 122 | .spyderproject 123 | .spyproject 124 | 125 | # Rope project settings 126 | .ropeproject 127 | 128 | # mkdocs documentation 129 | /site 130 | 131 | # mypy 132 | .mypy_cache/ 133 | .dmypy.json 134 | dmypy.json 135 | 136 | # Pyre type checker 137 | .pyre/ 138 | 139 | # pytype static type analyzer 140 | .pytype/ 141 | 142 | # Cython debug symbols 143 | cython_debug/ 144 | 145 | # End of https://www.toptal.com/developers/gitignore/api/python 146 | -------------------------------------------------------------------------------- /lidar-sensor-simulation/env.py: -------------------------------------------------------------------------------- 1 | import math 2 | import pygame 3 | 4 | class Buildenvironment: 5 | 6 | """_summary_ Build a environment for the simulation 7 | """ 8 | 9 | def __init__(self, MapDimensions): 10 | pygame.init() 11 | self.map_path = "map.png" 12 | self.point_cloud = [] 13 | self.external_map = pygame.image.load(self.map_path) 14 | 15 | self.maph, self.mapw = MapDimensions 16 | self.map_window_name = "RT path Planning" 17 | 18 | pygame.display.set_caption(self.map_window_name) 19 | self.map = pygame.display.set_mode((self.mapw, self.maph)) 20 | self.map.blit(self.external_map, (0, 0)) 21 | 22 | # colors 23 | self.black = (0, 0, 0) 24 | self.grey = (70, 70, 70) 25 | self.blue = (0, 0, 255) 26 | self.green = (0, 255, 0) 27 | self.red = (255, 0, 0) 28 | self.white = (255, 255, 255) 29 | 30 | def ad2pos(self, distance, angle, robot_position): 31 | x = distance*math.cos(angle) + robot_position[0] 32 | y = -distance*math.sin(angle) + robot_position[1] 33 | return (int(x), int(y)) 34 | 35 | def data_storage(self, data): 36 | 37 | print(len(self.point_cloud)) 38 | print("data -->", data) 39 | 40 | for element in data: 41 | point = self.ad2pos(element[0], element[1], element[2]) 42 | if point not in self.point_cloud: 43 | self.point_cloud.append(point) 44 | 45 | def show_sensor_data(self): 46 | self.infomap = self.map.copy() 47 | for point in self.point_cloud: 48 | self.infomap.set_at((int(point[0]), int(point[1])), (255, 0, 0)) 49 | 50 | 51 | -------------------------------------------------------------------------------- /lidar-sensor-simulation/main.py: -------------------------------------------------------------------------------- 1 | from env import Buildenvironment 2 | from sensors import LaserSensor 3 | import pygame 4 | import math 5 | import random 6 | 7 | # source: https://www.youtube.com/watch?v=JbUNsYPJK1U&list=PL9RPomGb9IpRJLw5UTdSy4eJeoLrwNcfC&index=2 8 | 9 | 10 | if __name__ == "__main__": 11 | environment = Buildenvironment((600, 1200)) 12 | environment.originalmap = environment.map.copy() 13 | laser = LaserSensor(200, environment.originalmap, uncertainty=(0.5, 0.01)) 14 | environment.map.fill((0, 0, 0)) 15 | environment.infomap = environment.map.copy() 16 | 17 | running = True 18 | 19 | while running: 20 | sensor_on = False 21 | 22 | for event in pygame.event.get(): 23 | if event.type == pygame.QUIT: 24 | running = False 25 | if pygame.mouse.get_focused(): 26 | sensor_on = True 27 | elif not pygame.mouse.get_focused(): 28 | sensor_on = False 29 | 30 | if sensor_on: 31 | position = pygame.mouse.get_pos() 32 | laser.position = position 33 | sensor_data = laser.sense_obstacles() 34 | environment.data_storage(sensor_data) 35 | environment.show_sensor_data() 36 | 37 | environment.map.blit(environment.infomap, (0, 0)) 38 | pygame.display.update() 39 | -------------------------------------------------------------------------------- /lidar-sensor-simulation/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PJarbas/slam-python/316c4d6318535d6850eff6c531eddf8a9b1fa92f/lidar-sensor-simulation/map.png -------------------------------------------------------------------------------- /lidar-sensor-simulation/scan.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PJarbas/slam-python/316c4d6318535d6850eff6c531eddf8a9b1fa92f/lidar-sensor-simulation/scan.jpg -------------------------------------------------------------------------------- /lidar-sensor-simulation/sensors.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import math 3 | import numpy as np 4 | 5 | 6 | def uncertainty_add(distance, angle, sigma): 7 | mean = np.array([distance, angle]) 8 | covariance = np.diag(sigma**2) 9 | distance, angle = np.random.multivariate_normal(mean, covariance) 10 | distance = max(distance, 0) 11 | angle = max(angle, 0) 12 | return [distance, angle] 13 | 14 | class LaserSensor: 15 | def __init__(self, range, map, uncertainty, speed=4): 16 | 17 | self.range = range 18 | 19 | # fix speed for the sensors rotation, rounds per second 20 | self.speed = speed 21 | self.sigma = np.array([uncertainty[0], uncertainty[1]]) 22 | self.position = (0, 0) 23 | self.map = map 24 | self.w, self.h = pygame.display.get_surface().get_size() 25 | self.sensed_obstacles = [] 26 | 27 | def distance(self, obstacle_position): 28 | px = (obstacle_position[0] - self.position[0])**2 29 | py = (obstacle_position[1] - self.position[1])**2 30 | return math.sqrt(px+py) 31 | 32 | def sense_obstacles(self): 33 | 34 | data = [] 35 | x1, y1 = self.position[0], self.position[1] 36 | 37 | for angle in np.linspace(0, 2*math.pi, 60, False): 38 | 39 | x2, y2 = (x1 + self.range*math.cos(angle), y1 + self.range*math.sin(angle)) 40 | 41 | for i in range(0, 100): 42 | u = i/100 43 | x = int(x2*u + x1*(1-u)) 44 | y = int(y2*u + y1*(1-u)) 45 | 46 | if 0 < x < self.w and 0 < y < self.h: 47 | color = self.map.get_at((x, y)) 48 | 49 | if (color[0], color[1], color[2]) == (0, 0, 0): 50 | 51 | distance = self.distance((x, y)) 52 | 53 | output = uncertainty_add(distance, angle, self.sigma) 54 | output.append(self.position) 55 | 56 | data.append(output) 57 | break 58 | 59 | if len(data) > 0: 60 | return data 61 | else: 62 | return [] 63 | 64 | 65 | -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/graphics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import pygame 4 | 5 | 6 | class Graphics: 7 | def __init__(self, dimensions, robot_img_path, map_img_path): 8 | pygame.init() 9 | 10 | self.sensor_color = (127, 255, 0) 11 | 12 | # map 13 | self.robot = pygame.image.load(robot_img_path) 14 | self.map_img = pygame.image.load(map_img_path) 15 | 16 | self.height, self.width = dimensions 17 | 18 | # window settings 19 | pygame.display.set_caption("Obstacle Avoidance") 20 | 21 | self.map = pygame.display.set_mode((self.width, self.height)) 22 | self.map.blit(self.map_img, (0, 0)) 23 | 24 | def draw_robot(self, x, y, heading): 25 | rotated = pygame.transform.rotozoom(self.robot, math.degrees(heading), 1) 26 | rect = rotated.get_rect(center=(x, y)) 27 | self.map.blit(rotated, rect) 28 | 29 | 30 | def draw_sensor_data(self, point_cloud): 31 | for point in point_cloud: 32 | pygame.draw.circle(self.map, self.sensor_color, point, 3, 0) 33 | 34 | -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/main.py: -------------------------------------------------------------------------------- 1 | import math 2 | import pygame 3 | from robot import Robot 4 | from graphics import Graphics 5 | from ultrasonic import Ultrasonic 6 | 7 | 8 | MAP_DIMENSIONS = (675, 1200) 9 | ROBOT_PATH = "robot-vacuum.png" 10 | MAP_PATH = "map.png" 11 | 12 | if __name__ == "__main__": 13 | 14 | gfx = Graphics(MAP_DIMENSIONS, ROBOT_PATH, MAP_PATH) 15 | 16 | # robot 17 | start = (200, 200) 18 | robot = Robot(start, 0.01*3779.52) 19 | 20 | # sensor 21 | sensor_range = 250, math.radians(40) 22 | 23 | ultra_sonic = Ultrasonic(sensor_range, gfx.map) 24 | 25 | time_step = 0 26 | last_time = pygame.time.get_ticks() 27 | 28 | running = True 29 | 30 | # simulation loop 31 | while running: 32 | 33 | for event in pygame.event.get(): 34 | if event.type == pygame.QUIT: 35 | running = False 36 | 37 | time_step = (pygame.time.get_ticks() - last_time)/1000 38 | last_time = pygame.time.get_ticks() 39 | 40 | gfx.map.blit(gfx.map_img, (0, 0)) 41 | 42 | robot.kinematics(time_step) 43 | 44 | gfx.draw_robot(robot.x, robot.y, robot.heading) 45 | 46 | point_cloud = ultra_sonic.sense_obstacles(robot.x, robot.y, robot.heading) 47 | 48 | robot.avoid_obstacles(point_cloud, time_step) 49 | 50 | gfx.draw_sensor_data(point_cloud) 51 | 52 | pygame.display.update() 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PJarbas/slam-python/316c4d6318535d6850eff6c531eddf8a9b1fa92f/obstacle-avoidance-simulation/map.png -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/robot-simulation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PJarbas/slam-python/316c4d6318535d6850eff6c531eddf8a9b1fa92f/obstacle-avoidance-simulation/robot-simulation.gif -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/robot-vacuum.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PJarbas/slam-python/316c4d6318535d6850eff6c531eddf8a9b1fa92f/obstacle-avoidance-simulation/robot-vacuum.png -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/robot.py: -------------------------------------------------------------------------------- 1 | import pygame 2 | import math 3 | import numpy as np 4 | 5 | 6 | def distance(point1, point2): 7 | point1 = np.array(point1) 8 | point2 = np.array(point2) 9 | return np.linalg.norm(point1 - point2) 10 | 11 | 12 | class Robot: 13 | def __init__(self, startpos, width): 14 | 15 | self.meters_to_pixel = 3779.52 16 | 17 | self.w = width 18 | self.x = startpos[0] 19 | self.y = startpos[1] 20 | 21 | self.heading = 0 22 | 23 | self.velocity_l = 0.01*self.meters_to_pixel 24 | self.velocity_r = 0.01*self.meters_to_pixel 25 | 26 | self.max_speed = 0.02*self.meters_to_pixel 27 | self.min_speed = 0.01*self.meters_to_pixel 28 | 29 | self.min_obs_dist = 100 30 | self.count_down = 5 31 | 32 | 33 | def avoid_obstacles(self, point_cloud, time_step): 34 | closest_obs = None 35 | dist = np.inf 36 | 37 | if len(point_cloud) > 1: 38 | for point in point_cloud: 39 | if dist > distance([self.x, self.y], point): 40 | dist = distance([self.x, self.y], point) 41 | closest_obs = (point, dist) 42 | 43 | if closest_obs[1] < self.min_obs_dist and self.count_down > 0: 44 | self.count_down -= time_step 45 | self.move_backward() 46 | else: 47 | self.count_down = 5 48 | self.move_forward() 49 | 50 | def move_backward(self): 51 | self.velocity_r = -self.min_speed 52 | self.velocity_l = -self.min_speed/2 53 | 54 | def move_forward(self): 55 | self.velocity_r = self.min_speed 56 | self.velocity_l = self.min_speed 57 | 58 | def kinematics(self, time_step): 59 | 60 | self.x += ((self.velocity_l + self.velocity_r)/2) * math.cos(self.heading) *time_step 61 | self.y -= ((self.velocity_l + self.velocity_r)/2) * math.sin(self.heading) *time_step 62 | 63 | self.heading += (self.velocity_r - self.velocity_l) / self.w * time_step 64 | 65 | if self.heading > 2*math.pi or self.heading < -2*math.pi: 66 | self.heading = 0 67 | 68 | self.velocity_r = max(min(self.max_speed, self.velocity_r), self.min_speed) 69 | self.velocity_l = max(min(self.max_speed, self.velocity_l), self.min_speed) 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /obstacle-avoidance-simulation/ultrasonic.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pygame 3 | import math 4 | 5 | 6 | class Ultrasonic: 7 | def __init__(self, sensor_range, map): 8 | self.sensor_range = sensor_range 9 | self.map_width, self.map_height = pygame.display.get_surface().get_size() 10 | self.map = map 11 | self.obstacle_color = (0, 0, 0) 12 | 13 | def sense_obstacles(self, x, y, heading): 14 | obstacles = [] 15 | x1, y1 = x, y 16 | 17 | start_angle = heading - self.sensor_range[1] 18 | finish_angle = heading + self.sensor_range[1] 19 | 20 | for angle in np.linspace(start_angle, finish_angle, 10, False): 21 | x2 = x1 + self.sensor_range[0] * math.cos(angle) 22 | y2 = y1 - self.sensor_range[0] * math.sin(angle) 23 | 24 | for i in range(0, 100): 25 | u = i/100 26 | x = int(x2 * u + x1 * (1-u)) 27 | y = int(y2 * u + y1 * (1-u)) 28 | 29 | if 0 < x < self.map_width and 0 < y < self.map_height: 30 | 31 | color = self.map.get_at((x, y)) 32 | self.map.set_at((x, y), (0, 208, 255)) 33 | 34 | # obstacle color is black 35 | if (color[0], color[1], color[2]) == self.obstacle_color: 36 | obstacles.append([x, y]) 37 | break 38 | 39 | return obstacles 40 | 41 | 42 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ### SLAM and LIDAR Simulation 2 | 3 | Ensure that your **python** version is >= 3.7 4 | 5 | ### Robot obstacle avoidance 6 | 7 | ``` 8 | . 9 | ├── obstacle-avoidance-simulation 10 | | ├── main.py 11 | | ├── robot.py 12 | | ├── ultrasonic.py 13 | | └── graphics.py 14 | ``` 15 | 16 | To run the robot simulation, type the following command in the `obstacle-avoidance-simulation/` directory: 17 | 18 | ```bash 19 | $ python main.py 20 | ``` 21 | 22 | 23 | ![scan](obstacle-avoidance-simulation/robot-simulation.gif) 24 | 25 | 26 | ### Lidar Simulation 27 | 28 | ``` 29 | . 30 | ├── lidar-sensor-simulation 31 | | ├── env.py 32 | | ├── sensors.py 33 | | └── main.py 34 | ``` 35 | 36 | To run the lidar sensor simulation, type the following command in the `lidar-sensor-simulation/` directory: 37 | 38 | ```bash 39 | $ python main.py 40 | ``` 41 | 42 | The output will be a screen where you can move the mouse simulating a lidar sensor 43 | 44 | ![scan](lidar-sensor-simulation/scan.jpg) 45 | 46 | Source: https://www.youtube.com/watch?v=pmmUi6DasoM 47 | --------------------------------------------------------------------------------