%% Display a Vehicle on a Costmap
% Load a costmap from a parking lot. Display the costmap.

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

% Create a template polyshape object with the dimensions of the car.

carDims = parkMap.CollisionChecker.VehicleDimensions
ro = carDims.RearOverhang;
fo = carDims.FrontOverhang;
wb = carDims.Wheelbase;
hw = carDims.Width/2;
X = [-ro,wb+fo,wb+fo,-ro];
Y = [-hw,-hw,hw,hw];
templateShape = polyshape(X',Y');

% Create a function handle to move the template to a specified vehicle pose. This move function translates the polyshape s to the coordinate (x,y) and then rotates the polyshape by an angle theta about the point (x,y).

move = @(s,x,y,theta) rotate(translate(s,[x,y]), ...
    theta,[x,y]);

% Move the car template to a pose.

carPose = [5,5,75];
carShape = move(templateShape,carPose(1),carPose(2),carPose(3));

% Plot the car on the costmap.

hold on
plot(carShape)

% Copyright 2012 The MathWorks, Inc.