def test_followLineG( self ):
robot = MagicMock()
robot.localisation.pose.return_value = (0, 0.3, 0)
driver = Driver(robot, maxSpeed = 0.5)
offsetDistance = 0.03
gen = driver.followPolyLineG(pts = [(0, 0), (1.0, 0)],
offsetDistance=offsetDistance)
speed, angularSpeed = gen.next()
self.assertAlmostEqual(angularSpeed, -4 * math.radians(20))
self.assertAlmostEqual(speed, 0.444444444444)
# now for closer distance reduce the turning angle
robot.localisation.pose.return_value = (0, 1.5 * offsetDistance, 0)
speed, angularSpeed = gen.next()
self.assertAlmostEqual(angularSpeed, -4 * math.radians(10))
self.assertAlmostEqual(speed, 0.4722222222222222)
class SICKRobotDay2016:
def __init__(self, robot, code, verbose = False):
self.random = random.Random(0).uniform
self.robot = robot
self.verbose = verbose
self.code = code
self.robot.attachEmergencyStopButton()
# change robot wheel calibration
wd = 427.0 / 445.0 * 0.26/4.0 # with gear 1:4
delta = -0.00015
self.robot._WHEEL_DIAMETER = {SIDE_LEFT: wd+delta, SIDE_RIGHT: wd-delta}
self.robot.attachCamera(sleep=0.5)
self.robot.attachLaser( remission=True, pose=((0.24, -0.13, 0.08), (0,math.radians(180),0)) )
self.robot.attachRFU620()
self.driver = Driver( self.robot, maxSpeed = 0.5, maxAngularSpeed = math.radians(180) )
self.robot.localisation = SimpleOdometry()
self.robot.last_valid_rfid = None, None # tag, position
self.robot.addExtension(draw_rfu620_extension)
self.robot.addExtension(draw_cubes_extension)
self.robot.laser.stopOnExit = False # for faster boot-up
def load_cube(self):
print "load cube (time = {:.1f}s)".format(self.robot.time - self.start_time)
self.driver.goStraight(0.3, timeout=30)
gripperClose(self.robot)
def place_cube(self):
pos = combinedPose(self.robot.localisation.pose(), (0.2, 0, 0))[:2] # TODO check proper offset
print "place cube at ({:.2f}, {:.2f} (time = {:.1f}))".format(pos[0], pos[1], self.robot.time - self.start_time)
viewlog.dumpBeacon(pos, color=(255, 255, 255))
viewlog.dumpObstacles([[pos]])
gripperOpen(self.robot)
self.driver.goStraight(-0.4, timeout=30)
def game_over(self):
print 'Game over (battery status = {}V)'.format(self.robot.battery)
for k in xrange(10):
self.robot.setSpeedPxPa(0.0, 0.0)
self.robot.update()
raise EmergencyStopException() # TODO: Introduce GameOverException as in Eurobot
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
def ver0( self, verbose=False ):
# Go straight for 2 meters
print "ver0", self.robot.battery
prevRFID = None
self.load_cube()
for cmd in self.driver.goStraightG(2.0):
self.robot.setSpeedPxPa(*cmd)
if prevRFID != self.robot.rfu620Data:
prevRFID = self.robot.rfu620Data
posXY = combinedPose(self.robot.localisation.pose(), (-0.35, 0.14, 0))[:2]
for d in self.robot.rfu620Data[1]:
i = d[0] # i.e. 0x1000 0206 0000
x, y, zone = (i >> 24)&0xFF, (i >> 16)&0xFF, i&0xFF
print hex(i), (x, y)
if x == 1:
viewlog.dumpBeacon(posXY, color=(0, 0, 255))
elif x == 2:
viewlog.dumpBeacon(posXY, color=(255, 0, 255))
else:
viewlog.dumpBeacon(posXY, color=(255, 255, 255))
self.robot.update()
self.place_cube()
self.game_over()
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()
def ver1(self, verbose=False):
# Navigate on polyline
print "ver1", self.robot.battery, self.driver.maxAngularSpeed
self.driver.maxAngularSpeed = math.pi/2.0
paths = [
( [(1.5, 2.5), (1.5, 1.5), (6.5, 1.5)], [(6.5, 1.5), (3.5, 1.5)] ),
( [(3.5, 2.5), (5.5, 2.5)], [(5.5, 2.5), (3.5, 2.5)] ),
( [(3.5, 3.5), (6.5, 3.5)], [(6.5, 3.5), (3.5, 3.5)] ),
( [(3.5, 4.5), (5.5, 4.5)], [(5.5, 4.5), (3.5, 4.5)] ),
( [(3.5, 5.5), (6.5, 5.5)], [(6.5, 5.5), (3.5, 5.5)] ),
( [(3.5, 5.5), (5.5, 5.5)], [(5.5, 5.5), (3.5, 5.5)] ),
( [(3.5, 4.5), (4.5, 4.5)], [(4.5, 4.5), (3.5, 4.5)] ),
( [(3.5, 3.5), (5.5, 3.5)], [(5.5, 3.5), (3.5, 3.5)] ),
( [(3.5, 2.5), (4.5, 2.5)], [(4.5, 2.5), (3.5, 2.5)] ),
( [(3.5, 1.5), (5.5, 1.5)], [(5.5, 1.5), (3.5, 1.5)] ),
( [(3.5, 1.5), (4.5, 1.5)], [(4.5, 1.5), (3.5, 1.5)] ),
( [(3.5, 3.5), (4.5, 3.5)], [(4.5, 3.5), (3.5, 3.5)] ),
( [(3.5, 5.5), (4.5, 5.5)], [(4.5, 5.5), (3.5, 5.5)] ),
]
self.robot.localisation.setPose( (1.5, 2.2, 0.0) )
for path, rev_path in paths:
self.handle_single_cube(path, rev_path)
self.game_over()
def test_line( self, verbose=False ):
print "test_line", self.robot.battery
self.driver.goStraight(10.0, timeout=60)
self.game_over()
def test_square( self, verbose=False ):
print "test_square", self.robot.battery
while True:
self.driver.turn(angle=math.radians(90), timeout=30)
self.driver.goStraight(1.0, timeout=30)
def test_pick_cube(self, verbose=False):
print "test_pick_cube", self.robot.battery
if self.find_cube(timeout=20.0):
self.load_cube()
self.driver.goStraight(0.5, timeout=10)
self.place_cube()
self.game_over()
def run( self ):
self.start_time = float('nan') # just to be defined in case of EmergencyStopException
try:
if getattr( self.robot.laser, 'startLaser', None ):
# trigger rotation of the laser, low level function, ignore for log files
print "Powering laser ON"
self.robot.laser.startLaser()
gripperOpen(self.robot)
self.robot.waitForStart()
self.start_time = self.robot.time
self.robot.laser.start() # laser also after start -- it should be already running
self.robot.camera.start()
self.robot.rfu620.start()
self.robot.localisation = SimpleOdometry()
while True:
# self.ver0(verbose = self.verbose)
self.ver1(verbose = self.verbose)
# self.test_line(verbose = self.verbose)
except EmergencyStopException, e:
print "EmergencyStopException at {} sec".format(self.robot.time - self.start_time)
gripperDisableServos(self.robot.can)
self.robot.laser.requestStop()
self.robot.rfu620.requestStop()
self.robot.camera.requestStop()