## root / scout / libscout / src / behaviors / navigationMap.cpp @ dcf49526

History | View | Annotate | Download (10.7 KB)

1 | 2d697b1f | Leon | ```
/**
``` |
---|---|---|---|

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 | d8caf546 | Priya | #include "navigationMap.h" |

41 | |||

42 | cccc25c9 | Priya | using namespace std; |

43 | |||

44 | 2d697b1f | Leon | ```
/**
``` |

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 | d140fd71 | Yuyang | ```
navigationMap::navigationMap(string scoutname, Sensors* sensors) :
``` |

52 | ```
Behavior(scoutname, "navigationMap", sensors)
``` |
||

53 | d8caf546 | Priya | { |

54 | ```
/** Initialize Map
``` |
||

55 | 11aa087a | Priya | ```
* _____
``` |

56 | ```
* 1 2 | | 3 4
``` |
||

57 | ```
* ----|-----------|--|----|--|---------|---------->
``` |
||

58 | d8caf546 | Priya | ```
* <---|--5--------|--6-------|--7------|--8-------
``` |

59 | ```
* | | | |
``` |
||

60 | ```
* 9| 10| 11| 12|
``` |
||

61 | ```
* | | | |
``` |
||

62 | 11aa087a | Priya | ```
* ---13 ---14 ---15 ---16
``` |

63 | d8caf546 | Priya | ```
*/
``` |

64 | |||

65 | 738e44fb | Priya | ```
Edge* a1 = new Edge[ARRAY_SIZE];
``` |

66 | a1[0] = MAKE_EDGE(ISTRAIGHT, 2, 10); |
||

67 | 58c19c15 | Priya | a1[1] = MAKE_EDGE(IRIGHT, 13, 40); |

68 | 738e44fb | Priya | a1[2] = MAKE_EDGE(IUTURN, DEADEND, 0); |

69 | |||

70 | ```
Edge* a2 = new Edge[ARRAY_SIZE];
``` |
||

71 | a2[0] = MAKE_EDGE(ISTRAIGHT, 3, 10); |
||

72 | 58c19c15 | Priya | a2[1] = MAKE_EDGE(IRIGHT, 14, 40); |

73 | 738e44fb | Priya | a2[2] = MAKE_EDGE(IUTURN, 5, 10); |

74 | |||

75 | ```
Edge* a3 = new Edge[ARRAY_SIZE];
``` |
||

76 | a3[0] = MAKE_EDGE(ISTRAIGHT, 4, 10); |
||

77 | 58c19c15 | Priya | a3[1] = MAKE_EDGE(IRIGHT, 15, 40); |

78 | 738e44fb | Priya | a3[2] = MAKE_EDGE(IUTURN, 6, 10); |

79 | |||

80 | ```
Edge* a4 = new Edge[ARRAY_SIZE];
``` |
||

81 | a4[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0); |
||

82 | 58c19c15 | Priya | a4[1] = MAKE_EDGE(IRIGHT, 16, 40); |

83 | 738e44fb | Priya | a4[2] = MAKE_EDGE(IUTURN, 7, 10); |

84 | |||

85 | ```
Edge* a5 = new Edge[ARRAY_SIZE];
``` |
||

86 | a5[0] = MAKE_EDGE(ISTRAIGHT, DEADEND, 0); |
||

87 | 58c19c15 | Priya | a5[1] = MAKE_EDGE(ILEFT, 13, 40); |

88 | 738e44fb | Priya | a5[2] = MAKE_EDGE(IUTURN, 2, 10); |

89 | |||

90 | ```
Edge* a6 = new Edge[ARRAY_SIZE];
``` |
||

91 | a6[0] = MAKE_EDGE(ISTRAIGHT, 5, 0); |
||

92 | 58c19c15 | Priya | a6[1] = MAKE_EDGE(ILEFT, 14, 40); |

93 | 738e44fb | Priya | a6[2] = MAKE_EDGE(IUTURN, 3, 10); |

94 | |||

95 | ```
Edge* a7 = new Edge[ARRAY_SIZE];
``` |
||

96 | a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0); |
||

97 | 58c19c15 | Priya | a7[1] = MAKE_EDGE(ILEFT, 15, 40); |

98 | afa9104d | Priya | a7[2] = MAKE_EDGE(IUTURN, 4, 10); |

99 | 738e44fb | Priya | |

100 | ```
Edge* a8 = new Edge[ARRAY_SIZE];
``` |
||

101 | a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0); |
||

102 | 58c19c15 | Priya | a8[1] = MAKE_EDGE(ILEFT, 16, 40); |

