MATLAB code excerpt
clc;
clear all;
%% Section 1 - Laying out the floor plan
load exampleMaps.mat
whos *Map*
map = binaryOccupancyMap(complexMap, 2);
robotRadius = 0.2;
inflateRadius = 0.1;
mapInflated = copy(map);
inflate(mapInflated, inflateRadius);
% Adding obstacles
x = [12.0; 16.5; 10.0; 13.0; 23.0];
y = [8.0; 9.0; 5.0; 2.0; 12.0];
setOccupancy(map, [x y], ones(5, 1));
show(mapInflated)
%% Section 2 - StateSpaceDubins
stateSpace = stateSpaceDubins;
sv = validatorOccupancyMap(stateSpace);
sv.Map = map;
turningRadius = 0.1;
stateBounds = [0, 24; 0, 22; -pi, pi];
stateSpace.StateBounds = stateBounds;
%% Section 3 - RRT Programme
start = [2, 2, 0];
goal = [24, 15, 0];
planner = plannerRRT(stateSpace, sv, MaxConnectionDistance = 0.2);
rng(100, 'twister');
[pthObj, solnInfo] = plan(planner, start, goal);
show(map)
grid on;
title('Office Floor Plan')
hold on
plot(solnInfo.TreeData(:, 1), solnInfo.TreeData(:, 2), '.-')
plot(pthObj.States(:, 1), pthObj.States(:, 2), 'r-', 'LineWidth', 2)