├── CMakeLists.txt ├── README.md ├── bagfiles ├── house.bag ├── stage_1.bag ├── stage_2.bag ├── stage_4.bag └── world.bag ├── maps ├── house.png ├── house_compared.png ├── house_grid_map.png ├── house_grid_map_mle.png ├── stage_4.png ├── stage_4_compared.png ├── stage_4_grid_map.png ├── stage_4_grid_map_mle.png ├── world.png ├── world_compared.png ├── world_grid_map.png └── world_grid_map_mle.png ├── package.xml ├── papers ├── Building an Efficient Occupancy Grid Map Based on Lidar Data.pdf ├── High Resolution Maps from Wide Angle Sonar.pdf └── Integratic Topological and Metric Maps for mobile robot navigation.pdf └── scripts ├── __pycache__ ├── bresenham.cpython-38.pyc ├── grid_map.cpython-38.pyc ├── message_handler.cpython-38.pyc ├── rosbag_msg_handler.cpython-38.pyc └── utils.cpython-38.pyc ├── bresenham.py ├── create_from_rosbag.py ├── grid_map.py ├── message_handler.py ├── rtime_gmapping_node.py └── utils.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(grid_mapping) 3 | 4 | ## Find catkin and any catkin packages 5 | find_package(catkin REQUIRED COMPONENTS rospy std_msgs ) 6 | 7 | ## Declare a catkin package 8 | catkin_package() 9 | 10 | # Install python scripts 11 | catkin_install_python(PROGRAMS scripts/create_from_rosbag.py 12 | scripts/message_handler.py 13 | scripts/utils.py 14 | scripts/grid_map.py 15 | scripts/bresenham.py 16 | scripts/rtime_gmapping_node.py 17 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 18 | ) 19 | 20 | ## Build talker and listener 21 | include_directories(include ${catkin_INCLUDE_DIRS}) 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # grid-mapping-in-ROS 3 | Creating Occupancy Grid Maps using Static State Bayes filter and Bresenham's algorithm for mobile robot (turtlebot3_burger) in ROS. 4 | 5 | * bagfiles are created by manually driving the robot with turtlebot3_teleop, topics recorded: '/scan' and '/odom' 6 | * grid maps can be created from bagfiles using [create_from_rosbag.py](scripts/create_from_rosbag.py) 7 | * grid maps can be created in real time using [rtime_gmapping_node.py](scripts/rtime_gmapping_node.py) 8 | 9 | # Content 10 | * [bagfiles](bagfiles) -> folder containing recorded rosbag files 11 | * [maps](maps) -> folder containing images of Gazebo maps as well as output grid maps 12 | * [papers](papers) -> materials used for the project 13 | * [scripts](scripts) -> python scripts 14 | 15 | # Results 16 | 17 | ![real_time_gmapping](https://user-images.githubusercontent.com/72970001/111978505-3e858580-8b04-11eb-9726-74e98ce94b16.gif) 18 | 19 | ![stage_4_compared](https://user-images.githubusercontent.com/72970001/111869094-eae92f80-897d-11eb-8ad8-7cfb21e23eaf.png) 20 | 21 | ![world_compared](https://user-images.githubusercontent.com/72970001/111869096-ecb2f300-897d-11eb-80fe-5737be27f72b.png) 22 | 23 | ![house_compared](https://user-images.githubusercontent.com/72970001/111869077-d9078c80-897d-11eb-8cb7-c6c33618d49a.png) 24 | -------------------------------------------------------------------------------- /bagfiles/house.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/bagfiles/house.bag -------------------------------------------------------------------------------- /bagfiles/stage_1.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/bagfiles/stage_1.bag -------------------------------------------------------------------------------- /bagfiles/stage_2.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/bagfiles/stage_2.bag -------------------------------------------------------------------------------- /bagfiles/stage_4.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/bagfiles/stage_4.bag -------------------------------------------------------------------------------- /bagfiles/world.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/bagfiles/world.bag -------------------------------------------------------------------------------- /maps/house.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/house.png -------------------------------------------------------------------------------- /maps/house_compared.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/house_compared.png -------------------------------------------------------------------------------- /maps/house_grid_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/house_grid_map.png -------------------------------------------------------------------------------- /maps/house_grid_map_mle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/house_grid_map_mle.png -------------------------------------------------------------------------------- /maps/stage_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/stage_4.png -------------------------------------------------------------------------------- /maps/stage_4_compared.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/stage_4_compared.png -------------------------------------------------------------------------------- /maps/stage_4_grid_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/stage_4_grid_map.png -------------------------------------------------------------------------------- /maps/stage_4_grid_map_mle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/stage_4_grid_map_mle.png -------------------------------------------------------------------------------- /maps/world.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/world.png -------------------------------------------------------------------------------- /maps/world_compared.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/world_compared.png -------------------------------------------------------------------------------- /maps/world_grid_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/world_grid_map.png -------------------------------------------------------------------------------- /maps/world_grid_map_mle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/maps/world_grid_map_mle.png -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_mapping 4 | 0.0.0 5 | The grid_mapping package 6 | 7 | 8 | 9 | 10 | maestro 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | rospy 55 | std_msgs 56 | rospy 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /papers/Building an Efficient Occupancy Grid Map Based on Lidar Data.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/papers/Building an Efficient Occupancy Grid Map Based on Lidar Data.pdf -------------------------------------------------------------------------------- /papers/High Resolution Maps from Wide Angle Sonar.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/papers/High Resolution Maps from Wide Angle Sonar.pdf -------------------------------------------------------------------------------- /papers/Integratic Topological and Metric Maps for mobile robot navigation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/papers/Integratic Topological and Metric Maps for mobile robot navigation.pdf -------------------------------------------------------------------------------- /scripts/__pycache__/bresenham.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/scripts/__pycache__/bresenham.cpython-38.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/grid_map.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/scripts/__pycache__/grid_map.cpython-38.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/message_handler.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/scripts/__pycache__/message_handler.cpython-38.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/rosbag_msg_handler.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/scripts/__pycache__/rosbag_msg_handler.cpython-38.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/utils.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lukovicaleksa/grid-mapping-in-ROS/c44e77baf0609c281a21c93f26a81cb261638660/scripts/__pycache__/utils.cpython-38.pyc -------------------------------------------------------------------------------- /scripts/bresenham.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | def bresenham(gridMap, x1, y1, x2, y2): 6 | """ 7 | Bresenham's line drawing algorithm - working for all 4 quadrants! 8 | """ 9 | 10 | # Output pixels 11 | X_bres = [] 12 | Y_bres = [] 13 | 14 | x = x1 15 | y = y1 16 | 17 | delta_x = np.abs(x2 - x1) 18 | delta_y = np.abs(y2 - y1) 19 | 20 | s_x = np.sign(x2 - x1) 21 | s_y = np.sign(y2 - y1) 22 | 23 | if delta_y > delta_x: 24 | 25 | delta_x, delta_y = delta_y, delta_x 26 | interchange = True 27 | 28 | else: 29 | 30 | interchange = False 31 | 32 | A = 2 * delta_y 33 | B = 2 * (delta_y - delta_x) 34 | E = 2 * delta_y - delta_x 35 | 36 | # mark output pixels 37 | X_bres.append(x) 38 | Y_bres.append(y) 39 | 40 | # point (x2,y2) must not be included 41 | for i in range(1, delta_x): 42 | 43 | if E < 0: 44 | 45 | if interchange: 46 | 47 | y += s_y 48 | 49 | else: 50 | 51 | x += s_x 52 | 53 | E = E + A 54 | 55 | else: 56 | 57 | y += s_y 58 | x += s_x 59 | E = E + B 60 | 61 | # mark output pixels 62 | X_bres.append(x) 63 | Y_bres.append(y) 64 | 65 | return zip(X_bres, Y_bres) -------------------------------------------------------------------------------- /scripts/create_from_rosbag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import rosbag 5 | 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from time import perf_counter 9 | 10 | import sys 11 | 12 | SCRIPTS_PATH = '/home/maestro/catkin_ws/src/grid_mapping/scripts' 13 | BAG_FILE_PATH = '/home/maestro/catkin_ws/src/grid_mapping/bagfiles' 14 | MAPS_PATH = '/home/maestro/catkin_ws/src/grid_mapping/maps' 15 | sys.path.insert(0, SCRIPTS_PATH) 16 | 17 | from grid_map import * 18 | from message_handler import * 19 | from utils import * 20 | 21 | P_prior = 0.5 # Prior occupancy probability 22 | P_occ = 0.9 # Probability that cell is occupied with total confidence 23 | P_free = 0.3 # Probability that cell is free with total confidence 24 | 25 | RESOLUTION = 0.04 # Grid resolution in [m] 26 | 27 | MAP_NAME = BAG_FILE_NAME = 'stage_2' # map and bagfile name without extension 28 | 29 | if __name__ == '__main__': 30 | 31 | try: 32 | 33 | ##################### Init section ##################### 34 | 35 | bag = rosbag.Bag(BAG_FILE_PATH + '/' + BAG_FILE_NAME + '.bag') 36 | 37 | if MAP_NAME[:5] == 'stage': 38 | 39 | map_x_lim = [-3, 3] 40 | map_y_lim = [-3, 3] 41 | dir_pointer_len = 0.12 42 | 43 | elif MAP_NAME[:5] == 'world': 44 | 45 | map_x_lim = [-4, 4] 46 | map_y_lim = [-4, 4] 47 | dir_pointer_len = 0.15 48 | 49 | else: 50 | 51 | map_x_lim = [-10, 10] 52 | map_y_lim = [-6, 6] 53 | dir_pointer_len = 0.25 54 | 55 | N_odom = 0 56 | N_scan = 0 57 | N_paired = 0 58 | N_unpaired = 0 59 | 60 | # Create message handler 61 | msgHanlder = MsgHandler() 62 | 63 | # Create grid map 64 | gridMap = GridMap(X_lim = map_x_lim, 65 | Y_lim = map_y_lim, 66 | resolution = RESOLUTION, 67 | p = P_prior) 68 | 69 | # Init figure 70 | plt.style.use('seaborn-ticks') 71 | fig = plt.figure(1) 72 | ax = fig.add_subplot(1,1,1) 73 | 74 | # Init time 75 | t_start = perf_counter() 76 | sim_time = 0 77 | step = 0 78 | 79 | print('\n*** Displaying rosbag files ***') 80 | 81 | ##################### Main loop ##################### 82 | for topic, msg, time_stamp in bag.read_messages(): 83 | 84 | if topic == '/scan': 85 | N_scan += 1 86 | elif topic == '/odom': 87 | N_odom += 1 88 | 89 | msgHanlder.accept_message(topic, msg) 90 | 91 | # if message pair (Odometry+Scan) is not complete continue reading 92 | if msgHanlder.pair_check() == False: 93 | N_unpaired += 1 94 | continue 95 | else: 96 | N_paired +=1 97 | 98 | msgOdom, msgScan = msgHanlder.process_paired_messages() 99 | 100 | # Odometry message processing 101 | x_odom, y_odom = get_odom_position(msgOdom) # x,y in [m] 102 | theta_odom = get_odom_orientation(msgOdom) # theta in [radians] 103 | 104 | # LidarScan message processing 105 | distances, angles, information = lidar_scan(msgScan) # distances in [m], angles in [radians], information [0-1] 106 | 107 | # Lidar measurements in X-Y plane 108 | distances_x, distances_y = lidar_scan_xy(distances, angles, x_odom, y_odom, theta_odom) 109 | filtered_distances_x = [] 110 | filtered_distances_y = [] 111 | 112 | ##################### Grid map update section ##################### 113 | 114 | # x1 and y1 for Bresenham's algorithm 115 | x1, y1 = gridMap.discretize(x_odom, y_odom) 116 | 117 | # for BGR image of the grid map 118 | X2 = [] 119 | Y2 = [] 120 | 121 | for (dist_x, dist_y, dist) in zip(distances_x, distances_y, distances): 122 | 123 | # x2 and y2 for Bresenham's algorithm 124 | x2, y2 = gridMap.discretize(dist_x, dist_y) 125 | 126 | # draw a discrete line of free pixels, [robot position -> laser hit spot) 127 | for (x_bres, y_bres) in bresenham(gridMap, x1, y1, x2, y2): 128 | gridMap.update(x = x_bres, y = y_bres, p = P_free) 129 | 130 | # mark laser hit spot as ocuppied (if exists) 131 | if dist < msgScan.range_max: 132 | 133 | gridMap.update(x = x2, y = y2, p = P_occ) 134 | 135 | # filtered distances in X-Y plane for Ploting 136 | filtered_distances_x.append(dist_x) 137 | filtered_distances_y.append(dist_y) 138 | 139 | # for BGR image of the grid map 140 | X2.append(x2) 141 | Y2.append(y2) 142 | 143 | ##################### Image section ##################### 144 | 145 | # converting grip map to BGR image 146 | bgr_image = gridMap.to_BGR_image() 147 | 148 | # marking robot position with blue pixel value 149 | set_pixel_color(bgr_image, x1, y1, 'BLUE') 150 | 151 | # marking neighbouring pixels with blue pixel value 152 | for (x, y) in gridMap.find_neighbours(x1, y1): 153 | set_pixel_color(bgr_image, x, y, 'BLUE') 154 | 155 | # marking laser hit spots with green value 156 | for (x, y) in zip(X2,Y2): 157 | set_pixel_color(bgr_image, x, y, 'GREEN') 158 | 159 | resized_image = cv2.resize(src = bgr_image, 160 | dsize = (500, 500), 161 | interpolation = cv2.INTER_AREA) 162 | 163 | rotated_image = cv2.rotate(src = resized_image, 164 | rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) 165 | 166 | cv2.imshow("Grid map", rotated_image) 167 | cv2.waitKey(1) 168 | 169 | ##################### Plot section ##################### 170 | ax.clear() 171 | 172 | # Plot Lidar measurements 173 | ax.plot(filtered_distances_x, filtered_distances_y, 'b.', markersize = 1.2, label = 'lidar') 174 | 175 | # Plot Robot position 176 | dir_robot_x = np.array([x_odom, x_odom + dir_pointer_len * np.cos(theta_odom)]) 177 | dir_robot_y = np.array([y_odom, y_odom + dir_pointer_len * np.sin(theta_odom)]) 178 | 179 | ax.plot(x_odom, y_odom, 'r.', markersize = 8, label = 'robot') 180 | ax.plot(dir_robot_x, dir_robot_y, color = 'red', lw = 1.5) 181 | 182 | plt.xlabel('x[m]') 183 | plt.ylabel('y[m]') 184 | plt.title(BAG_FILE_NAME + '.bag') 185 | plt.xlim(map_x_lim) 186 | plt.ylim(map_y_lim) 187 | plt.draw() 188 | plt.pause(0.0001) 189 | 190 | # Calculate step time in [s] 191 | t_step = perf_counter() 192 | step_time = t_step - t_start 193 | sim_time += step_time 194 | t_start = t_step 195 | step += 1 196 | 197 | print('Step %d ==> %d [ms]' % (step, step_time * 1000)) 198 | 199 | ##################### Final output section ##################### 200 | 201 | # Terminal outputs 202 | print('\nScan messages: %d' % N_scan) 203 | print('Odom messages: %d' % N_odom) 204 | print('Paired messages: %d' % N_paired) 205 | print('Unpaired messages: %d' % N_unpaired) 206 | 207 | print('\nSimulation time: %.2f [s]' % sim_time) 208 | print('Average step time: %d [ms]' % (sim_time * 1000 / step)) 209 | print('Frames per second: %.1f' % (step / sim_time)) 210 | 211 | # Saving Grid Map 212 | resized_image = cv2.resize(src = gridMap.to_BGR_image(), 213 | dsize = (500, 500), 214 | interpolation = cv2.INTER_AREA) 215 | 216 | rotated_image = cv2.rotate(src = resized_image, 217 | rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) 218 | 219 | flag_1 = cv2.imwrite(img = rotated_image * 255.0, 220 | filename = MAPS_PATH + '/' + MAP_NAME + '_GRID_MAP.png') 221 | 222 | # Calculating Maximum likelihood estimate of the map 223 | gridMap.calc_MLE() 224 | 225 | # Saving MLE of the Grid Map 226 | resized_image_MLE = cv2.resize(src = gridMap.to_BGR_image(), 227 | dsize = (500, 500), 228 | interpolation = cv2.INTER_AREA) 229 | 230 | rotated_image_MLE = cv2.rotate(src = resized_image_MLE, 231 | rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) 232 | 233 | flag_2 = cv2.imwrite(img = rotated_image_MLE * 255.0, 234 | filename = MAPS_PATH + '/' + MAP_NAME + '_GRID_MAP_MLE.png') 235 | 236 | if flag_1 and flag_2: 237 | print('\nGrid map successfully saved!\n') 238 | 239 | if cv2.waitKey(0) == 27: 240 | cv2.destroyAllWindows() 241 | 242 | except rospy.ROSInterruptException: 243 | 244 | print('\r\nSIMULATION TERMINATED!') 245 | 246 | pass 247 | -------------------------------------------------------------------------------- /scripts/grid_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import sys 5 | import cv2 6 | 7 | SCRIPTS_PATH = '/home/maestro/catkin_ws/src/grid_mapping/scripts' 8 | sys.path.insert(0, SCRIPTS_PATH) 9 | 10 | from bresenham import * 11 | 12 | TRESHOLD_P_FREE = 0.3 13 | TRESHOLD_P_OCC = 0.6 14 | 15 | def log_odds(p): 16 | """ 17 | Log odds ratio of p(x): 18 | 19 | p(x) 20 | l(x) = log ---------- 21 | 1 - p(x) 22 | 23 | """ 24 | return np.log(p / (1 - p)) 25 | 26 | 27 | def retrieve_p(l): 28 | """ 29 | Retrieve p(x) from log odds ratio: 30 | 31 | 1 32 | p(x) = 1 - --------------- 33 | 1 + exp(l(x)) 34 | 35 | """ 36 | return 1 - 1 / (1 + np.exp(l)) 37 | 38 | class GridMap: 39 | """ 40 | Grid map 41 | """ 42 | def __init__(self, X_lim, Y_lim, resolution, p): 43 | 44 | self.X_lim = X_lim 45 | self.Y_lim = Y_lim 46 | self.resolution = resolution 47 | 48 | x = np.arange(start = X_lim[0], stop = X_lim[1] + resolution, step = resolution) 49 | y = np.arange(start = Y_lim[0], stop = Y_lim[1] + resolution, step = resolution) 50 | 51 | # probability matrix in log-odds scale: 52 | self.l = np.full(shape = (len(x), len(y)), fill_value = log_odds(p)) 53 | 54 | def get_shape(self): 55 | """ 56 | Get dimensions 57 | """ 58 | return np.shape(self.l) 59 | 60 | def calc_MLE(self): 61 | """ 62 | Calculate Maximum Likelihood estimate of the map 63 | """ 64 | for x in range(self.l.shape[0]): 65 | 66 | for y in range(self.l.shape[1]): 67 | 68 | # cell is free 69 | if self.l[x][y] < log_odds(TRESHOLD_P_FREE): 70 | 71 | self.l[x][y] = log_odds(0.01) 72 | 73 | # cell is occupied 74 | elif self.l[x][y] > log_odds(TRESHOLD_P_OCC): 75 | 76 | self.l[x][y] = log_odds(0.99) 77 | 78 | # cell state uncertain 79 | else: 80 | 81 | self.l[x][y] = log_odds(0.5) 82 | 83 | def to_BGR_image(self): 84 | """ 85 | Transformation to BGR image format 86 | """ 87 | # grayscale image 88 | gray_image = 1 - retrieve_p(self.l) 89 | 90 | # repeat values of grayscale image among 3 axis to get BGR image 91 | rgb_image = np.repeat(a = gray_image[:,:,np.newaxis], 92 | repeats = 3, 93 | axis = 2) 94 | 95 | return rgb_image 96 | 97 | def to_grayscale_image(self): 98 | """ 99 | Transformation to GRAYSCALE image format 100 | """ 101 | return 1 - retrieve_p(self.l) 102 | 103 | def discretize(self, x_cont, y_cont): 104 | """ 105 | Discretize continious x and y 106 | """ 107 | x = int((x_cont - self.X_lim[0]) / self.resolution) 108 | y = int((y_cont - self.Y_lim[0]) / self.resolution) 109 | return (x,y) 110 | 111 | def update(self, x, y, p): 112 | """ 113 | Update x and y coordinates in discretized grid map 114 | """ 115 | # update probability matrix using inverse sensor model 116 | self.l[x][y] += log_odds(p) 117 | 118 | def check_pixel(self, x, y): 119 | """ 120 | Check if pixel (x,y) is within the map bounds 121 | """ 122 | if x >= 0 and x < self.get_shape()[0] and y >= 0 and y < self.get_shape()[1]: 123 | 124 | return True 125 | 126 | else: 127 | 128 | return False 129 | 130 | def find_neighbours(self, x, y): 131 | """ 132 | Find neighbouring pixels to pixel (x,y) 133 | """ 134 | X_neighbours = [] 135 | Y_neighbours = [] 136 | 137 | if self.check_pixel(x + 1, y): 138 | 139 | X_neighbours.append(x + 1) 140 | Y_neighbours.append(y) 141 | 142 | if self.check_pixel(x + 1, y + 1): 143 | 144 | X_neighbours.append(x + 1) 145 | Y_neighbours.append(y + 1) 146 | 147 | if self.check_pixel(x + 1, y - 1): 148 | 149 | X_neighbours.append(x + 1) 150 | Y_neighbours.append(y - 1) 151 | 152 | if self.check_pixel(x, y + 1): 153 | 154 | X_neighbours.append(x) 155 | Y_neighbours.append(y + 1) 156 | 157 | if self.check_pixel(x, y - 1): 158 | 159 | X_neighbours.append(x) 160 | Y_neighbours.append(y - 1) 161 | 162 | if self.check_pixel(x - 1, y): 163 | 164 | X_neighbours.append(x - 1) 165 | Y_neighbours.append(y) 166 | 167 | if self.check_pixel(x - 1, y + 1): 168 | 169 | X_neighbours.append(x - 1) 170 | Y_neighbours.append(y + 1) 171 | 172 | if self.check_pixel(x - 1, y - 1): 173 | 174 | X_neighbours.append(x - 1) 175 | Y_neighbours.append(y - 1) 176 | 177 | return zip(X_neighbours, Y_neighbours) 178 | 179 | 180 | def set_pixel_color(bgr_image, x, y, color): 181 | """ 182 | Set 'color' to the given pixel (x,y) on 'bgr_image' 183 | """ 184 | 185 | if x < 0 or y < 0 or x >= bgr_image.shape[0] or y >= bgr_image.shape[1]: 186 | return 187 | 188 | if color == 'BLUE': 189 | 190 | bgr_image[x, y, 0] = 1.0 191 | bgr_image[x, y, 1] = 0.0 192 | bgr_image[x, y, 2] = 0.0 193 | 194 | elif color == 'GREEN': 195 | 196 | bgr_image[x, y, 0] = 0.0 197 | bgr_image[x, y, 1] = 1.0 198 | bgr_image[x, y, 2] = 0.0 199 | 200 | elif color == 'RED': 201 | 202 | bgr_image[x, y, 0] = 0.0 203 | bgr_image[x, y, 1] = 0.0 204 | bgr_image[x, y, 2] = 1.0 205 | -------------------------------------------------------------------------------- /scripts/message_handler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from sensor_msgs.msg import LaserScan 4 | from nav_msgs.msg import Odometry 5 | 6 | class MsgStack: 7 | """ 8 | Stack of messages 9 | """ 10 | def __init__(self): 11 | self.messages = [] 12 | 13 | def push(self, msg): 14 | """ 15 | Push method 16 | """ 17 | self.messages.append(msg) 18 | 19 | def top(self): 20 | """ 21 | Top method 22 | """ 23 | return self.messages[-1] 24 | 25 | def pop(self): 26 | """ 27 | Pop method 28 | """ 29 | return self.messages.pop() 30 | 31 | def empty(self): 32 | """ 33 | Is empty method 34 | """ 35 | return len(self.messages) == 0 36 | 37 | 38 | class MsgHandler: 39 | """ 40 | Message handler 41 | """ 42 | def __init__(self): 43 | self.scanMsgStack = MsgStack() 44 | self.odomMsgStack = MsgStack() 45 | 46 | def accept_message(self, topic, msg): 47 | """ 48 | Accepting message method 49 | """ 50 | if topic == '/scan': 51 | self.scanMsgStack.push(msg) 52 | elif topic == '/odom': 53 | self.odomMsgStack.push(msg) 54 | 55 | def pair_check(self): 56 | """ 57 | Checking if /odom and /scan messages are paired 58 | """ 59 | if self.scanMsgStack.empty() or self.odomMsgStack.empty(): 60 | return False 61 | else: 62 | return True 63 | 64 | def process_paired_messages(self): 65 | """ 66 | Processing paired messages 67 | """ 68 | msgOdom = self.odomMsgStack.pop() 69 | msgScan = self.scanMsgStack.pop() 70 | return (msgOdom, msgScan) -------------------------------------------------------------------------------- /scripts/rtime_gmapping_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | from time import perf_counter 8 | 9 | import sys 10 | 11 | SCRIPTS_PATH = '/home/maestro/catkin_ws/src/grid_mapping/scripts' 12 | MAPS_PATH = '/home/maestro/catkin_ws/src/grid_mapping/maps' 13 | sys.path.insert(0, SCRIPTS_PATH) 14 | 15 | from grid_map import * 16 | from utils import * 17 | 18 | P_prior = 0.5 # Prior occupancy probability 19 | P_occ = 0.9 # Probability that cell is occupied with total confidence 20 | P_free = 0.3 # Probability that cell is free with total confidence 21 | 22 | RESOLUTION = 0.03 # Grid resolution in [m] 23 | 24 | MAP_NAME = 'world' # map name without extension 25 | 26 | if __name__ == '__main__': 27 | 28 | try: 29 | 30 | # Init map parameters 31 | if MAP_NAME[:5] == 'stage': 32 | 33 | map_x_lim = [-3, 3] 34 | map_y_lim = [-3, 3] 35 | 36 | elif MAP_NAME[:5] == 'world': 37 | 38 | map_x_lim = [-4, 4] 39 | map_y_lim = [-4, 4] 40 | 41 | else: 42 | 43 | map_x_lim = [-10, 10] 44 | map_y_lim = [-6, 6] 45 | 46 | # Init ROS node 47 | rospy.init_node('gmapping_node', anonymous = False) 48 | rate = rospy.Rate(10) 49 | 50 | # Create grid map 51 | gridMap = GridMap(X_lim = map_x_lim, 52 | Y_lim = map_y_lim, 53 | resolution = RESOLUTION, 54 | p = P_prior) 55 | 56 | # Init time 57 | t_start = perf_counter() 58 | sim_time = 0 59 | step = 0 60 | 61 | # Main loop 62 | while not rospy.is_shutdown(): 63 | 64 | # Lidar measurements 65 | msgScan = rospy.wait_for_message('/scan', LaserScan) 66 | distances, angles, information = lidar_scan(msgScan) # distances in [m], angles in [radians] 67 | 68 | # Odometry measurements 69 | msgOdom = rospy.wait_for_message('/odom', Odometry) 70 | x_odom, y_odom = get_odom_position(msgOdom) # x,y in [m] 71 | theta_odom = get_odom_orientation(msgOdom) # theta in [radians] 72 | 73 | # Lidar measurements in X-Y plane 74 | distances_x, distances_y = lidar_scan_xy(distances, angles, x_odom, y_odom, theta_odom) 75 | 76 | # x1 and y1 for Bresenham's algorithm 77 | x1, y1 = gridMap.discretize(x_odom, y_odom) 78 | 79 | # for BGR image of the grid map 80 | X2 = [] 81 | Y2 = [] 82 | 83 | for (dist_x, dist_y, dist) in zip(distances_x, distances_y, distances): 84 | 85 | # x2 and y2 for Bresenham's algorithm 86 | x2, y2 = gridMap.discretize(dist_x, dist_y) 87 | 88 | # draw a discrete line of free pixels, [robot position -> laser hit spot) 89 | for (x_bres, y_bres) in bresenham(gridMap, x1, y1, x2, y2): 90 | 91 | gridMap.update(x = x_bres, y = y_bres, p = P_free) 92 | 93 | # mark laser hit spot as ocuppied (if exists) 94 | if dist < msgScan.range_max: 95 | 96 | gridMap.update(x = x2, y = y2, p = P_occ) 97 | 98 | # for BGR image of the grid map 99 | X2.append(x2) 100 | Y2.append(y2) 101 | 102 | # converting grip map to BGR image 103 | bgr_image = gridMap.to_BGR_image() 104 | 105 | # marking robot position with blue pixel value 106 | set_pixel_color(bgr_image, x1, y1, 'BLUE') 107 | 108 | # marking neighbouring pixels with blue pixel value 109 | for (x, y) in gridMap.find_neighbours(x1, y1): 110 | set_pixel_color(bgr_image, x, y, 'BLUE') 111 | 112 | # marking laser hit spots with green value 113 | for (x, y) in zip(X2,Y2): 114 | set_pixel_color(bgr_image, x, y, 'GREEN') 115 | 116 | resized_image = cv2.resize(src = bgr_image, 117 | dsize = (500, 500), 118 | interpolation = cv2.INTER_AREA) 119 | 120 | rotated_image = cv2.rotate(src = resized_image, 121 | rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) 122 | 123 | cv2.imshow("Grid map", rotated_image) 124 | cv2.waitKey(1) 125 | 126 | # Calculate step time in [s] 127 | t_step = perf_counter() 128 | step_time = t_step - t_start 129 | sim_time += step_time 130 | t_start = t_step 131 | step += 1 132 | 133 | print('Step %d ==> %d [ms]' % (step, step_time * 1000)) 134 | 135 | rate.sleep() 136 | 137 | except rospy.ROSInterruptException: 138 | 139 | print('\r\nSIMULATION TERMINATED!') 140 | print('\nSimulation time: %.2f [s]' % sim_time) 141 | print('Average step time: %d [ms]' % (sim_time * 1000 / step)) 142 | print('Frames per second: %.1f' % (step / sim_time)) 143 | 144 | # Saving Grid Map 145 | resized_image = cv2.resize(src = gridMap.to_BGR_image(), 146 | dsize = (500, 500), 147 | interpolation = cv2.INTER_AREA) 148 | 149 | rotated_image = cv2.rotate(src = resized_image, 150 | rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) 151 | 152 | flag_1 = cv2.imwrite(img = rotated_image * 255.0, 153 | filename = MAPS_PATH + '/' + MAP_NAME + '_grid_map_TEST.png') 154 | 155 | # Calculating Maximum likelihood estimate of the map 156 | gridMap.calc_MLE() 157 | 158 | # Saving MLE of the Grid Map 159 | resized_image_MLE = cv2.resize(src = gridMap.to_BGR_image(), 160 | dsize = (500, 500), 161 | interpolation = cv2.INTER_AREA) 162 | 163 | rotated_image_MLE = cv2.rotate(src = resized_image_MLE, 164 | rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE) 165 | 166 | flag_2 = cv2.imwrite(img = rotated_image_MLE * 255.0, 167 | filename = MAPS_PATH + '/' + MAP_NAME + '_grid_map_TEST_mle.png') 168 | 169 | if flag_1 and flag_2: 170 | print('\nGrid map successfully saved!\n') 171 | 172 | if cv2.waitKey(0) == 27: 173 | cv2.destroyAllWindows() 174 | 175 | pass 176 | -------------------------------------------------------------------------------- /scripts/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from sensor_msgs.msg import LaserScan 4 | from nav_msgs.msg import Odometry 5 | from tf.transformations import euler_from_quaternion, quaternion_from_euler 6 | 7 | import numpy as np 8 | 9 | def lidar_scan(msgScan): 10 | """ 11 | Convert LaserScan msg to array 12 | """ 13 | distances = np.array([]) 14 | angles = np.array([]) 15 | information = np.array([]) 16 | 17 | for i in range(len(msgScan.ranges)): 18 | # angle calculation 19 | ang = i * msgScan.angle_increment 20 | 21 | # distance calculation 22 | if ( msgScan.ranges[i] > msgScan.range_max ): 23 | dist = msgScan.range_max 24 | elif ( msgScan.ranges[i] < msgScan.range_min ): 25 | dist = msgScan.range_min 26 | else: 27 | dist = msgScan.ranges[i] 28 | 29 | # smaller the distance, bigger the information (measurement is more confident) 30 | inf = ((msgScan.range_max - dist) / msgScan.range_max) ** 2 31 | 32 | distances = np.append(distances, dist) 33 | angles = np.append(angles, ang) 34 | information = np.append(information, inf) 35 | 36 | # distances in [m], angles in [radians], information [0-1] 37 | return ( distances, angles, information ) 38 | 39 | 40 | def lidar_scan_xy(distances, angles, x_odom, y_odom, theta_odom): 41 | """ 42 | Lidar measurements in X-Y plane 43 | """ 44 | distances_x = np.array([]) 45 | distances_y = np.array([]) 46 | 47 | for (dist, ang) in zip(distances, angles): 48 | distances_x = np.append(distances_x, x_odom + dist * np.cos(ang + theta_odom)) 49 | distances_y = np.append(distances_y, y_odom + dist * np.sin(ang + theta_odom)) 50 | 51 | return (distances_x, distances_y) 52 | 53 | 54 | def transform_orientation(orientation_q): 55 | """ 56 | Transform theta to [radians] from [quaternion orientation] 57 | """ 58 | orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] 59 | (roll, pitch, yaw) = euler_from_quaternion(orientation_list) 60 | if yaw < 0: 61 | yaw = 2 * np.pi + yaw # 0->360 degrees >> 0->2pi 62 | return yaw 63 | 64 | 65 | def get_odom_orientation(msgOdom): 66 | """" 67 | Get theta from Odometry msg in [radians] 68 | """ 69 | orientation_q = msgOdom.pose.pose.orientation 70 | theta = transform_orientation(orientation_q) 71 | return theta 72 | 73 | 74 | def get_odom_position(msgOdom): 75 | """ 76 | Get (x,y) coordinates from Odometry msg in [m] 77 | """ 78 | x = msgOdom.pose.pose.position.x 79 | y = msgOdom.pose.pose.position.y 80 | return (x, y) --------------------------------------------------------------------------------