Search
The main task is to implement a function that will fuse the laser scan data into the occupancy grid map.
GridMapFuser.py
In file GridMapFuser.py implement the fuse_laser_scan function. The purpose of the function is to fuse new data into the occupancy grid map as the robot traverses the environment. The occupancy grid map is initialized to the size of the VREP scene (10$\times$10 m). The laser scan measurements shall be fused to the grid using the Bayesian update described in Lab03 - Grid-based Path Planning.
fuse_laser_scan
The obstacle sensing is achieved using the simulated laser range finder through the RobotHAL interface through the self.robot object.
RobotHAL
self.robot
scan_x, scan_y = self.robot.get_laser_scan()
The fuse_laser_scan function has a following prescription
def fuse_laser_scan(gridmap, pose, scan_x, scan_y): """ Method to fuse the laser scanner data into the map Parameters ----------- gridmap: (GridMap.GridMap) gridmap where to fuse the laser scans pose: (float, float, float) pose of the robot (x, y, orientation) scan_x: list(float) scan_y: list(float) relative x and y coordinates of the points in laser scan w.r.t. the current robot pose """
The recommended approach for the occupancy map building using Bayesian approach is described in Lab03 - Grid-based Path Planning.
The GridMap represents a probabilistic representation of the word. In particular the variable “GridMap.grid” holds the probabilities of individual states to be occupied GridMap.grid['p'] and the derived binary information about the passability of the given cell GridMap.grid['free']. Access and update of the probabilities can be done using functions GridMap.get_cell_p(coord) and GridMap.set_cell_p(coord) that will also automatically update the passability information, when the probability changes.
GridMap
GridMap.grid['p']
GridMap.grid['free']
GridMap.get_cell_p(coord)
GridMap.set_cell_p(coord)
Hence the correct approach to the fusion of the laser scan data into the occupancy grid map is as follows
bresenham_line(start, goal)
To test the fuse_laser_scan function, it is recommended to construct an evaluation dataset by recording the data necessary as the input of the function during a single simulated run and then only read a process these data. To run the recording script t1c-map-collect.py copy the robot folder to facilitate the robot locomotion. The recording script is as follows
t1c-map-collect.py
robot
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import sys import math import time import numpy as np import threading sys.path.append('robot') import Robot as rob DISTANCE_THLD = 0.15 #m def collect_scans(robot, filename): with open(filename,'w') as scanfile: print ( 'Started collecting' ) while not robot.navigation_stop: scan = None pose = None # read scan while scan is None or len(scan[0]) < 1 or pose is None or np.any(np.isnan(pose)) or np.any(np.isnan(scan)): pose = robot.get_pose() scan = robot.robot.get_laser_scan() scan_x, scan_y = scan # write scan to file towrite = '%f,%f,%f,%s,%s\n' % (pose[0],pose[1],pose[2],','.join(['%f' % scanval for scanval in scan_x]),','.join(['%f' % scanval for scanval in scan_y])) scanfile.write(towrite) time.sleep(1.0) print ('Scanning done') if __name__=="__main__": #navigation points route = [(8.5, 4.0, None), (6.0, 6.0, None), (5.0, 5.0, None), (1.5, 8.5, None)] if len(sys.argv) >= 2: scanfile = sys.argv[1] else: scanfile = 'test.scan' print ( 'Collecting scan into file %s' % scanfile) #instantiate the robot robot = rob.Robot() robot.navigation_stop = False #run scan collection scan_thread = threading.Thread(target=collect_scans, args=(robot,scanfile)) scan_thread.start() #navigate for waypoint in route: pos_des = waypoint[0:2] print("Navigation point " + str(pos_des)) #navigate the robot towards the target status1 = robot.goto(pos_des) #stop the robot robot.navigation_stop = True
Using a dataset collected by the recording script, the code can be evaluated using the following script (also attached as t1c-map-eval.py)
t1c-map-eval.py
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import sys import os import math import time import numpy as np import matplotlib.pyplot as plt sys.path.append('gridmap') import GridMap as gmap import GridMapFuser as fuser FLAG_PLOT = True if __name__ == "__main__": if len(sys.argv) >= 2: scanfile = sys.argv[1] else: scanfile = 'test.scan' gridmap = gmap.GridMap(100, 100, 0.1) i = 0 with open(scanfile,'r') as scanfile: for scanline in scanfile: vals = scanline.replace('\n','').split(',') pose = [ float(val) for val in vals[:3]] d = int ( ( len(vals) - 3 ) / 2 ) scan_x = [ float(val) for val in vals[3:3+d] ] scan_y = [ float(val) for val in vals[3+d:] ] i += 1 print ('Fusing scan #%d' % i) fuser.fuse_laser_scan(gridmap, pose, scan_x, scan_y) #plot the gridmap if FLAG_PLOT: gmap.plot_map(gridmap) plt.pause(.000001) if FLAG_PLOT: plt.show()
Following figure visualize the possible sequence of map building given the evaluation script.