===== T1c-map - Map building ======
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** | {{ :courses:b4m36uir:hw:uir-t1c-map.zip | T1c-map resource package}} |
| | {{ :courses:b4m36uir:hw:blocks.zip | Blocks V-REP scene}} |
===Assignment===
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 [[courses:b4m36uir:labs:lab03|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
"""
=== Approach ===
The recommended approach for the occupancy map building using Bayesian approach is described in [[courses:b4m36uir:labs:lab03|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
- Compensate for the heading of the robot by rotating the scanned points
- Compensate for the position of the robot by offsetting the scanned points to the robot's coordinates
- Raytrace individual scanned points (preferably using Bresenham line algorithm, e.g., ''bresenham_line(start, goal)'') which will give you coordinates of the cells which occupancy probability should be updated
- Update the occupancy grid using the Bayesian update and the simplified laser scan sensor model with $\epsilon = 0$, i.e., using the Bresenham line algorithm trace all the points lying between the position of the robot and the reflection point $(d \in [0,r))$ and update their probability (being free) and only the reflection point $(d = r)$ being occupied
Note, when using the sensory model as it is described in [[courses:b4m36uir:labs:lab03|Lab03 - Grid-based Path Planning]], once the grid cell probability is set to 0, it stays 0, hence, all the nice features of the probabilistic mapping vanishes. Therefore it is recommended to use a different sensory model with:
$$
S^z_{occupied} = \begin{cases} 0.95 \qquad\text{for}\qquad d = r\\ 0 \qquad\text{otherwise} \end{cases},
$$
$$
S^z_{free} = \begin{cases} 0.95 \qquad\text{for}\qquad d \in [0,r)\\ 0 \qquad\text{otherwise} \end{cases},
$$
=== Laser scan dataset creation ===
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
=== Evaluation ===
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()
=== Possible built maps ===
Following figure visualize the possible sequence of map building given the evaluation script.
{{:courses:b4m36uir:hw:map-development.gif?960|}}