scoutos / scout / libscout / src / behaviors / navigationMap.cpp @ d140fd71
History  View  Annotate  Download (10.7 KB)
1 
/**


2 
* Copyright (c) 2011 Colony Project

3 
*

4 
* Permission is hereby granted, free of charge, to any person

5 
* obtaining a copy of this software and associated documentation

6 
* files (the "Software"), to deal in the Software without

7 
* restriction, including without limitation the rights to use,

8 
* copy, modify, merge, publish, distribute, sublicense, and/or sell

9 
* copies of the Software, and to permit persons to whom the

10 
* Software is furnished to do so, subject to the following

11 
* conditions:

12 
*

13 
* The above copyright notice and this permission notice shall be

14 
* included in all copies or substantial portions of the Software.

15 
*

16 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

17 
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

18 
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

19 
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

20 
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

21 
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

22 
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

23 
* OTHER DEALINGS IN THE SOFTWARE.

24 
*/

25  
26 
/**

27 
* @file navigationMap.cpp

28 
* @brief Contains navigation map Behavior declarations and definitions

29 
*

30 
* Contains functions and definitions for the use of

31 
* navigation map Behavior

32 
*

33 
* @author Colony Project, CMU Robotics Club

34 
* @author Priya Deo

35 
* @author Lalitha

36 
* @author James

37 
* @author Leon

38 
**/

39  
40 
#include "navigationMap.h" 
41  
42 
using namespace std; 
43  
44 
/**

45 
* @brief Initializes the navigation map

46 
*

47 
* Initialize the navigation map.

48 
* The map itself is represented as a dynamic array of edge arrays

49 
* @param the string name of the scout

50 
*/

51 
navigationMap::navigationMap(string scoutname, Sensors* sensors) :

52 
Behavior(scoutname, "navigationMap", sensors)

53 
{ 
54 
/** Initialize Map

55 
* _____

56 
* 1 2   3 4

57 
* >

58 
* <5678

59 
*    

60 
* 9 10 11 12

61 
*    

62 
* 13 14 15 16

63 
*/

64 

65 
Edge* a1 = new Edge[ARRAY_SIZE];

66 
a1[0] = MAKE_EDGE(ISTRAIGHT, 2, 10); 
67 
a1[1] = MAKE_EDGE(IRIGHT, 13, 40); 
68 
a1[2] = MAKE_EDGE(IUTURN, DEADEND, 0); 
69  
70 
Edge* a2 = new Edge[ARRAY_SIZE];

71 
a2[0] = MAKE_EDGE(ISTRAIGHT, 3, 10); 
72 
a2[1] = MAKE_EDGE(IRIGHT, 14, 40); 
73 
a2[2] = MAKE_EDGE(IUTURN, 5, 10); 
74  
75 
Edge* a3 = new Edge[ARRAY_SIZE];

76 
a3[0] = MAKE_EDGE(ISTRAIGHT, 4, 10); 
77 
a3[1] = MAKE_EDGE(IRIGHT, 15, 40); 
78 
a3[2] = MAKE_EDGE(IUTURN, 6, 10); 
79  
80 
Edge* a4 = new Edge[ARRAY_SIZE];

81 
a4[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0); 
82 
a4[1] = MAKE_EDGE(IRIGHT, 16, 40); 
83 
a4[2] = MAKE_EDGE(IUTURN, 7, 10); 
84  
85 
Edge* a5 = new Edge[ARRAY_SIZE];

86 
a5[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0); 
87 
a5[1] = MAKE_EDGE(ILEFT, 13, 40); 
88 
a5[2] = MAKE_EDGE(IUTURN, 2, 10); 
89  
90 
Edge* a6 = new Edge[ARRAY_SIZE];

91 
a6[0] = MAKE_EDGE(ISTRAIGHT, 5, 0); 
92 
a6[1] = MAKE_EDGE(ILEFT, 14, 40); 
93 
a6[2] = MAKE_EDGE(IUTURN, 3, 10); 
94  
95 
Edge* a7 = new Edge[ARRAY_SIZE];

96 
a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0); 
97 
a7[1] = MAKE_EDGE(ILEFT, 15, 40); 
98 
a7[2] = MAKE_EDGE(IUTURN, 4, 10); 
99  
100 
Edge* a8 = new Edge[ARRAY_SIZE];

101 
a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0); 
102 
a8[1] = MAKE_EDGE(ILEFT, 16, 40); 
103 
a8[2] = MAKE_EDGE(IUTURN, DEADEND, 10); 
104  
105 
Edge* a9 = new Edge[ARRAY_SIZE];

106 
a9[0] = MAKE_EDGE(IRIGHT, 2, 10); 
107 
a9[1] = MAKE_EDGE(ILEFT, DEADEND, 0); 
108 
a9[2] = MAKE_EDGE(ISPOTTURN, 13, 40); 
109  
110 
Edge* a10 = new Edge[ARRAY_SIZE];

