-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathmain.cpp
More file actions
59 lines (52 loc) · 1.74 KB
/
Copy pathmain.cpp
File metadata and controls
59 lines (52 loc) · 1.74 KB
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
#include "../source/include/PathFinder.hpp"
#include "../source/include/AStarPathFinder.hpp"
#include "../source/include/Dummy.hpp"
#include "../source/include/Astar.hpp"
#include "Box.hpp"
#include "Coordinate.hpp"
#include "Length.hpp"
#include "Angle.hpp"
#include "Translation.hpp"
#include "SharedObject.hpp"
#include "LockingSharedObject.hpp"
#include "iostream"
int main(int ac, char*av []){
//Creating a map
std::vector<std::vector<int>> cornerSqueezeMap;
for (int x = 0; x < 50; x++) {
std::vector<int> current;
for (int y = 50; y > 0; y--) {
if (x == y) {
if (x >= 4 && x <= 6){
current.push_back(0);
}
else{
current.push_back(1);
}
} else {
current.push_back(0);
}
}
cornerSqueezeMap.push_back(current);
}
//initializing a map
r2d2::Map map(cornerSqueezeMap);
map.print_map();
// initializing pathfinding
r2d2::Translation robotBox{ .5 * r2d2::Length::METER,
.5 * r2d2::Length::METER,
0 * r2d2::Length::METER};
LockingSharedObject<r2d2::Map> sharedMap{map};
r2d2::AStarPathFinder path_finder(sharedMap, {{}, robotBox});
//Computing a path between coordinates
r2d2::Coordinate c1 {1 * r2d2::Length::METER, 2 * r2d2::Length::METER,
0 * r2d2::Length::METER};
r2d2::Coordinate c2 {4 * r2d2::Length::METER,
3 * r2d2::Length::METER,
0 * r2d2::Length::METER};
std::vector<r2d2::Coordinate> path_vector{r2d2::Coordinate(
1 * r2d2::Length::METER,
3 * r2d2::Length::METER,
0 * r2d2::Length::METER)};
cout << path_finder.get_path_to_coordinate(c1, c2, path_vector);
}