multi_goal/readproblem.m at master · meghdeepj/multi_goal · 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
function [N,C,R,T,M,NO,O,SO,NT] = readproblem(filename)
%READPROBLEM Read problem definition text file.
%   Params:
%         filename    - name of problem text file
%         N           - dimensions of map (1x2)
%         C           - collision threshold for map (1x1)
%         R           - robot start location (1x2)
%         T           - target trajectory (Mx2*NT)     EDITED
%         M           - map cell costs (PxQ where P = N(1), Q = N(2))

%  NO   - number of objects
% O                -M x (NO) * 2 -- dynamic obstacle
%  SO  - size of objects [x y]
% NT - number of targets

FID = fopen(filename, 'r');

if(fgetl(FID) ~= 'N')
    fprintf('Error parsing problem file.')
    return;
end
N = fscanf(FID, '%d,%d')';

if(fgetl(FID) ~= 'C')
    fprintf('Error parsing problem file.')
    return;
end
C = fscanf(FID, '%d');

if(fgetl(FID) ~= 'R')
    fprintf('Error parsing problem file.')
    return;
end
R = fscanf(FID, '%f,%f')';

if(fgetl(FID) ~= 'G')
    fprintf('Error parsing problem file.NT')
    return;
end
NT = fscanf(FID, '%d');


if(fgetl(FID) ~= 'T')
    fprintf('Error parsing problem file.')
    return;
end
form = repmat('%f,%f,',1,NT);
T = textscan(FID, form, 'CollectOutput', true);
T = T{1};


if(fgetl(FID) ~= 'M')
    fprintf('Error parsing problem file.')
    return;
end
formatSpec = repmat(replace(cat(2,repmat('%f,', 1, N(2)), '\n'), ',\n', '\n'), 1, N(1));
M = reshape(fscanf(FID, formatSpec), N(2), N(1))';


if(fgetl(FID) ~= 'G')
    fprintf('Error parsing problem file. NO')
    return;
end
NO = fscanf(FID, '%d')';

if(fgetl(FID) ~= 'R')
    fprintf('Error parsing problem file. SO') %wtf this wont work reee
    return;
end
SO = fscanf(FID, '%f %f\n')';

if(fgetl(FID) ~= 'O')
    fprintf('Error parsing problem file. O')
    return;
end
steps = size(T,1);
formatSpec = '%f,';
sizeO = [NO*2 steps];
O = transpose(fscanf(FID,formatSpec,sizeO));

end