scoutos / scout / libscout / src / behaviors / navigationMap.cpp @ afa9104d
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) : Behavior(scoutname, "navigationMap") 
52 
{ 
53 
/** Initialize Map

54 
*

55 
* 1 2 3 4

56 
* >

57 
* <5678

58 
*    

59 
* 9 10 11 12

60 
*    

61 
*    

62 
*/

63 

64 
Edge* a1 = new Edge[ARRAY_SIZE];

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

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

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

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

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

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

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

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

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

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

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

120 
a12[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); 
121 
a12[1] = MAKE_EDGE(ILEFT, 7, 10); 
122 
a12[2] = MAKE_EDGE(IUTURN, 12, 40); 
123  
124 
map.push_back(a1); 
125 
map.push_back(a2); 
126 
map.push_back(a3); 
127 
map.push_back(a4); 
128 
map.push_back(a5); 
129 
map.push_back(a6); 
130 
map.push_back(a7); 
131 
map.push_back(a8); 
132 
map.push_back(a9); 
133 
map.push_back(a10); 
134 
map.push_back(a11); 
135 
map.push_back(a12); 
136  
137 
curr_state = START_STATE; 
138 
arrival_time = ros::TIME_MAX; 
139 
} 
140  
141 
/** @brief Goes through and frees all allocated memory */

142 
navigationMap::~navigationMap() 
143 
{ 
144 
while(!map.empty())

145 
{ 
146 
Edge* temp = map.back(); 
147 
map.pop_back(); 
148 
delete temp;

149 
} 
150 
return;

151 
} 
152  
153 
/** @brief FSM implementation*/

154 
void navigationMap::run()

155 
{ 
156 
Duration t; 
157 
ROS_INFO("My state is: %d\n", curr_state);

158 
//Straight, straight, right, left, straight 5

159 
update_state(ISTRAIGHT); 
160 
ROS_INFO("My state is: %d\n", curr_state);

161 
t = get_time_remaining(); 
162 
while(t.sec > 0) 
163 
t = get_time_remaining(); 
164 
update_state(ISTRAIGHT); 
165 
ROS_INFO("My state is: %d\n", curr_state);

166 
t = get_time_remaining(); 
167 
while(t.sec > 0) 
168 
t = get_time_remaining(); 
169 
update_state(IRIGHT); 
170 
ROS_INFO("My state is: %d\n", curr_state);

171 
t = get_time_remaining(); 
172 
while(t.sec > 0) 
173 
t = get_time_remaining(); 
174 
update_state(ILEFT); 
175 
ROS_INFO("My state is: %d\n", curr_state);

176 
t = get_time_remaining(); 
177 
while(t.sec > 0) 
178 
t = get_time_remaining(); 
179 
update_state(ISTRAIGHT); 
180 
ROS_INFO("My state is: %d\n", curr_state);

181 
t = get_time_remaining(); 
182 
while(t.sec > 0) 
183 
t = get_time_remaining(); 
184 
ROS_INFO("Traveled route!\n");

185  
186 
ROS_INFO("Going to state 6\n");

187 
Path path = shortest_path(6);

188 
if(path.path == NULL) 
189 
return;

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

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

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

203  
204 
while(ok())

205 
continue;

206 
} 
207  
208 
/**@brief sets the current state to the state associated with the turn made

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

210 
* @return our new State after making the turn

211 
*/

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

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

219 
{ 
220 
int speed = 10000;//its over 9000 
221 
curr_state = GET_EDGE_STATE(possible_edges[i]); 
222 
//TODO: get actual speed

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

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

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

233 
*/

234 
Time navigationMap::get_eta() 
235 
{ 
236 
return arrival_time;

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

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

241 
*/

242 
Duration navigationMap::get_time_remaining() 
243 
{ 
244 
return (arrival_time  Time::now());

245 
} 
246  
247 
/**@brief returns the current state of the scout in the map

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

249 
*/

250 
State navigationMap::get_state() 
251 
{ 
252 
return curr_state;

253 
} 
254  
255 
/**@brief returns the Edges connecting from a given State

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

257 
* @return the Edges connecting from the given State

258 
*/

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

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

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

267 
*/

268 
Path navigationMap::shortest_path(State target_state) 
269 
{ 
270 
return shortest_path(curr_state, target_state);

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

274 
* @param start_state the State that we start from

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

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

277 
*/

278 
Path navigationMap::shortest_path(State start_state, State target_state) 
279 
{ 
280 
// BFS algorithm

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

287 
visited[curr_state] = 1;

288  
289 
while (!q.empty())

290 
{ 
291 
State state = q.front(); 
292 
//actually dequeue it

293 
q.pop(); 
294 
if (state == target_state)

295 
{ 
296 
Path path; 
297 
path.path = (Turn*)calloc(sizeof(Turn), MAX_NODES);

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

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

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

311 
} 
312 
} 
313 
} 
314 
/** Reverse moves list */

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

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

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

320 
} 
321 
path.len = j; 
322 
return path;

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

330 
{ 
331 
// set this state in visited as the parent of the new_state

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

338 
Path path; 
339 
path.len = 0;

340 
path.path = NULL;

341 
return path;

342 
} 
343  
344 
/** @brief returns worst case time to travel to dest

345 
*

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

347 
*

348 
* @param start_state Node that we start at

349 
* @param target_state Node that we end up at

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

351 
*/

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

356  
357 
State curr_state = start_state; 
358 
//keep iterating over path while there are turns

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

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

375 
} 