Revision 339f64d2 scout/libscout/src/test_behaviors/smart_runaround.cpp

View differences:

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