The main task is to implement a function to fuse the laser scan data into the occupancy grid map. You will learn the Bayesian update of the grid map and the practical limitations of its usage. Further, you will experience a dataset with sensory data to speed up your workflow.
Files | Mapper.py - File to be implemented bresenham.py - Implementation of the Bresenham's line algorithm utils.py - Helpers for evaluation, drawing, and saving figures data - Scenarios and reference solutions in pkl files t1d-map.py - Evaluation script t1d-map-navsim.py - Evaluation script for simulation |
Resources | Introduction to CoppeliaSim/V-REP and Open-Loop Robot Locomotion Control Exteroceptive sensing and Reactive-based Obstacle Avoidance redcp.zip, t1d-map.zip - Initial files and evaluation script |
Mapper.fuse_laser_scan()
in Mapper.py
,
Mapper.update_free()
in Mapper.py
,
Map.update_occupied()
in Mapper.py
.
In Mapper.py
, implement the fuse_laser_scan
function to fuse new data into the occupancy grid map as the robot traverses the environment.
The laser scan measurements shall be fused to the grid using the Bayesian update described in Map Building.
The input parameters of the function are:
grid_map
- OccupancyGrid - grid map representation;laser_scan
- LaserScan - contains the current laser scan data perceived by the robot.;odometry
- Odometry - pose.position.x
, pose.position.y
and pose.orientation.quaternion
encode the current robot absolute position in the environment.The function returns:
The fuse_laser_scan
function in the Mapper.py
class has the following prescription.
def fuse_laser_scan(self, grid_map: OccupancyGrid, laser_scan: LaserScan, odometry: Odometry) -> OccupancyGrid: """ Method to fuse the laser scan data sampled by the robot with a given odometry into the probabilistic occupancy grid map Args: grid_map: OccupancyGrid - grid map to fuse the laser scan to laser_scan: LaserScan - laser scan perceived by the robot odometry: Odometry - perceived odometry of the robot Returns: grid_map_update: OccupancyGrid - grid map updated with the laser scan data """
The recommended approach for the occupancy map building using the Bayesian approach is to follow Map Building.
The OccupancyGrid
represents a probabilistic representation of the word.
In particular, the variable “OccupancyGrid.data” holds the probabilities of individual states to be occupied.
The metadata further parameterizes the map in the OccupancyGrid
:
resolution
- the size of the single cell in the real (simulated) world;
width
- width of the map;
height
- height of the map;
origin
- the real-world pose of the cell (0,0) in the map;
data
- the actual probabilistic map data (in row-major order).
Hence the approach to the fusion the laser scan data into the occupancy grid map can be as follows.
$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)
$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)
$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)
$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)
odometry
) and each of the reflection points: $(d \in [0,r))$ and update their probability (being free) and only the reflection point $(d = r)$ being occupied
$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)
The probability values shall never reach 0 or 1!!! The effect of letting it happen is the inability of the map to dynamically add/delete obstacles, as demonstrated in the following example.
t1d-map
, the grid map is predefined with the size $100\times100$ points and correct map origin of $(-5,-5, 0)^T$ and orientation $(0,0,0,1)$, which is prepared exactly for seamless finishing of the t1d-map
assignment.
However, consider generalizing your solution to allow for a dynamically growing map.
The evaluation focus on the ability of the robot to do the correct probabilistic update.
Two evaluation scripts are provided, t1d-map.py
using recorded dataset data/scan_dataset.pkl
and data/grid_map.pkl
.
The second script, t1d-map-navsim.py
, utilizes ControllerReactive
from t1b-react for navigation of the robot in the simulation environment.
The code can be evaluated using the following script (also attached as t1d-map.py
).
dataset = pickle.load( open( "data/scan_dataset.pkl", "rb" ) ) reference = pickle.load( open( "data/gridmap.pkl", "rb" ) ) print('Loaded dataset and reference') scan_n = 0 for scan, odometry in dataset: #process the dataset sys.stdout.write('\rFusing scan %3d' % scan_n) scan_n += 1 gridmap = mapper.fuse_laser_scan(gridmap, scan, odometry) #get the current laser scan and odometry and fuse them to the map print('\nAll scan fused compare the map with reference') fig = plot_map(gridmap, 'Map - Created map') image_file = RESULT_DIR + '/map-fused.png' save_image(fig, 'Save created map to ' + image_file, image_file)
Furthermore, the evaluation using simulation t1d-map-navsim.py
is as follows.
robot = hexapod.HexapodRobot(Controller()) mapper = Mapper() robot.turn_on() #turn on the robot robot.start_navigation() #start navigation thread goals = [ #assign goal for navigation Pose(Vector3(3.5, 3.5, 0), Quaternion(1, 0, 0, 0)), Pose(Vector3(0.0, 0.0, 0), Quaternion(1, 0, 0, 0)), ] plt.ion() #prepare the online plot fig, ax = plt.subplots() gridmap = OccupancyGrid() #prepare the gridmap gridmap.resolution = 0.1 gridmap.width = 100 gridmap.height = 100 gridmap.origin = Pose(Vector3(-5.0, -5.0, 0.0), Quaternion(1, 0, 0, 0)) gridmap.data = 0.5*np.ones((gridmap.height*gridmap.width)) for goal in goals: #go from goal to goal robot.goto_reactive(goal) while robot.navigation_goal is not None: plt.cla() #get the current laser scan and odometry and fuse them to the map gridmap = mapper.fuse_laser_scan(gridmap, robot.laser_scan_, robot.odometry_) redraw_map(ax, gridmap) #plot the map robot.stop_navigation() robot.turn_off()
The expected output is the visually correct environment map, even for dynamic events.