def test_sparse_cubes(self):
data = [233, 242, 240, 246, 252, 258, 265, 270, 281, 283, 288, 287, 287, 278, 269,
268, 263, 261, 263, 263, 257, 255, 257, 257, 256, 256, 258, 260, 2113,
747, 758, 766, 768, 764, 772, 765, 764, 791, 0, 1769, 1787, 1813, 1821,
1836, 1856, 1868, 1887, 1878, 1859, 1888, 1915, 1921, 1924, 1936, 1975,
1999, 2037, 2057, 2079, 2103, 2112, 2126, 2149, 2163, 2173, 2203, 2225,
2250, 557, 510, 481, 456, 439, 428, 411, 408, 406, 412, 414, 418, 425,
429, 433, 423, 435, 442, 450, 455, 462, 471, 467, 2, 3162, 3221, 3289,
3356, 3434, 3509, 3593, 3659, 3624, 3587, 3639, 3603, 3565, 3531, 3492,
3458, 3434, 3404, 3372, 3349, 3324, 3301, 3280, 3217, 3155, 3138, 3115,
3102, 3086, 3076, 3066, 3055, 3042, 3034, 3031, 3026, 3011, 3010, 3006,
3000, 2997, 2993, 2996, 2997, 2984, 2985, 2990, 2989, 2996, 3005, 3011,
3023, 3031, 3040, 3053, 3067, 3073, 3087, 3100, 3120, 3137, 3157, 1589,
234, 232, 230, 221, 229, 223, 228, 227, 234, 226, 225, 223, 230, 228,
220, 225, 226, 231, 229, 224, 220, 222, 229, 231, 232, 239, 235, 235,
236, 235, 244, 235, 245, 245, 243, 238, 238, 153, 0, 1520, 1502, 1484,
1469, 1460, 1447, 1436, 1428, 1425, 1403, 1399, 1340, 1376, 1368, 1365,
1358, 1345, 1328, 1343, 1339, 1323, 1333, 1311, 1313, 1316, 1301, 1299,
1301, 1303, 1296, 1289, 1275, 1283, 1277, 1287, 1285, 1284, 1279, 1283,
1285, 1287, 1288, 1293, 1298, 1300, 1304, 1312, 1321, 1330, 1330, 1341,
1321, 1350, 1369, 1377, 1390, 1399, 1412, 1422, 1437, 1450, 1466, 1479,
1495, 1514, 1531, 1549, 1572, 1590, 1613, 1634, 1658, 1689, 1719, 1746,
1779, 1820]
cd = CubeDetector(laser_pose_6D = ((0,0,0), (0,0,0)), verbose=True)
cubes = cd.detect_cubes_xy(data)
self.assertEqual(len(cubes), 3, cubes)
def test_near_object(self):
# originally failed due to wrong size estimation
data = [228, 236, 242, 236, 246, 249, 252, 265, 262, 270, 277, 280, 282, 281, 271,
259, 258, 257, 255, 252, 248, 241, 231, 244, 248, 312, 427, 432, 421,
437, 431, 434, 440, 446, 444, 444, 444, 442, 467, 6487, 6463, 6510,
6588, 6306, 6219, 6318, 6326, 0, 4402, 4228, 4172, 2, 0, 3825, 162,
157, 158, 156, 157, 153, 147, 144, 149, 145, 143, 144, 140, 138, 138,
130, 129, 124, 122, 116, 114, 107, 102, 98, 91, 92, 96, 101, 99, 95,
96, 90, 91, 84, 74, 70, 62, 65, 66, 60, 59, 60, 60, 51, 61, 56, 52, 50,
52, 50, 50, 49, 44, 48, 55, 54, 41, 49, 52, 39, 44, 60, 53, 51, 68, 82,
120, 151, 145, 0, 0, 0, 0, 5734, 0, 8908, 0, 0, 0, 0, 4682, 4700, 0,
5329, 5339, 5370, 5280, 4895, 4905, 5019, 5082, 4550, 4514, 5224, 4801,
4849, 5384, 4146, 4198, 4195, 4158, 4118, 4081, 4045, 4023, 3996, 2504,
2475, 2525, 2542, 740, 687, 645, 644, 655, 646, 660, 660, 677, 674,
671, 676, 680, 686, 0, 0, 6124, 6097, 6087, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8061, 0, 0, 0, 586, 586, 584, 582,
571, 573, 568, 578, 574, 576, 561, 565, 553, 552, 542, 598, 683, 0, 0,
2, 4170, 4101, 4147, 4380, 4357, 4318, 4255, 4209, 5911, 0, 0, 0, 2,
3793, 3683, 3626, 3547, 3469, 3421, 3413, 3403, 3408, 3447, 3458, 3483,
3491, 3488, 0, 2621, 2648, 0, 3540, 3599, 3618, 3483, 3391, 2196, 2175,
2223, 2568, 2561, 2518, 2482]
cd = CubeDetector(laser_pose_6D = ((0,0,0), (0,0,0)), verbose=True)
cubes = cd.detect_cubes_xy(data, limit = 1)
self.assertEqual(len(cubes), 1, cubes)
cube_x, cube_y = cubes[0]
self.assertTrue(-0.1 < cube_y < 0.1, cube_y)
def test_bad_cube_edge(self):
data = [238, 241, 246, 250, 249, 258, 269, 278, 283, 287, 290, 289, 288, 278, 267,
268, 270, 264, 261, 257, 253, 253, 253, 256, 255, 253, 252, 256, 221,
1504, 0, 1388, 1348, 1311, 1292, 1260, 1225, 1196, 1177, 1152, 1121,
1108, 1084, 1067, 1050, 1031, 1011, 1001, 985, 973, 954, 949, 934, 923,
914, 896, 894, 889, 876, 863, 856, 847, 838, 828, 826, 828, 815, 813,
803, 798, 795, 784, 785, 786, 778, 783, 776, 767, 768, 763, 763, 778,
760, 774, 756, 753, 752, 754, 753, 756, 756, 756, 755, 754, 762, 759,
763, 767, 782, 772, 785, 773, 779, 783, 785, 788, 793, 797, 802, 804,
811, 813, 824, 835, 839, 848, 854, 861, 870, 878, 889, 901, 912, 923,
929, 940, 958, 974, 987, 1001, 1014, 1032, 1049, 1070, 1085, 1107,
1127, 1154, 1166, 1204, 1224, 1254, 1286, 1307, 1345, 1385, 1426, 1467,
1507, 1556, 1604, 1665, 1723, 1788, 1856, 1928, 2010, 2101, 2202, 2316,
2433, 2575, 2630, 2619, 2604, 2622, 2673, 2665, 2658, 2648, 2638, 2635,
2628, 2625, 2620, 2616, 2610, 2611, 2588, 2503, 2503, 2552, 2607, 2619,
2622, 2617, 2548, 2559, 2568, 2580, 2587, 2595, 2608, 2617, 2628, 2644,
2656, 2678, 2690, 2702, 2729, 2748, 163, 312, 295, 287, 291, 296, 289,
300, 291, 288, 293, 292, 296, 306, 294, 305, 299, 300, 304, 305, 305,
309, 316, 310, 316, 316, 313, 320, 318, 303, 1870, 1834, 1812, 1796,
1813, 1640, 1696, 1688, 1662, 1568, 1551, 1548, 1576, 3139, 3013, 2887,
2869, 2851, 0, 1958, 1950, 1889, 1937, 1966, 1944, 1991, 2019, 0, 8633,
9240, 2684, 2675, 2676, 2663, 0, 2684, 2682, 2658, 2687]
cd = CubeDetector(laser_pose_6D = ((0,0,0), (0,0,0)), verbose=False)
cubes = cd.detect_cubes_xy(data)
self.assertNotEqual(cubes, [])
def test_detection( self ):
cd = CubeDetector(laser_pose_6D = ((0,0,0), (0,0,0)))
# two cubes, 2016-09-13-apu2\czuB\src_laser_160913_191514.log
data = [6766, 6771, 6779, 6780, 6782, 6788, 6799, 6794, 6790, 6804, 6809, 6815, 6823,
6837, 6852, 6855, 6853, 6856, 6851, 6849, 6846, 6833, 6835, 6838, 6831,
6835, 6847, 6834, 6830, 6824, 6824, 8647, 7306, 7285, 6871, 6832, 6838,
0, 7301, 7314, 7310, 7311, 7311, 7311, 7314, 7296, 7298, 7310, 7312,
7313, 7299, 7321, 7302, 7322, 7312, 7313, 7334, 7322, 7319, 7329, 7347,
7333, 7335, 7344, 7353, 7354, 7360, 7363, 7367, 7378, 7392, 7393, 7399,
7407, 7419, 7426, 7434, 7448, 7452, 7468, 7481, 7488, 7505, 7515, 7529,
7551, 7562, 7578, 7589, 7617, 7635, 7657, 7683, 7691, 7716, 7749, 7773,
7798, 7835, 7872, 7912, 7941, 7987, 8031, 8071, 8124, 8188, 8247, 8303,
8370, 8446, 8529, 8621, 8718, 8832, 8946, 9086, 9237, 9354, 9350, 9338,
9345, 9405, 9397, 9390, 9381, 9374, 9376, 9363, 9356, 9360, 9361, 9350,
9353, 9354, 9359, 9349, 9358, 9369, 9374, 9373, 9291, 9313, 9326, 9323,
9344, 9350, 9358, 7801, 7799, 7767, 7729, 7740, 7741, 7756, 7768, 7788,
7810, 9561, 9584, 9613, 9641, 9660, 9689, 9721, 9752, 9791, 7143, 7161,
7144, 7121, 7105, 7109, 7095, 7083, 7078, 7071, 7068, 7080, 7088, 7092,
7105, 7098, 7114, 7132, 7147, 7150, 7108, 0, 9117, 0, 9785, 9756, 9717,
9685, 8171, 8497, 8516, 8507, 8487, 8401, 8388, 8441, 8438, 8455, 8433,
8506, 8522, 8542, 8376, 0, 9384, 9377, 9367, 9358, 9350, 0, 9343, 9341,
9331, 9309, 9332, 9331, 9341, 7989, 9333, 9334, 9338, 9331, 0, 9348,
9354, 9360, 9268, 9332, 0, 9404, 9405, 9415, 9217, 9039, 9059, 9105,
9115, 8617, 8534, 8533, 8556, 8605, 8608, 8611, 8632, 0, 9367, 9362, 0,
0, 8803, 8795, 8841, 8902, 8889, 0, 9770, 9703, 9668, 9742, 9755, 9811,
9867, 9932]
self.assertEqual(len(data), 271)
cubes = cd.detect_cubes_xy(data)
self.assertEqual(cubes, [])
# 2016-09-13-apu2\czuB\src_laser_160913_191555.log
data = [165, 167, 175, 173, 177, 179, 180, 183, 188, 198, 208, 203, 208, 217, 221,
230, 241, 264, 269, 270, 260, 263, 264, 265, 253, 244, 242, 245, 246,
241, 242, 237, 234, 236, 235, 228, 230, 237, 230, 236, 231, 418, 2405,
2372, 2340, 2301, 2263, 2235, 2210, 2181, 2159, 2131, 2106, 2077, 2057,
2036, 2012, 1986, 1974, 1949, 1933, 1917, 1909, 1886, 1877, 1857, 1847,
1830, 1822, 1815, 1802, 1788, 1784, 1772, 1769, 1755, 1757, 1750, 1745,
1740, 1732, 1726, 1723, 1759, 543, 543, 543, 542, 541, 543, 544, 565, # cube on the left
539, 541, 546, 546, 545, 545, 548, 531, 1766, 1771, 1781, 1782, 1798,
1797, 1819, 1828, 1835, 1844, 1863, 1875, 1893, 1910, 1926, 1940, 1955,
1977, 1990, 2011, 2032, 2059, 2080, 2107, 2134, 2164, 2190, 2223, 2252,
2291, 2324, 2363, 2403, 2442, 2487, 2532, 2580, 2633, 2692, 1039, 1009, # cube in front
1004, 986, 971, 969, 967, 998, 1017, 1043, 2752, 2704, 2681, 2659,
2636, 2613, 2594, 2576, 2563, 2546, 2529, 2512, 2500, 2478, 2381, 2372,
2374, 2362, 2357, 2345, 2338, 2329, 2325, 2326, 2318, 2319, 2312, 2312,
2311, 2319, 2308, 2310, 2308, 2317, 2321, 2324, 2328, 2337, 2340, 2351,
2352, 2356, 2360, 2370, 2383, 2397, 2408, 0, 0, 2452, 2475, 2648, 1066,
2523, 2548, 1129, 1156, 1127, 1106, 1090, 1086, 1090, 1095, 1146, 1221,
1194, 1178, 1169, 1049, 1084, 1144, 1144, 4347, 2302, 2350, 2364, 576,
538, 535, 2214, 2184, 2283, 829, 810, 859, 842, 838, 827, 833, 820,
750, 727, 716, 707, 712, 734, 769, 757, 759, 752, 761, 768, 782, 793,
747, 751, 815, 823, 842, 856, 875, 916, 947, 1040, 0, 647, 0, 1687,
1687, 1691, 1689, 1691]
cubes = cd.detect_cubes_xy(data, limit=1)
self.assertEqual(cubes, [(0.40642698719133791, 0.39248197930933343)]) # corresponds to angle center (91, 565)
def find_cube(self, timeout):
print "find_cube-v1"
prev = None
cd = CubeDetector(self.robot.laser.pose)
startTime = self.robot.time
while self.robot.time < startTime + timeout:
self.robot.update()
if prev != self.robot.laserData:
prev = self.robot.laserData
cubes = cd.detect_cubes_xy(self.robot.laserData, limit=4)
if len(cubes) > 0:
for i, (cube_x, cube_y) in enumerate(cubes):
goal = combinedPose(self.robot.localisation.pose(), (cube_x, cube_y, 0))[:2]
viewlog.dumpBeacon(goal, color=(200, 200, 0) if i > 0 else (255, 255, 0))
cube_x, cube_y = cubes[0]
cube_y += 0.05 # hack for bended(?) laser
print "{:.2f}\t{:.2f}".format(cube_x, cube_y)
speed = 0.0
tolerance = 0.01
if cube_x > 0.5:
tolerance = 0.05
angle = math.atan2(cube_y, cube_x)
turnStep = math.radians(10)
if abs(angle) > math.radians(10):
turnStep = math.radians(50)
if cube_y > tolerance:
angularSpeed = turnStep
elif cube_y < -tolerance:
angularSpeed = -turnStep
else:
angularSpeed = 0
speed = 0.1
if cube_x > 0.5:
speed = 0.3 # move faster toward further goals
if cube_x < 0.30:
return True
self.robot.setSpeedPxPa(speed, angularSpeed)
else:
self.robot.setSpeedPxPa(0.0, 0.0)
print "TIMEOUT"
return False
