scoutos / scout / libscout / src / behaviors / navigationMap.cpp @ aa28526f
History | View | Annotate | Download (5.36 KB)
1 | d8caf546 | Priya | #include "navigationMap.h" |
---|---|---|---|
2 | |||
3 | /** @brief Initializes the map */
|
||
4 | navigationMap::navigationMap(std::string scoutname) : Behavior(scoutname)
|
||
5 | { |
||
6 | /** Initialize Map
|
||
7 | *
|
||
8 | * 1 2 3 4
|
||
9 | * ----|-----------|----------|---------|---------->
|
||
10 | * <---|--5--------|--6-------|--7------|--8-------
|
||
11 | * | | | |
|
||
12 | * 9| 10| 11| 12|
|
||
13 | * | | | |
|
||
14 | * --- --- --- ---
|
||
15 | */
|
||
16 | |||
17 | aa5e4ddc | Leon | Edge a1[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 2, 10), |
18 | MAKE_EDGE(IRIGHT, 9, 40), |
||
19 | MAKE_EDGE(IUTURN, DEADEND, 0)};
|
||
20 | d8caf546 | Priya | |
21 | aa5e4ddc | Leon | Edge a2[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 3, 10), |
22 | MAKE_EDGE(IRIGHT, 10, 40), |
||
23 | MAKE_EDGE(IUTURN, 5, 10)}; |
||
24 | d8caf546 | Priya | |
25 | aa5e4ddc | Leon | Edge a3[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 4, 10), |
26 | MAKE_EDGE(IRIGHT, 11, 40), |
||
27 | MAKE_EDGE(IUTURN, 6, 10)}; |
||
28 | d8caf546 | Priya | |
29 | aa5e4ddc | Leon | Edge a4[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
|
30 | MAKE_EDGE(IRIGHT, 12, 40), |
||
31 | MAKE_EDGE(IUTURN, 7, 10)}; |
||
32 | d8caf546 | Priya | |
33 | aa5e4ddc | Leon | Edge a5[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
|
34 | MAKE_EDGE(ILEFT, 9, 40), |
||
35 | MAKE_EDGE(IUTURN, 2, 10)}; |
||
36 | d8caf546 | Priya | |
37 | aa5e4ddc | Leon | Edge a6[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 5, 10), |
38 | MAKE_EDGE(ILEFT, 10, 40), |
||
39 | MAKE_EDGE(IUTURN, 3, 10)}; |
||
40 | d8caf546 | Priya | |
41 | aa5e4ddc | Leon | Edge a7[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 6, 10), |
42 | MAKE_EDGE(ILEFT, 11, 40), |
||
43 | MAKE_EDGE(IUTURN, 4, 10)}; |
||
44 | d8caf546 | Priya | |
45 | aa5e4ddc | Leon | Edge a8[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 7, 10), |
46 | MAKE_EDGE(ILEFT, 12, 40), |
||
47 | MAKE_EDGE(IUTURN, DEADEND, 0)};
|
||
48 | d8caf546 | Priya | |
49 | aa5e4ddc | Leon | Edge a9[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 2, 10), |
50 | MAKE_EDGE(ILEFT, DEADEND, 0),
|
||
51 | MAKE_EDGE(IUTURN, 9, 40)}; |
||
52 | d8caf546 | Priya | |
53 | aa5e4ddc | Leon | Edge a10[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 3, 10), |
54 | MAKE_EDGE(ILEFT, 5, 10), |
||
55 | MAKE_EDGE(IUTURN, 10, 40)}; |
||
56 | d8caf546 | Priya | |
57 | aa5e4ddc | Leon | Edge a11[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 4, 10), |
58 | MAKE_EDGE(ILEFT, 6, 10), |
||
59 | MAKE_EDGE(IUTURN, 11, 40)}; |
||
60 | d8caf546 | Priya | |
61 | aa5e4ddc | Leon | Edge a12[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, DEADEND, 0),
|
62 | MAKE_EDGE(ILEFT, 7, 10), |
||
63 | MAKE_EDGE(IUTURN, 12, 40)}; |
||
64 | d8caf546 | Priya | |
65 | map.pushback(a1); |
||
66 | map.pushback(a2); |
||
67 | map.pushback(a3); |
||
68 | map.pushback(a4); |
||
69 | map.pushback(a5); |
||
70 | map.pushback(a6); |
||
71 | map.pushback(a7); |
||
72 | map.pushback(a8); |
||
73 | map.pushback(a9); |
||
74 | map.pushback(a10); |
||
75 | map.pushback(a11); |
||
76 | map.pushback(a12); |
||
77 | ede03b64 | James Carroll | |
78 | curr_state = START_STATE; |
||
79 | eta = 99999;
|
||
80 | d8caf546 | Priya | } |
81 | |||
82 | /** @brief Goes through and frees all allocated memory */
|
||
83 | navigationMap::~navigationMap() |
||
84 | { |
||
85 | while(!map.empty())
|
||
86 | { |
||
87 | aa5e4ddc | Leon | Edge* e = map.pop_back(); |
88 | delete e;
|
||
89 | d8caf546 | Priya | } |
90 | return;
|
||
91 | } |
||
92 | |||
93 | navigationMap::run() |
||
94 | { |
||
95 | |||
96 | } |
||
97 | |||
98 | ede03b64 | James Carroll | navigationMap::update_state(Turn turn_made) |
99 | { |
||
100 | Edge* possible_edges = get_outbound_edges(current_state); |
||
101 | aa28526f | James Carroll | int arr_size = sizeof(possible_edges)/sizeof(Edge); |
102 | for(i=0;i<arr_size;i++) |
||
103 | { |
||
104 | ede03b64 | James Carroll | //sets the current state to the state associated with the turn made
|
105 | if(GET_EDGE_DIR(possible_edges[i]) == turn_made)
|
||
106 | { |
||
107 | int speed = 10000;//its over 9000 |
||
108 | current_state = GET_EDGE_STATE(possible_edges[i]); |
||
109 | //TODO: get actual speed
|
||
110 | arrival_time = ros::Time::now() + |
||
111 | GET_EDGE_DIST(possible_edges[i])/speed; |
||
112 | return current_state;
|
||
113 | } |
||
114 | } |
||
115 | return -1;//failure to succeed |
||
116 | } |
||
117 | |||
118 | navigationMap::get_eta() |
||
119 | { |
||
120 | return arrival_time;
|
||
121 | } |
||
122 | |||
123 | navigationMap::get_time_remaining() |
||
124 | { |
||
125 | return (arrival_time - ros::Time::now());
|
||
126 | } |
||
127 | |||
128 | d8caf546 | Priya | navigationMap::get_state() |
129 | { |
||
130 | aa28526f | James Carroll | return curr_state;
|
131 | d8caf546 | Priya | } |
132 | |||
133 | 0b6591e2 | Priya | navigationMap::get_outbound_edges(State state) |
134 | { |
||
135 | aa28526f | James Carroll | return map(state-1); |
136 | 0b6591e2 | Priya | } |
137 | |||
138 | d8caf546 | Priya | navigationMap::shortest_path(State target_state) |
139 | { |
||
140 | // BFS algorithm
|
||
141 | 93210a92 | Leon | State curr_state = get_state(); |
142 | char visited[MAX_NODES+1]; |
||
143 | aa5e4ddc | Leon | queue<State> q; |
144 | q.push(curr_state); |
||
145 | // not zero = visited, zero = unvisited, negative = start state
|
||
146 | visited[curr_state] = -1;
|
||
147 | 93210a92 | Leon | |
148 | aa5e4ddc | Leon | while (!q.is_empty)
|
149 | 93210a92 | Leon | { |
150 | aa5e4ddc | Leon | State state = q.pop(); |
151 | 93210a92 | Leon | if (state == target_state)
|
152 | { |
||
153 | aa5e4ddc | Leon | Turn moves_list[MAX_NODES]; |
154 | int j = 0; // counter |
||
155 | for(State child = state; visited[child] >= 0; |
||
156 | child = visited[child]) //while not start state
|
||
157 | 93210a92 | Leon | { |
158 | aa5e4ddc | Leon | State parent = visited[child]; |
159 | Edge* edges = get_outbound_edges(parent); |
||
160 | for (int i = 0; i < ARRAY_SIZE; i++) |
||
161 | { |
||
162 | if (GET_EDGE_STATE(edges[i]) == child)
|
||
163 | { |
||
164 | moves_list[j] = GET_EDGE_DIR(edges[i]); |
||
165 | j++; |
||
166 | break;
|
||
167 | } |
||
168 | } |
||
169 | 93210a92 | Leon | } |
170 | return moves_list;
|
||
171 | } |
||
172 | Edge* edges = get_outbound_edges(state); |
||
173 | int length = sizeof(edges) / sizeof(Edge); |
||
174 | for (int i = 0; i < length; i++) |
||
175 | { |
||
176 | State new_state = GET_EDGE_STATE(edges[i]); |
||
177 | if (!visited[new_state])
|
||
178 | { |
||
179 | aa5e4ddc | Leon | // set visited to the parent of the new state
|
180 | 93210a92 | Leon | visited[new_state] = state; |
181 | aa5e4ddc | Leon | q.push(new_state); |
182 | 93210a92 | Leon | } |
183 | } |
||
184 | } |
||
185 | aa5e4ddc | Leon | //oops, no way to get to target from state
|
186 | return NULL; |
||
187 | d8caf546 | Priya | } |