matlab-multi-algorithm-robot-navigation/plotEnvironment.m at main · ruddrho/matlab-multi-algorithm-robot-navigation · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
function environmentHandles = plotEnvironment( ...
    axesHandle,environment,cfg)
%PLOTENVIRONMENT Draw the indoor world once.
%
% No path, waypoint, spline, or planned line is drawn.

cla(axesHandle);
hold(axesHandle,'on');
axis(axesHandle,'equal');
axis(axesHandle,[ ...
    0 environment.worldWidth ...
    0 environment.worldHeight]);
grid(axesHandle,'on');
box(axesHandle,'on');

set(axesHandle, ...
    'Color',[0.97 0.97 0.96], ...
    'GridAlpha',0.20, ...
    'FontSize',11);

xlabel(axesHandle,'X position (m)');
ylabel(axesHandle,'Y position (m)');

%% Boundary walls
environmentHandles.boundary = rectangle( ...
    axesHandle, ...
    'Position',[0 0 ...
    environment.worldWidth ...
    environment.worldHeight], ...
    'EdgeColor',[0.10 0.10 0.12], ...
    'LineWidth',5);

%% Rectangular obstacles
environmentHandles.rectangles = ...
    gobjects(size(environment.rectangles,1),1);

for index = 1:size(environment.rectangles,1)
    environmentHandles.rectangles(index) = ...
        rectangle( ...
        axesHandle, ...
        'Position',environment.rectangles(index,:), ...
        'FaceColor',[0.30 0.34 0.38], ...
        'EdgeColor',[0.08 0.08 0.10], ...
        'LineWidth',1.2);
end

%% Cylindrical obstacles
environmentHandles.circles = ...
    gobjects(size(environment.circles,1),1);

for index = 1:size(environment.circles,1)
    circle = environment.circles(index,:);

    environmentHandles.circles(index) = ...
        rectangle( ...
        axesHandle, ...
        'Position',[ ...
        circle(1)-circle(3), ...
        circle(2)-circle(3), ...
        2*circle(3),2*circle(3)], ...
        'Curvature',[1 1], ...
        'FaceColor',[0.20 0.42 0.55], ...
        'EdgeColor',[0.04 0.10 0.14], ...
        'LineWidth',1.2);
end

%% Irregular obstacles
environmentHandles.polygons = ...
    gobjects(numel(environment.polygons),1);

for index = 1:numel(environment.polygons)
    polygon = environment.polygons{index};

    environmentHandles.polygons(index) = ...
        patch(axesHandle, ...
        polygon(:,1),polygon(:,2), ...
        [0.48 0.29 0.18], ...
        'EdgeColor',[0.15 0.07 0.03], ...
        'LineWidth',1.2);
end

%% Start marker
plot(axesHandle, ...
    cfg.robot.startPose(1), ...
    cfg.robot.startPose(2), ...
    'gs','MarkerSize',10, ...
    'MarkerFaceColor','g', ...
    'LineWidth',1.5);

text(axesHandle, ...
    cfg.robot.startPose(1)+0.25, ...
    cfg.robot.startPose(2)-0.35, ...
    'START', ...
    'Color',[0 0.45 0], ...
    'FontWeight','bold');

title(axesHandle, ...
    'Click anywhere in the map to select the destination.', ...
    'FontSize',15,'FontWeight','bold');
end