%% Create and Populate a Vehicle Costmap
% Create a 10-by-20 meter costmap that is divided into square cells of size 0.5-by-0.5 meters. Specify a default cost value of 0.5 for all cells.

mapWidth = 10;
mapLength = 20;
costVal = 0.5;
cellSize = 0.5;

costmap = vehicleCostmap(mapWidth,mapLength,costVal,'CellSize',cellSize)

% Mark an obstacle on the costmap. Display the costmap.

occupiedVal = 0.9;
xyPoint = [2,4]; 
setCosts(costmap,xyPoint,occupiedVal)

plot(costmap)

% Mark an obstacle-free area on the costmap. Display the costmap again.

freeVal = 0.15; 
[X,Y] = meshgrid(3.5:cellSize:5,0.5:cellSize:1.5); 
setCosts(costmap,[X(:),Y(:)],freeVal)
plot(costmap) 