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()