111 
a10[0] = MAKE_EDGE(IRIGHT, 3, 10); 
112 
a10[1] = MAKE_EDGE(ILEFT, 5, 10); 
113 
a10[2] = MAKE_EDGE(ISPOTTURN, 14, 40); 
114  
115 
Edge* a11 = new Edge[ARRAY_SIZE];

116 
a11[0] = MAKE_EDGE(IRIGHT, 4, 10); 
117 
a11[1] = MAKE_EDGE(ILEFT, 6, 10); 
118 
a11[2] = MAKE_EDGE(ISPOTTURN, 15, 40); 
119  
120 
Edge* a12 = new Edge[ARRAY_SIZE];

121 
a12[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); 
122 
a12[1] = MAKE_EDGE(ILEFT, 7, 10); 
123 
a12[2] = MAKE_EDGE(ISPOTTURN, 16, 40); 
124  
125 
Edge* a13 = new Edge[ARRAY_SIZE];

126 
a13[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); 
127 
a13[1] = MAKE_EDGE(ILEFT, DEADEND, 0); 
128 
a13[2] = MAKE_EDGE(ISPOTTURN, 9, 40); 
129  
130 
Edge* a14 = new Edge[ARRAY_SIZE];

131 
a14[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); 
132 
a14[1] = MAKE_EDGE(ILEFT, DEADEND, 0); 
133 
a14[2] = MAKE_EDGE(ISPOTTURN, 10, 40); 
134  
135 
Edge* a15 = new Edge[ARRAY_SIZE];

136 
a15[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); 
137 
a15[1] = MAKE_EDGE(ILEFT, DEADEND, 0); 
138 
a15[2] = MAKE_EDGE(ISPOTTURN, 11, 40); 
139  
140 
Edge* a16 = new Edge[ARRAY_SIZE];

141 
a16[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); 
142 
a16[1] = MAKE_EDGE(ILEFT, DEADEND, 0); 
143 
a16[2] = MAKE_EDGE(ISPOTTURN, 12, 40); 
144  
145  
146 
map.push_back(a1); 
147 
map.push_back(a2); 
148 
map.push_back(a3); 
149 
map.push_back(a4); 
150 
map.push_back(a5); 
151 
map.push_back(a6); 
152 
map.push_back(a7); 
153 
map.push_back(a8); 
154 
map.push_back(a9); 
155 
map.push_back(a10); 
156 
map.push_back(a11); 
157 
map.push_back(a12); 
158 
map.push_back(a13); 
159 
map.push_back(a14); 
160 
map.push_back(a15); 
161 
map.push_back(a16); 
162  
163 
curr_state = START_STATE; 
164 
arrival_time = ros::TIME_MAX; 
165 
} 
166  
167 
/** @brief Goes through and frees all allocated memory */

168 
navigationMap::~navigationMap() 
169 
{ 
170 
while(!map.empty())

171 
{ 
172 
Edge* temp = map.back(); 
173 
map.pop_back(); 
174 
delete temp;

175 
} 
176 
return;

177 
} 
178  
179 
/** @brief FSM implementation*/

180 
void navigationMap::run()

181 
{ 
182 
Duration t; 
183  
184 
ROS_INFO("Going to state 16\n");

185 
Path path = shortest_path(16);

186 
if(path.path == NULL) 
187 
{ 
188 
ROS_WARN("There is no path to state 16");

189 
return;

190 
} 
191  
192 
ROS_INFO("Worst case time to 16 is %d", get_worst_case_time(curr_state, 6).sec); 
193  
194 
for(int i=0; i<path.len; i++) 
195 
{ 
196 
update_state(path.path[i]); 
197 
ROS_INFO("Made turn %d, at state %d\n", path.path[i], curr_state);

198 
t = get_time_remaining(); 
199 
while(t.sec > 0) 
200 
t = get_time_remaining(); 
201 
ROS_INFO("Now at state %d\n", curr_state);

202 
} 
203  
204 
ROS_INFO("Traveled route!\n");

205  
206 
while(ok())

207 
continue;

208 
} 
209  
210 
/**@brief sets the current state to the state associated with the turn made

211 
* @param the Turn that we made from our state

212 
* @return our new State after making the turn

213 
*/

214 
State navigationMap::update_state(Turn turn_made) 
215 
{ 
216 
Edge* possible_edges = get_outbound_edges(curr_state); 
217 
for(int i=0;i<ARRAY_SIZE;i++) 
218 
{ 
219 
//sets the current state to the state associated with the turn made

220 
if(GET_EDGE_DIR(possible_edges[i]) == turn_made)

221 
{ 
222 
//TODO: get actual speed

223 
int speed = 10; 
224 
curr_state = GET_EDGE_STATE(possible_edges[i]); 
225 
Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed); 
226 
arrival_time = Time::now() + travel_time; 
227 
return curr_state;

228 
} 
229 
} 
230 
return 1;//failure to succeed 
231 
} 
232  
233 
/**@brief returns the predicted time of arrival for our current task

234 
* @return the predicted time of arrival for our current task

235 
*/

