def test_badly_loaded_cube(self):
# logs\2016-10-07\czu1\src_laser_161007_115410.log
# diagonally loaded cube
data = [227, 229, 241, 245, 236, 246, 252, 261, 265, 272, 279, 280, 280, 282, 273,
261, 257, 258, 252, 250, 247, 237, 238, 242, 242, 306, 422, 419, 430,
423, 426, 430, 425, 431, 448, 446, 442, 444, 448, 2, 6469, 6506, 6583,
6293, 6200, 6316, 6354, 4475, 4412, 4280, 4164, 2, 0, 3872, 0, 5363,
153, 160, 166, 164, 157, 160, 155, 154, 149, 149, 146, 143, 146, 145,
141, 136, 136, 129, 130, 127, 119, 119, 113, 108, 103, 102, 103, 106,
108, 106, 105, 105, 98, 95, 92, 85, 71, 69, 66, 67, 71, 65, 71, 58, 57,
66, 65, 54, 53, 52, 58, 57, 58, 48, 58, 54, 54, 56, 55, 53, 51, 43, 64,
73, 94, 143, 156, 0, 0, 0, 0, 5735, 0, 7207, 0, 0, 0, 4756, 4680, 4709,
0, 5333, 5372, 5427, 5313, 4897, 4906, 5055, 5101, 4538, 4545, 5312,
4808, 0, 5474, 4168, 4204, 4192, 4164, 4128, 4074, 4052, 4014, 3990,
3961, 3852, 3965, 2492, 730, 671, 658, 653, 666, 655, 664, 666, 679,
669, 688, 689, 694, 688, 0, 6120, 6129, 6098, 6084, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 580, 592, 588,
585, 579, 574, 563, 572, 563, 564, 558, 556, 561, 547, 562, 660, 0, 0,
2, 4336, 4134, 4084, 4122, 4256, 4177, 4183, 4114, 5784, 4035, 0, 2, 2,
3656, 3634, 3595, 3513, 3472, 3403, 3374, 3371, 3365, 3367, 3406, 3412,
3417, 3442, 3437, 0, 2617, 2648, 0, 3529, 3577, 3576, 3418, 3419, 2173,
2187, 2233, 2586, 2547, 2510, 2470]
loaded, blocked_view_index = verify_loaded_cube(data)
self.assertTrue(loaded)
def handle_single_cube(self, pts, rev_pts, timeout=60):
if self.find_cube(timeout=20.0):
self.load_cube()
print verify_loaded_cube(self.robot.laserData)
start_time = self.robot.time
for cmd in self.driver.followPolyLineG(pts, withStops=True):
self.robot.setSpeedPxPa(*cmd)
self.robot.update()
if (not is_in_loading_zone(self.robot.localisation.pose(),
self.robot.last_valid_rfid)
and is_path_blocked(self.robot.laserData, self.robot.remissionData)):
print "ESCAPING FROM", self.robot.localisation.pose()
self.driver.stop()
break
if self.robot.time - start_time > timeout:
print "TIMEOUT", self.robot.time - start_time
self.driver.goStraight(-0.3, timeout=10)
self.driver.turn(angle=math.radians(90), timeout=10)
self.driver.turn(angle=math.radians(-90), timeout=10)
self.place_cube()
return # demo - game3 experiment
self.place_cube()
# handle offset in case of blocked path
print rev_pts
route = Route(pts=rev_pts, conv=DummyConvertor())
_, dist, signed_index = route.findNearestEx(self.robot.localisation.pose())
rev_pts = rev_pts[max(0, abs(signed_index) - 1):]
print "cut path", dist, signed_index, rev_pts
self.driver.turn(angle=math.radians(180), timeout=30)
for cmd in self.driver.followPolyLineG(rev_pts):
self.robot.setSpeedPxPa(*cmd)
self.robot.update()