103 | 738e44fb | Priya | 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 | 58c19c15 | Priya | a9[2] = MAKE_EDGE(ISPOTTURN, 13, 40); |

109 | 738e44fb | Priya | |

110 | ```
Edge* a10 = new Edge[ARRAY_SIZE];
``` |
||

111 | a10[0] = MAKE_EDGE(IRIGHT, 3, 10); |
||

112 | a10[1] = MAKE_EDGE(ILEFT, 5, 10); |
||

113 | 58c19c15 | Priya | a10[2] = MAKE_EDGE(ISPOTTURN, 14, 40); |

114 | 738e44fb | Priya | |

115 | ```
Edge* a11 = new Edge[ARRAY_SIZE];
``` |
||

116 | a11[0] = MAKE_EDGE(IRIGHT, 4, 10); |
||

117 | a11[1] = MAKE_EDGE(ILEFT, 6, 10); |
||

118 | 58c19c15 | Priya | a11[2] = MAKE_EDGE(ISPOTTURN, 15, 40); |

119 | 738e44fb | Priya | |

120 | ```
Edge* a12 = new Edge[ARRAY_SIZE];
``` |
||

121 | a12[0] = MAKE_EDGE(IRIGHT, DEADEND, 0); |
||

122 | a12[1] = MAKE_EDGE(ILEFT, 7, 10); |
||

123 | 58c19c15 | Priya | 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 | 738e44fb | Priya | |

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 | 58c19c15 | Priya | map.push_back(a13); |

159 | map.push_back(a14); |
||

160 | map.push_back(a15); |
||

161 | map.push_back(a16); |
||

162 | 738e44fb | Priya | |

163 | curr_state = START_STATE; |
||

164 | cccc25c9 | Priya | arrival_time = ros::TIME_MAX; |

165 | d8caf546 | Priya | } |

166 | |||

167 | ```
/** @brief Goes through and frees all allocated memory */
``` |
||

168 | navigationMap::~navigationMap() |
||

169 | { |
||

170 | 738e44fb | Priya | ```
while(!map.empty())
``` |

171 | { |
||

172 | Edge* temp = map.back(); |
||

173 | map.pop_back(); |
||

174 | ```
delete temp;
``` |
||

175 | } |
||

176 | ```
return;
``` |
||

177 | d8caf546 | Priya | } |

178 | |||

179 | 2d697b1f | Leon | ```
/** @brief FSM implementation*/
``` |

180 | cccc25c9 | Priya | ```
void navigationMap::run()
``` |

181 | d8caf546 | Priya | { |

182 | cccc25c9 | Priya | Duration t; |

183 | 6642eee3 | Priya | |

184 | 58c19c15 | Priya | ```
ROS_INFO("Going to state 16\n");
``` |

185 | ```
Path path = shortest_path(16);
``` |
||

186 | 6642eee3 | Priya | if(path.path == NULL) |

187 | 58c19c15 | Priya | { |

188 | ```
ROS_WARN("There is no path to state 16");
``` |
||

189 | 6642eee3 | Priya | ```
return;
``` |

190 | 58c19c15 | Priya | } |

191 | 6642eee3 | Priya | |

192 | 58c19c15 | Priya | ROS_INFO("Worst case time to 16 is %d", get_worst_case_time(curr_state, 6).sec); |

193 | cc399ab3 | Priya | |

194 | dcf49526 | Priya | for(int i=0; i<path.len && ok(); i++) |

195 | 6642eee3 | Priya | { |

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 | cc399ab3 | Priya | while(t.sec > 0) |

200 | 6642eee3 | Priya | t = get_time_remaining(); |

201 | ```
ROS_INFO("Now at state %d\n", curr_state);
``` |
||

202 | } |
||

203 | 58c19c15 | Priya | |

204 | 6642eee3 | Priya | ```
ROS_INFO("Traveled route!\n");
``` |

205 | |||

206 | cccc25c9 | Priya | ```
while(ok())
``` |

207 | ```
continue;
``` |
||

208 | d8caf546 | Priya | } |

209 | |||