236 
Time navigationMap::get_eta() 
237 
{ 
238 
return arrival_time;

239 
} 
240  
241 
/**@brief returns the predicted amount of time it will take to finish our task

242 
* @return the predicted amount of time it will take to finish our task

243 
*/

244 
Duration navigationMap::get_time_remaining() 
245 
{ 
246 
return (arrival_time  Time::now());

247 
} 
248  
249 
/**@brief returns the current state of the scout in the map

250 
* @return the current State (ie: int) of the scout in the map

251 
*/

252 
State navigationMap::get_state() 
253 
{ 
254 
return curr_state;

255 
} 
256  
257 
/**@brief returns the Edges connecting from a given State

258 
* @param the State whose edges we want to get

259 
* @return the Edges connecting from the given State

260 
*/

261 
Edge* navigationMap::get_outbound_edges(State state) 
262 
{ 
263 
return map.at(state1); 
264 
} 
265  
266 
/**@brief uses BFS to find the shortest path to a target State node

267 
* @param target_state the State that we want to get to

268 
* @return a Path struct containing the Turn* to take to get to the target state

269 
*/

270 
Path navigationMap::shortest_path(State target_state) 
271 
{ 
272 
return shortest_path(curr_state, target_state);

273 
} 
274  
275 
/**@brief uses BFS to find the shortest path to a target State node

276 
* @param start_state the State that we start from

277 
* @param target_state the State that we want to get to

278 
* @return a Path struct containing the Turn* to take to get to the target state

279 
*/

280 
Path navigationMap::shortest_path(State start_state, State target_state) 
281 
{ 
282 
// BFS algorithm

283 
State curr_state = start_state; 
284 
int visited[MAX_NODES+1] = {0}; 
285  
286 
queue<State> q; 
287 
q.push(curr_state); 
288 
// not zero = visited, zero = unvisited, negative = start state

289 
visited[curr_state] = 1;

290  
291 
while (!q.empty())

292 
{ 
293 
State state = q.front(); 
294 
//actually dequeue it

295 
q.pop(); 
296 
if (state == target_state)

297 
{ 
298 
Path path; 
299 
path.path = (Turn*)calloc(sizeof(Turn), MAX_NODES);

300 
int j = 0; // counter 
301 
for(State child = state; visited[child] >= 0; 
302 
child = visited[child]) //while not start state

303 
{ 
304 
State parent = visited[child]; 
305 
Edge* edges = get_outbound_edges(parent); 
306 
for (int i = 0; i < ARRAY_SIZE; i++) 
307 
{ 
308 
if (GET_EDGE_STATE(edges[i]) == child)

309 
{ 
310 
path.path[j] = GET_EDGE_DIR(edges[i]); 
311 
j++; 
312 
break;

313 
} 
314 
} 
315 
} 
316 
/** Reverse moves list */

317 
for (int i = 0; i < j/2; i++) 
318 
{ 
319 
path.path[i] ^= path.path[ji1];

320 
path.path[ji1] ^= path.path[i];

321 
path.path[i] ^= path.path[ji1];

322 
} 
323 
path.len = j; 
324 
return path;

325 
} 
326 
Edge* edges = get_outbound_edges(state); 
327  
328 
for (int i = 0; i < ARRAY_SIZE; i++) 
329 
{ 
330 
State new_state = GET_EDGE_STATE(edges[i]); 
331 
if (new_state != DEADEND && !visited[new_state])

332 
{ 
333 
// set this state in visited as the parent of the new_state

334 
visited[new_state] = state; 
335 
q.push(new_state); 
336 
} 
337 
} 
338 
} 
339 
//oops, no way to get to target from state

340 
Path path; 
341 
path.len = 0;

342 
path.path = NULL;

343 
return path;

344 
} 
345  
346 
/** @brief returns worst case time to travel to dest

347 
*

348 
* Takes into account speed of robot and interactions with other robots

349 
*

350 
* @param start_state Node that we start at

351 
* @param target_state Node that we end up at

352 
* @return the worst case time to travel to target node

353 
*/

354 
Duration navigationMap::get_worst_case_time(State start_state, State target_state) 
355 
{ 
356 
Path path = shortest_path(start_state, target_state); 
357 
Duration worst_case_time(0);

358  
359 
State curr_state = start_state; 
360 
//keep iterating over path while there are turns

361 
for(int i=0; i<path.len; i++) 
362 
{ 
363 
Edge* edges = get_outbound_edges(curr_state); 
364 
for(int j=0; j<ARRAY_SIZE; j++) 
365 
{ 
366 
if(GET_EDGE_DIR(edges[j]) == path.path[i])

367 
{ 
368 
Duration turn_time(TURN_TIME + (GET_EDGE_DIST(edges[j])/SPEED)); 
369 
worst_case_time += turn_time; 
370 
} 
371 
} 
372 
} 
373 
Duration additional_time(DROPOFF_TIME + WAIT_TIME); 
374 
worst_case_time += additional_time; 
375  
376 
return worst_case_time;

377 
} 