The main task is to implement a function that will fuse the laser scan data into the occupancy grid map.
Deadline | 12. October 2019, 23:59 PST |
Points | 2 |
Label in BRUTE | t1c-map |
Files to submit | archive with GridMapFuser.py file |
Resources | T1c-map resource package |
Blocks V-REP scene |
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.
The obstacle sensing is achieved using the simulated laser range finder through the RobotHAL
interface through the self.robot
object.
scan_x, scan_y = self.robot.get_laser_scan()The laser scan is in relative coordinates w.r.t. the hexapod base where the $x$ axis corresponds to the heading of the robot and $y$ axis is perpendicular to the robot heading.
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.
Hence the correct approach to the fusion of the laser scan data into the occupancy grid map is as follows
bresenham_line(start, goal)
) which will give you coordinates of the cells which occupancy probability should be updated
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
#!/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
)
#!/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()