210 | 2d697b1f | Leon | ```
/**@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 | cccc25c9 | Priya | State navigationMap::update_state(Turn turn_made) |

215 | ede03b64 | James Carroll | { |

216 | 57e82e6c | James Carroll | Edge* possible_edges = get_outbound_edges(curr_state); |

217 | 6642eee3 | Priya | for(int i=0;i<ARRAY_SIZE;i++) |

218 | 738e44fb | Priya | { |

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 | 58c19c15 | Priya | int speed = 10; |

224 | curr_state = GET_EDGE_STATE(possible_edges[i]); |
||

225 | cccc25c9 | Priya | Duration travel_time(GET_EDGE_DIST(possible_edges[i])/speed); |

226 | arrival_time = Time::now() + travel_time; |
||

227 | 57e82e6c | James Carroll | ```
return curr_state;
``` |

228 | 738e44fb | Priya | } |

229 | } |
||

230 | return -1;//failure to succeed |
||

231 | ede03b64 | James Carroll | } |

232 | |||

233 | 2d697b1f | Leon | ```
/**@brief returns the predicted time of arrival for our current task
``` |

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

235 | ```
*/
``` |
||

236 | cccc25c9 | Priya | Time navigationMap::get_eta() |

237 | ede03b64 | James Carroll | { |

238 | 738e44fb | Priya | ```
return arrival_time;
``` |

239 | ede03b64 | James Carroll | } |

240 | |||

241 | 2d697b1f | Leon | ```
/**@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 | cccc25c9 | Priya | Duration navigationMap::get_time_remaining() |

245 | ede03b64 | James Carroll | { |

246 | cccc25c9 | Priya | ```
return (arrival_time - Time::now());
``` |

247 | ede03b64 | James Carroll | } |

248 | |||

249 | 2d697b1f | Leon | ```
/**@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 | cccc25c9 | Priya | State navigationMap::get_state() |

253 | d8caf546 | Priya | { |

254 | 738e44fb | Priya | ```
return curr_state;
``` |

255 | d8caf546 | Priya | } |

256 | |||

257 | 2d697b1f | Leon | ```
/**@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 | cccc25c9 | Priya | Edge* navigationMap::get_outbound_edges(State state) |

262 | 0b6591e2 | Priya | { |

263 | cccc25c9 | Priya | return map.at(state-1); |

264 | 0b6591e2 | Priya | } |

265 | |||

266 | 2d697b1f | Leon | ```
/**@brief uses BFS to find the shortest path to a target State node
``` |

267 | cc399ab3 | Priya | ```
* @param target_state the State that we want to get to
``` |

268 | 2d697b1f | Leon | ```
* @return a Path struct containing the Turn* to take to get to the target state
``` |

269 | ```
*/
``` |
||

270 | 738e44fb | Priya | Path navigationMap::shortest_path(State target_state) |

271 | d8caf546 | Priya | { |

272 | cc399ab3 | Priya | ```
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 | 738e44fb | Priya | ```
// BFS algorithm
``` |

283 | cc399ab3 | Priya | State curr_state = start_state; |

284 | 738e44fb | Priya | 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 | 6642eee3 | Priya | { |

298 | 738e44fb | Priya | Path path; |

299 | 6642eee3 | Priya | ```
path.path = (Turn*)calloc(sizeof(Turn), MAX_NODES);
``` |

300 | 738e44fb | Priya | 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 | 93210a92 | Leon | { |

308 | 738e44fb | Priya | ```
if (GET_EDGE_STATE(edges[i]) == child)
``` |

309 | { |
||

310 | path.path[j] = GET_EDGE_DIR(edges[i]); |
||

311 | j++; |
||

312 | ```
break;
``` |
||

313 | } |
||

314 | 93210a92 | Leon | } |

315 | 738e44fb | Priya | } |

316 | ```
/** Reverse moves list */
``` |
||

317 | for (int i = 0; i < j/2; i++) |
||

318 | { |
||

319 | ```
path.path[i] ^= path.path[j-i-1];
``` |
||

320 | ```
path.path[j-i-1] ^= path.path[i];
``` |
||

321 | ```
path.path[i] ^= path.path[j-i-1];
``` |
||

322 | } |
||

323 | path.len = j; |
||

324 | ```
return path;
``` |
||

325 | } |
||

326 | Edge* edges = get_outbound_edges(state); |
||

327 | 6642eee3 | Priya | |

328 | 738e44fb | Priya | 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 | 2d697b1f | Leon | ```
// set this state in visited as the parent of the new_state
``` |

334 | 738e44fb | Priya | visited[new_state] = state; |

335 | q.push(new_state); |
||

336 | } |
||

337 | 93210a92 | Leon | } |

338 | 738e44fb | Priya | } |

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 | d8caf546 | Priya | } |

345 | 738e44fb | Priya | |

346 | cc399ab3 | Priya | ```
/** @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 | 738e44fb | Priya | |

376 | cc399ab3 | Priya | ```
return worst_case_time;
``` |

377 | } |