-
Notifications
You must be signed in to change notification settings - Fork 0
/
RUN_Astar.m
64 lines (53 loc) · 1.58 KB
/
RUN_Astar.m
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
% A*: Single Robot Path Planning Algorithm - MATLAB
% Initialization
clc
clear
close
% adding paths
addpath('..\models');
addpath('..\common');
%% settings
Model.expandMethod = 'heading'; % random or heading
Model.distType = 'euclidean'; % euclidean or manhattan;
Model.adjType = '8adj'; % 4adj or 8adj
%% create Map and Model
create_model_method = 'from_custom'; % from_map_file, from_samples, from_custom
switch create_model_method
case 'from_map_file'
% load Map file and create model - (1:free, o:obstacles)
map_name = 'Map.mat';
load(map_name, 'Map');
Model = createModelFromMap(Map, Model);
Model = addRobotToModel(Model);
case 'from_samples'
sample_model_name = "Obstacle2";
Model = createModelSamples(sample_model_name, Model);
case 'from_custom'
Model = createModelBase(Model);
end
% complete base model for Astar
Model = createModelAstar(Model);
%% optimal path by Astar
% Path: nodeNumbers, coords, dirs
tic
[Model, Path] = myAStar(Model);
Sol = Path;
Sol.runTime = toc;
Sol.cost = calCostL(Sol.coords);
Sol.smoothness = calSmoothnessbyDir(Sol);
%% modify path
tic
Mpath = modifyPath (Model, Path);
Msol = Mpath;
Msol.runTime = Sol.runTime + toc;
Msol.cost = calCostL(Msol.coords);
Msol.smoothness = calSmoothness(Msol.coords);
%Msol.smoothness = calSmoothnessbyDir(Msol); #todo
%% # display data and plot solution
disp(['run time for path= ' num2str(Sol.runTime)])
disp(Sol)
plotModel(Model)
plotSolution(Sol.coords, Msol.coords)
% plotAnimation2(Model, Sol.coords)
%% clear temporal data
clear adj_type dist_type