Revision cc399ab3
| ID | cc399ab319f31b645b41e8adec17a4d2061166cf |
added get_worst_case_time to nav maps
| scout/libscout/src/behaviors/navigationMap.cpp | ||
|---|---|---|
| 159 | 159 |
update_state(ISTRAIGHT); |
| 160 | 160 |
ROS_INFO("My state is: %d\n", curr_state);
|
| 161 | 161 |
t = get_time_remaining(); |
| 162 |
while(t.toSec() > 0)
|
|
| 162 |
while(t.sec > 0)
|
|
| 163 | 163 |
t = get_time_remaining(); |
| 164 | 164 |
update_state(ISTRAIGHT); |
| 165 | 165 |
ROS_INFO("My state is: %d\n", curr_state);
|
| 166 | 166 |
t = get_time_remaining(); |
| 167 |
while(t.toSec() > 0)
|
|
| 167 |
while(t.sec > 0)
|
|
| 168 | 168 |
t = get_time_remaining(); |
| 169 | 169 |
update_state(IRIGHT); |
| 170 | 170 |
ROS_INFO("My state is: %d\n", curr_state);
|
| 171 | 171 |
t = get_time_remaining(); |
| 172 |
while(t.toSec() > 0)
|
|
| 172 |
while(t.sec > 0)
|
|
| 173 | 173 |
t = get_time_remaining(); |
| 174 | 174 |
update_state(ILEFT); |
| 175 | 175 |
ROS_INFO("My state is: %d\n", curr_state);
|
| 176 | 176 |
t = get_time_remaining(); |
| 177 |
while(t.toSec() > 0)
|
|
| 177 |
while(t.sec > 0)
|
|
| 178 | 178 |
t = get_time_remaining(); |
| 179 | 179 |
update_state(ISTRAIGHT); |
| 180 | 180 |
ROS_INFO("My state is: %d\n", curr_state);
|
| 181 | 181 |
t = get_time_remaining(); |
| 182 |
while(t.toSec() > 0)
|
|
| 182 |
while(t.sec > 0)
|
|
| 183 | 183 |
t = get_time_remaining(); |
| 184 | 184 |
ROS_INFO("Traveled route!\n");
|
| 185 | 185 |
|
| ... | ... | |
| 188 | 188 |
if(path.path == NULL) |
| 189 | 189 |
return; |
| 190 | 190 |
|
| 191 |
ROS_INFO("Now at state %d\n", curr_state);
|
|
| 191 |
ROS_INFO("Worst case time to 6 is %d", get_worst_case_time(curr_state, 6).sec);
|
|
| 192 |
|
|
| 192 | 193 |
for(int i=0; i<path.len; i++) |
| 193 | 194 |
{
|
| 194 | 195 |
update_state(path.path[i]); |
| 195 | 196 |
ROS_INFO("Made turn %d, at state %d\n", path.path[i], curr_state);
|
| 196 | 197 |
t = get_time_remaining(); |
| 197 |
while(t.toSec() > 0)
|
|
| 198 |
while(t.sec > 0)
|
|
| 198 | 199 |
t = get_time_remaining(); |
| 199 | 200 |
ROS_INFO("Now at state %d\n", curr_state);
|
| 200 | 201 |
} |
| ... | ... | |
| 261 | 262 |
} |
| 262 | 263 |
|
| 263 | 264 |
/**@brief uses BFS to find the shortest path to a target State node |
| 264 |
* @param the State that we want to get to |
|
| 265 |
* @param target_state the State that we want to get to
|
|
| 265 | 266 |
* @return a Path struct containing the Turn* to take to get to the target state |
| 266 | 267 |
*/ |
| 267 | 268 |
Path navigationMap::shortest_path(State target_state) |
| 268 | 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 |
{
|
|
| 269 | 280 |
// BFS algorithm |
| 270 |
State curr_state = get_state();
|
|
| 281 |
State curr_state = start_state;
|
|
| 271 | 282 |
int visited[MAX_NODES+1] = {0};
|
| 272 | 283 |
|
| 273 | 284 |
queue<State> q; |
| ... | ... | |
| 330 | 341 |
return path; |
| 331 | 342 |
} |
| 332 | 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; |
|
| 333 | 373 |
|
| 374 |
return worst_case_time; |
|
| 375 |
} |
|
Also available in: Unified diff