Revision 339f64d2
Fixed warnings in libscout behaviors.
Some of these were actually serious errors. Others were little things.
| scout/libscout/src/test_behaviors/smart_runaround.cpp | ||
|---|---|---|
| 290 | 290 |
float right_distance = readings[24]/1000.0; |
| 291 | 291 |
int right_idx = m_to_idx(right_distance); |
| 292 | 292 |
|
| 293 |
ROS_INFO("front: %d left: %d right: %d", front_distance, left_distance, right_distance);
|
|
| 293 |
ROS_INFO("front: %lf left: %lf right: %lf",
|
|
| 294 |
front_distance, left_distance, right_distance); |
|
| 294 | 295 |
if (right_distance == 0 || front_distance == 0 || left_distance == 0) |
| 295 | 296 |
return false; |
| 296 | 297 |
|
| 297 | 298 |
// determine relative distances on map, based on robot position |
| 298 |
int up_d, right_d, down_d, left_d; |
|
| 299 |
int up_d = 0; |
|
| 300 |
int right_d = 0; |
|
| 301 |
int down_d = 0; |
|
| 302 |
int left_d = 0; |
|
| 303 |
|
|
| 299 | 304 |
// determine upward distance |
| 300 | 305 |
switch (dir) |
| 301 | 306 |
{
|
| ... | ... | |
| 331 | 336 |
// map blocks above robot (on map) |
| 332 | 337 |
for(int u = 0; u < 8; u++) |
| 333 | 338 |
{
|
| 334 |
if(u = up_d) {
|
|
| 335 |
map[row-u][col] = (up_d)?WALL:SEEN;
|
|
| 339 |
if(u == up_d) {
|
|
| 340 |
map[row-u][col] = (up_d)?WALL:SEEN;
|
|
| 336 | 341 |
break; |
| 337 |
}
|
|
| 338 |
map[row-u][col] = SEEN;
|
|
| 342 |
}
|
|
| 343 |
map[row-u][col] = SEEN;
|
|
| 339 | 344 |
} |
| 340 | 345 |
|
| 341 | 346 |
// map blocks to right of robot |
| 342 | 347 |
for(int r = 0; r < 8; r++) |
| 343 | 348 |
{
|
| 344 |
if(r = right_d) {
|
|
| 345 |
map[row][col+r] = (right_d)?WALL:SEEN;
|
|
| 346 |
break; |
|
| 347 |
}
|
|
| 348 |
map[row][col+r] = SEEN;
|
|
| 349 |
if(r == right_d) {
|
|
| 350 |
map[row][col+r] = (right_d)?WALL:SEEN;
|
|
| 351 |
break;
|
|
| 352 |
}
|
|
| 353 |
map[row][col+r] = SEEN;
|
|
| 349 | 354 |
} |
| 350 | 355 |
|
| 351 | 356 |
// map blocks under robot (on map) |
| 352 | 357 |
for(int d = 0; d < 8; d++) |
| 353 | 358 |
{
|
| 354 |
if(d = down_d) {
|
|
| 355 |
map[row+d][col] = (down_d)?WALL:SEEN;
|
|
| 356 |
break; |
|
| 357 |
}
|
|
| 358 |
map[row+d][col] = SEEN;
|
|
| 359 |
if(d == down_d) {
|
|
| 360 |
map[row+d][col] = (down_d)?WALL:SEEN;
|
|
| 361 |
break;
|
|
| 362 |
}
|
|
| 363 |
map[row+d][col] = SEEN;
|
|
| 359 | 364 |
} |
| 360 | 365 |
|
| 361 | 366 |
// map blocks to left of robot |
| 362 | 367 |
for(int l = 0; l < 8; l++) |
| 363 | 368 |
{
|
| 364 |
if(l = left_d) {
|
|
| 365 |
map[row][col-l] = (left_d)?WALL:SEEN;
|
|
| 366 |
break; |
|
| 367 |
}
|
|
| 368 |
map[row][col-l] = SEEN;
|
|
| 369 |
if(l == left_d) {
|
|
| 370 |
map[row][col-l] = (left_d)?WALL:SEEN;
|
|
| 371 |
break;
|
|
| 372 |
}
|
|
| 373 |
map[row][col-l] = SEEN;
|
|
| 369 | 374 |
} |
| 370 | 375 |
|
| 371 | 376 |
return true; |
| ... | ... | |
| 378 | 383 |
{
|
| 379 | 384 |
// check whether there is a square portion with |
| 380 | 385 |
// only SEEN or WALL blocks, of the size of the map. |
| 381 |
int start_row = -1;
|
|
| 382 |
int start_col = -1;
|
|
| 386 |
//int start_row = -1; // Unused
|
|
| 387 |
//int start_col = -1; // Unused
|
|
| 383 | 388 |
int seen_width = 0; |
| 384 | 389 |
int seen_height = 0; |
| 385 | 390 |
for(int r = 0; r < MAP_LENGTH; r++) {
|
| 386 |
for(int c = 0; c < MAP_LENGTH; c++) {
|
|
| 387 |
if(map[r][c] == UNSEEN) {
|
|
| 388 |
start_row = -1;
|
|
| 389 |
start_col = -1;
|
|
| 390 |
seen_width = 0;
|
|
| 391 |
seen_height = 0;
|
|
| 392 |
}
|
|
| 393 |
else {
|
|
| 394 |
start_row = r;
|
|
| 395 |
start_col = c;
|
|
| 396 |
seen_width++;
|
|
| 397 |
seen_height++;
|
|
| 398 |
}
|
|
| 399 |
if(seen_width >= 15)
|
|
| 400 |
return true;
|
|
| 401 |
}
|
|
| 391 |
for(int c = 0; c < MAP_LENGTH; c++) {
|
|
| 392 |
if(map[r][c] == UNSEEN) {
|
|
| 393 |
//start_row = -1;
|
|
| 394 |
//start_col = -1;
|
|
| 395 |
seen_width = 0;
|
|
| 396 |
seen_height = 0;
|
|
| 397 |
}
|
|
| 398 |
else {
|
|
| 399 |
//start_row = r;
|
|
| 400 |
//start_col = c;
|
|
| 401 |
seen_width++;
|
|
| 402 |
seen_height++;
|
|
| 403 |
}
|
|
| 404 |
if(seen_width >= 15)
|
|
| 405 |
return true;
|
|
| 406 |
}
|
|
| 402 | 407 |
} |
| 403 | 408 |
return false; |
| 404 | 409 |
} |
Also available in: Unified diff