%% Check If Sequence of Poses Is Collision-Free
% Load a costmap from a parking lot.

data = load('parkingLotCostmap.mat');
parkMap = data.parkingLotCostmap;
plot(parkMap)

% Create vehicle poses following a straight-line path. x and y are the (x,y) coordinates of the rear axle of the vehicle. theta is the angle of the rear axle with respect to the x-axis. Note that the dimensions of the vehicle are stored in the CollisionChecker.VehicleDimensions property of the costmap, and that there is an offset between the rear axle of the vehicle and its center.

x = 4:0.25:6;
y = 3:0.25:5;
theta = repmat(45,size(x));
vehiclePoses = [x',y',theta'];
hold on
plot(x,y,'b.')
hold off

% The first few (x,y) coordinates of the rear axle are within the inflated area. However, this does not imply a collision because the center of the vehicle may be outside the inflated area. Check if the poses are collision-free.

free = checkFree(parkMap,vehiclePoses)

% All values of free are 1 (true), so all poses are collision-free. The center of the vehicle does not enter the inflated area at any pose.
% Copyright 2012 The MathWorks, Inc.