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 updateExtension( self, robot, id, data ):
global g_weedReportEnabled
if id == 0x80:
self.counter += 1
if len(self.lastCamera)> 0:
# print self.counter, self.lastCamera
cmd = self.lastCamera[0]
if self.counter >= cmd[0]:
self.lastCamera = self.lastCamera[1:]
if cmd[1] or cmd[2]:
robot.beep = 1
else:
robot.beep = 0
sprayer( robot, cmd[1], cmd[2] )
if id == 'camera':
if self.verbose and len(data) > 1:
print data[1]
cc = [int(x) for x in data[0].split()]
leftPip = (cc[0] > BALL_SIZE_LIMIT_MIN and cc[0] < BALL_SIZE_LIMIT_MAX)
rightPip = (cc[1] > BALL_SIZE_LIMIT_MIN and cc[1] < BALL_SIZE_LIMIT_MAX)
if leftPip or rightPip:
print "WEED:", leftPip, rightPip, g_weedReportEnabled
if leftPip and g_weedReportEnabled:
xy = combinedPose(robot.localisation.pose(), (0,0.35,0))[:2]
# reportBall( robot.gpsData )
viewlog.dumpBeacon( xy, color=(255,255,0) )
if rightPip and g_weedReportEnabled:
xy = combinedPose(robot.localisation.pose(), (0,-0.35,0))[:2]
# reportBall( robot.gpsData )
viewlog.dumpBeacon( xy, color=(255,255,0) )
if g_weedReportEnabled:
self.lastCamera.append( (self.counter + 0, leftPip, rightPip ) )
else:
self.lastCamera.append( (self.counter + 0+4, False, False) )
def reset( self, pose = (0,0,0), offsetDeg=0 ):
global g_weedReportEnabled
g_weedReportEnabled = True
print "RESET ROW (%0.2f, %0.2f, %0.1f), offset=" % (pose[0], pose[1], math.degrees(pose[2])), offsetDeg
viewlog.dumpBeacon( pose[:2], color=(128,128,128) )
self.preference = None
self.center = None
if self.rowHeading:
self.directionAngle = normalizeAnglePIPI(self.rowHeading-pose[2])
if abs(self.directionAngle) > math.radians(90):
self.directionAngle = normalizeAnglePIPI(self.rowHeading-pose[2]+math.radians(180))
if self.verbose:
print "RESET_DIFF %.1f" % math.degrees(self.directionAngle)
else:
self.directionAngle = 0.0 # if you do not know, go ahead
goal = combinedPose( (pose[0], pose[1], pose[2]+self.directionAngle), (self.radius, 0, 0) )
self.line = Line( pose, goal )
# self.line = Line( (0.0, 0.0), (1.0, 0.0) )
self.newData = False
self.endOfRow = False
self.cornLeft = 10.0 # far
self.cornRight = 10.0
self.collisionAhead = 10.0,10.0,False,False # far (wide, narrow, override, danger)
self.lastLeft, self.lastRight = None, None
self.prevLR = None
self.poseHistory = []
def reset(self, pose=(0, 0, 0), offsetDeg=0):
print "RESET ROW (%0.2f, %0.2f, %0.1f), offset=" % (
pose[0], pose[1], math.degrees(pose[2])), offsetDeg
viewlog.dumpBeacon(pose[:2], color=(128, 128, 128))
self.preference = None
self.center = None
if self.rowHeading:
self.directionAngle = normalizeAnglePIPI(self.rowHeading - pose[2])
if abs(self.directionAngle) > math.radians(90):
self.directionAngle = normalizeAnglePIPI(self.rowHeading -
pose[2] +
math.radians(180))
if self.verbose:
print "RESET_DIFF %.1f" % math.degrees(self.directionAngle)
else:
self.directionAngle = 0.0 # if you do not know, go ahead
goal = combinedPose((pose[0], pose[1], pose[2] + self.directionAngle),
(self.radius, 0, 0))
self.line = Line(pose, goal)
# self.line = Line( (0.0, 0.0), (1.0, 0.0) )
self.newData = False
self.endOfRow = False
self.cornLeft = 10.0 # far
self.cornRight = 10.0
self.collisionAhead = 10.0, 10.0, False # far (wide, narrow, override)
self.lastLeft, self.lastRight = None, None
self.prevLR = None
self.poseHistory = []
def draw_cubes_extension(robot, id, data):
if id == 'laser':
# cd = CubeDetector(robot.laser.pose)
# cubes = cd.detect_cubes_xy(data, limit=4)
# Jakub's alternative
cubes = cubesFromScan(data)
for cube_x, cube_y in cubes:
goal = combinedPose(robot.localisation.pose(), (cube_x, cube_y, 0))[:2]
viewlog.dumpBeacon(goal, color=(128, 255, 128))
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 updateExtension(self, robot, id, data):
global g_weedReportEnabled
if id == 0x80:
self.counter += 1
if len(self.lastCamera) > 0:
# print self.counter, self.lastCamera
cmd = self.lastCamera[0]
if self.counter >= cmd[0]:
self.lastCamera = self.lastCamera[1:]
if cmd[1] or cmd[2]:
robot.beep = 1
else:
robot.beep = 0
sprayer(robot, cmd[1], cmd[2])
if id == 'camera':
if self.verbose and len(data) > 1:
print data[1]
cc = [int(x) for x in data[0].split()]
leftPip = (cc[0] > BALL_SIZE_LIMIT_MIN
and cc[0] < BALL_SIZE_LIMIT_MAX)
rightPip = (cc[1] > BALL_SIZE_LIMIT_MIN
and cc[1] < BALL_SIZE_LIMIT_MAX)
if leftPip or rightPip:
print "WEED:", leftPip, rightPip, g_weedReportEnabled
if leftPip and g_weedReportEnabled:
xy = combinedPose(robot.localisation.pose(),
(0, 0.35, 0))[:2]
# reportBall( robot.gpsData )
viewlog.dumpBeacon(xy, color=(255, 255, 0))
if rightPip and g_weedReportEnabled:
xy = combinedPose(robot.localisation.pose(),
(0, -0.35, 0))[:2]
# reportBall( robot.gpsData )
viewlog.dumpBeacon(xy, color=(255, 255, 0))
if g_weedReportEnabled:
self.lastCamera.append(
(self.counter + 0, leftPip, rightPip))
else:
self.lastCamera.append((self.counter + 0 + 4, False, False))
def draw_rfu620_extension(robot, id, data):
if id=='rfu620':
posXY = combinedPose(robot.localisation.pose(), RFU620_POSE)[:2]
for index, d in enumerate(data[1], start=1):
robot.last_valid_rfid = d, posXY
i, rssi = d[:2] # i.e. 0x1000 0206 0000
x, y, zone = (i >> 24)&0xFF, (i >> 16)&0xFF, i&0xFF
print hex(i), (x, y), rssi, '({}/{})'.format(index, len(data[1]))
if (x + y) % 2 == 0:
if rssi > -60:
viewlog.dumpBeacon(posXY, color=(0, 0, 255))
else:
viewlog.dumpBeacon(posXY, color=(0, 0, 128))
else:
if rssi > -60:
viewlog.dumpBeacon(posXY, color=(255, 0, 255))
else:
viewlog.dumpBeacon(posXY, color=(128, 0, 128))
def approachFeeder(self, timeout=60, digitHelper=None, verifyTarget=True):
"robot should be within 1m of the feeder"
print "Approaching Feeder"
verified = False
desiredDist = 0.4 #0.2
countOK = 0
startTime = self.robot.time
angularSpeed = 0
prevPose = None
prevName = None
prevLaser = None
target = None
frontDist, minDistL, minDistR = None, None, None
while startTime + timeout > self.robot.time:
if self.robot.cameraData is not None and len(
self.robot.cameraData
) > 0 and self.robot.cameraData[0] is not None:
if prevName != self.robot.cameraData[1]:
# print prevName, self.robot.localisation.pose()
prevName = self.robot.cameraData[1]
if prevPose is None:
prevPose = self.robot.localisation.pose()
prevLaser = self.robot.laserData[:]
arr = eval(self.robot.cameraData[0])
angularSpeed = None
for a in arr:
for digit, (x, y, dx, dy) in a:
if digit == 'X':
verified = True
angularSpeed = (320 - (x + dx / 2)) / 100.0
# print "angularSpeed", math.degrees(angularSpeed), self.robot.cameraData[1], (x,y,dx,dy)
centerX = 320
angle = (centerX -
(x + dx / 2)) * 0.002454369260617026
dist = prevLaser[int(angle / 2.) + 271] / 1000.
print "Target at", dist, self.robot.cameraData[
1]
t = combinedPose((prevPose[0], prevPose[1],
prevPose[2] + angle),
(dist, 0, 0))
target = (t[0], t[1])
viewlog.dumpBeacon(target, color=(255, 128, 0))
break
if target is None and digitHelper is not None:
# if you do not have goal try to re-search old number
for a in arr:
for digit, (x, y, dx, dy) in a:
if digit == 'X':
angularSpeed = (320 - (x + dx / 2)) / 100.0
centerX = 320
angle = (
centerX -
(x + dx / 2)) * 0.002454369260617026
dist = prevLaser[int(angle / 2.) +
271] / 1000.
t = combinedPose((prevPose[0], prevPose[1],
prevPose[2] + angle),
(dist, 0, 0))
target = (t[0], t[1])
viewlog.dumpBeacon(target,
color=(255, 0, 255))
break
prevPose = self.robot.localisation.pose()
prevLaser = self.robot.laserData[:]
if angularSpeed is None:
angularSpeed = 0
if target is None or distance(
target, self.robot.localisation.pose()) < 0.3:
angularSpeed = 0.0
else:
pose = self.robot.localisation.pose()
angularSpeed = normalizeAnglePIPI(
angleTo(pose, target) - pose[2])
# print "target:", target, math.degrees(angularSpeed)
if self.robot.laserData == None or len(
self.robot.laserData) != 541:
self.robot.setSpeedPxPa(0, 0)
else:
minDistR = min(
[10000] +
[x for x in self.robot.laserData[180:541 / 2]
if x > 0]) / 1000.
minDistL = min(
[10000] +
[x for x in self.robot.laserData[541 / 2:-180]
if x > 0]) / 1000.
minDist = min(minDistL, minDistR)
frontDist = min(
[10000] +
[x
for x in self.robot.laserData[265:-265] if x > 0]) / 1000.
self.robot.setSpeedPxPa(
min(self.driver.maxSpeed, minDist - desiredDist),
angularSpeed)
# print min(self.driver.maxSpeed, minDist - desiredDist)
# self.robot.setSpeedPxPa( 0, angularSpeed )
if abs(minDist - desiredDist) < 0.01:
countOK += 1
else:
countOK = 0
if countOK >= 10:
break
self.robot.update()
self.robot.setSpeedPxPa(0, 0)
self.robot.update()
print "done."
if verifyTarget and not verified:
print "TARGET not verified!"
return False
# compute proper rotation and backup distance
toTurn, toBackup = computeLoadManeuver(minDistL, frontDist, minDistR)
print "Suggestion: ", math.degrees(toTurn), toBackup
self.driver.turn(toTurn,
angularSpeed=math.radians(20),
timeout=30,
verbose=True)
self.driver.goStraight(toBackup, timeout=30)
return True
def competeRobotemRovne( drone, desiredHeight = 1.5 ):
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
desiredSpeed = MAX_ALLOWED_SPEED
refLine = DEFAULT_REF_LINE
try:
drone.wait(1.0)
drone.setVideoChannel( front=True )
downloadOldVideo( drone, timeout=20.0 )
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
# TODO sonar/vision altitude
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), None) )
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
startTime = drone.time
sx,sy,sz,sa = 0,0,0,0
lastUpdate = None
while drone.time < startTime + 600.0:
altitude = desiredHeight
if drone.altitudeData != None:
altVision = drone.altitudeData[0]/1000.0
altSonar = drone.altitudeData[3]/1000.0
altitude = (altSonar+altVision)/2.0
# TODO selection based on history? panic when min/max too far??
if abs(altSonar-altVision) > 0.5:
print altSonar, altVision
altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)
sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
if altitude > 2.5:
# wind and "out of control"
sz = max( -0.5, min( 0.5, desiredHeight - altitude ))
sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))
if drone.lastImageResult:
lastUpdate = drone.time
assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpVideoFrame( frameNumber, timestamp )
# keep history small
videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max( videoDelay, maxVideoDelay )
toDel = 0
for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),
# print "angle", math.degrees(drone.angleFB-oldAngles[0]), math.degrees(drone.angleLR-oldAngles[1])
if rects != None and len(rects) > 0:
# note, that "rects" could be None, when video processing is OFF (g_processingEnabled==False)
if oldAltitude == None:
oldAltitude = altitude
debugImg = None
if drone.replayLog != None:
debugFilename = "tmp_%04d.jpg" % (frameNumber/FRAMES_PER_INDEX)
debugImg = cv2.imread( debugFilename )
line = chooseBestWidth( rects, coord=oldPose[:2], height=oldAltitude,
heading=oldPose[2], angleFB=oldAngles[0], angleLR=oldAngles[1], debugImg=debugImg )
if line:
rejected = False
if refLine != None:
print "%.1fdeg %.2fm" % (math.degrees(normalizeAnglePIPI(refLine.angle - line.angle)), refLine.signedDistance( line.start ))
if abs(normalizeAnglePIPI(refLine.angle - line.angle)) < MAX_REF_LINE_ANGLE and \
abs(refLine.signedDistance( line.start )) < MAX_REF_LINE_DIST:
refLine = line
else:
rejected = True
else:
refLine = line
viewlog.dumpBeacon( line.start, index=3 )
viewlog.dumpBeacon( line.end, index=3 )
if drone.replayLog != None:
if rejected:
# redraw image with yellow color
chooseBestWidth( rects, coord=oldPose[:2], height=oldAltitude,
heading=oldPose[2], angleFB=oldAngles[0], angleLR=oldAngles[1], debugImg=debugImg, color=(0,255,255) )
cv2.imwrite( debugFilename, debugImg )
else:
print rects
desiredSpeed = MAX_ALLOWED_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
if refLine:
errY = refLine.signedDistance( drone.coord )
errA = normalizeAnglePIPI( drone.heading - refLine.angle )
# get the height first
if drone.coord[2] < desiredHeight - 0.1 and drone.time-startTime < 5.0:
sx = 0.0
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA( sx, sy, sz, sa )
maxControlGap = max( drone.time - prevTime, maxControlGap )
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), altitude) )
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel( front=True )
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def approachFeeder( self, timeout=60, digitHelper=None, verifyTarget=True ):
"robot should be within 1m of the feeder"
print "Approaching Feeder"
verified = False
desiredDist = 0.4 #0.2
countOK = 0
startTime = self.robot.time
angularSpeed = 0
prevPose = None
prevName = None
prevLaser = None
target = None
frontDist, minDistL, minDistR = None, None, None
while startTime + timeout > self.robot.time:
if self.robot.cameraData is not None and len(self.robot.cameraData)> 0 and self.robot.cameraData[0] is not None:
if prevName != self.robot.cameraData[1]:
# print prevName, self.robot.localisation.pose()
prevName = self.robot.cameraData[1]
if prevPose is None:
prevPose = self.robot.localisation.pose()
prevLaser = self.robot.laserData[:]
arr = eval(self.robot.cameraData[0])
angularSpeed = None
for a in arr:
for digit, (x,y,dx,dy) in a:
if digit == 'X':
verified = True
angularSpeed = (320-(x+dx/2))/100.0
# print "angularSpeed", math.degrees(angularSpeed), self.robot.cameraData[1], (x,y,dx,dy)
centerX = 320
angle = (centerX-(x+dx/2))*0.002454369260617026
dist = prevLaser[int(angle/2.)+271]/1000.
print "Target at", dist, self.robot.cameraData[1]
t = combinedPose( (prevPose[0], prevPose[1], prevPose[2]+angle), (dist,0,0) )
target = (t[0],t[1])
viewlog.dumpBeacon( target, color=(255,128,0) )
break
if target is None and digitHelper is not None:
# if you do not have goal try to re-search old number
for a in arr:
for digit, (x,y,dx,dy) in a:
if digit == 'X':
angularSpeed = (320-(x+dx/2))/100.0
centerX = 320
angle = (centerX-(x+dx/2))*0.002454369260617026
dist = prevLaser[int(angle/2.)+271]/1000.
t = combinedPose( (prevPose[0], prevPose[1], prevPose[2]+angle), (dist,0,0) )
target = (t[0],t[1])
viewlog.dumpBeacon( target, color=(255,0,255) )
break
prevPose = self.robot.localisation.pose()
prevLaser = self.robot.laserData[:]
if angularSpeed is None:
angularSpeed = 0
if target is None or distance( target, self.robot.localisation.pose() ) < 0.3:
angularSpeed = 0.0
else:
pose = self.robot.localisation.pose()
angularSpeed = normalizeAnglePIPI( angleTo( pose, target) - pose[2] )
# print "target:", target, math.degrees(angularSpeed)
if self.robot.laserData == None or len(self.robot.laserData) != 541:
self.robot.setSpeedPxPa( 0, 0 )
else:
minDistR = min([10000]+[x for x in self.robot.laserData[180:541/2] if x > 0])/1000.
minDistL = min([10000]+[x for x in self.robot.laserData[541/2:-180] if x > 0])/1000.
minDist = min(minDistL, minDistR)
frontDist = min([10000]+[x for x in self.robot.laserData[265:-265] if x > 0])/1000.
self.robot.setSpeedPxPa( min(self.driver.maxSpeed, minDist - desiredDist), angularSpeed )
# print min(self.driver.maxSpeed, minDist - desiredDist)
# self.robot.setSpeedPxPa( 0, angularSpeed )
if abs(minDist - desiredDist) < 0.01:
countOK += 1
else:
countOK = 0
if countOK >= 10:
break
self.robot.update()
self.robot.setSpeedPxPa( 0, 0 )
self.robot.update()
print "done."
if verifyTarget and not verified:
print "TARGET not verified!"
return False
# compute proper rotation and backup distance
toTurn, toBackup = computeLoadManeuver( minDistL, frontDist, minDistR )
print "Suggestion: ", math.degrees(toTurn), toBackup
self.driver.turn( toTurn, angularSpeed = math.radians(20), timeout=30, verbose=True )
self.driver.goStraight( toBackup, timeout=30 )
return True
def crossRows2( self, row, num, rowsOnLeft ):
IGNORE_NEIGHBORS = 2
ROWS_OFFSET = 0.7 #0.5
# if row.lastLeft:
# viewlog.dumpBeacon(row.lastLeft[:2], color=(0,128,0))
# if row.lastRight:
# viewlog.dumpBeacon(row.lastRight[:2], color=(0,128,50))
start = self.robot.localisation.pose()[:2]
viewlog.dumpBeacon( start, color=(255,128,0))
goal = combinedPose( self.robot.localisation.pose(), (1.0, 0, 0) )
line = Line( self.robot.localisation.pose(), goal )
lastA, lastB = None, None
ends = []
while True:
for cmd in self.driver.followLineG( line ):
self.robot.setSpeedPxPa( *cmd )
self.robot.update()
if row.newData:
row.newData = False
break
else:
print "END OF LINE REACHED!"
if self.robot.preprocessedLaser != None:
tmp = self.robot.preprocessedLaser[:]
tlen = len(tmp)
if rowsOnLeft:
tmp = tmp[:tlen/2] + [3.0]*(tlen-tlen/2)
else:
tmp = [3.0]*(tlen-tlen/2) + tmp[tlen/2:]
sarr = sorted([(x,i) for (i,x) in enumerate(tmp)])[:5]
ax,ai = sarr[0]
for bx,bi in sarr[1:]:
if abs(ai-bi) > IGNORE_NEIGHBORS:
break
else:
print "NO 2nd suitable minimum"
print (ax,ai), (bx,bi)
if rowsOnLeft:
offset = -ROWS_OFFSET
if ai > bi:
(ax,ai), (bx,bi) = (bx,bi), (ax,ai)
else:
offset = ROWS_OFFSET
if ai < bi: # rows on right
(ax,ai), (bx,bi) = (bx,bi), (ax,ai)
p = self.robot.localisation.pose()
A = combinedPose( (p[0], p[1], p[2]+math.radians(135-5*ai) ), (ax,0,0) )
B = combinedPose( (p[0], p[1], p[2]+math.radians(135-5*bi) ), (bx,0,0) )
if lastA == None:
ends.extend( [A,B] )
lastA, lastB = A, B
if self.verbose:
print "DIST", distance(lastA, A), distance(lastB, B), distance(lastB,A)
if distance(lastB,A) < 0.2:
dist = distance(start, self.robot.localisation.pose())
print "NEXT ROW", dist
if dist > 0.4:
ends.append( B ) # new one
lastA, lastB = A, B
line = Line(A,B) # going through the ends of rows
A2 = combinedPose( (A[0], A[1], line.angle), (0, offset, 0) )
B2 = combinedPose( (B[0], B[1], line.angle), (2.0, offset, 0) )
line = Line(A2, B2)
viewlog.dumpBeacon( A[:2], color=(200,0,0) )
viewlog.dumpBeacon( B[:2], color=(200,128,0) )
viewlog.dumpBeacon( A2[:2], color=(255,0,0) )
viewlog.dumpBeacon( B2[:2], color=(255,128,0) )
if len(ends) > num + 1:
break
else:
print "BACKUP solution!!!"
goal = combinedPose( self.robot.localisation.pose(), (1.0, 0, 0) )
line = Line( self.robot.localisation.pose(), goal )
def updateExtension( self, robot, id, data ):
global g_weedReportEnabled
if id == 'remission' and len(data) > 0:
pass # ignored for FRE 2014
if id == 'laser' and len(data) > 0:
self.poseHistory.append( robot.localisation.pose()[:2] )
if len(self.poseHistory) > 20:
self.poseHistory = self.poseHistory[-20:]
step = 10
data2 = [x == 0 and 10000 or x for x in data]
arr = [min(i)/1000.0 for i in [itertools.islice(data2, start, start+step) for start in range(0,len(data2),step)]]
arr.reverse()
robot.preprocessedLaser = arr
limit = self.radius
danger = False
prevCenter = self.center
if self.center != None:
# if center > 0 and center < len(arr)-1 and arr[center] < limit and arr[center-1] < limit and arr[center+1] < limit:
if self.center > 0 and self.center < len(arr)-1 and arr[self.center] < limit:
print "!!!DANGER!!! PATH BLOCKED!!!", self.center
danger = True
if self.center==None or danger:
# offset is not set - find nearest
if self.center == None:
self.center = len(arr)/2
if arr[self.center] < limit:
i = 0
while i < self.center:
if arr[self.center - i] >= limit:
break
i += 1
left = i
i = 0
while i + self.center < len(arr):
if arr[self.center + i] >= limit:
break
i += 1
right = i
print "HACK", left, right
if left < right:
self.center += -left
else:
self.center += right
i = 0
if self.center < 0 or self.center >= len(arr):
self.center = prevCenter
if self.center == None:
self.center = len(arr)/2
while i <= self.center:
if arr[self.center - i] < limit:
break
i += 1
left = i # arr is already reversed
i = 0
while i + self.center < len(arr):
if arr[self.center + i] < limit:
break
i += 1
right = i
p = robot.localisation.pose()
if self.center-left >= 0:
self.lastLeft = combinedPose( (p[0], p[1], p[2]+math.radians(135-5*(self.center-left)) ), (arr[self.center-left],0,0) )
# viewlog.dumpBeacon(self.lastLeft[:2], color=(0,128,0))
if self.center+right < len(arr):
self.lastRight = combinedPose( (p[0], p[1], p[2]+math.radians(135-5*(self.center+right)) ), (arr[self.center+right],0,0) )
# viewlog.dumpBeacon(self.lastRight[:2], color=(0,128,50))
if self.verbose:
if danger and left+right < MIN_GAP_SIZE:
print "MIN GAP SIZE danger:", left+right
s = ''
for i in arr:
s += (i < 0.5 and 'X' or (i<1.0 and 'x' or (i<1.5 and '.' or ' ')))
s = "".join( list(s)[:self.center] + ['C'] + list(s)[self.center:] )
print "'" + s + "'"
# print "LRLR\t%d\t%d\t%d" % (left, right, left+right)
# if self.prevLR :
# print "LRDIFF", left-self.prevLR[0], right-self.prevLR[1]
self.line = None # can be defined inside
if left+right <= MAX_GAP_SIZE: # TODO limit based on minSize, radius and sample angle
self.center += (right-left)/2
offset = self.center-len(arr)/2
self.directionAngle = math.radians( -offset*step/2.0 )
self.collisionAhead = min(arr[54/3:2*54/3]), min(arr[4*54/9:5*54/9]), (left+right < MIN_GAP_SIZE), danger
if False: #self.verbose:
if self.collisionAhead[0] < 0.25:
print "!!! COLISSION AHEAD !!!", self.collisionAhead
else:
print "free space", self.collisionAhead
elif left < 3 or right < 3:
# wide row & collision ahead
if self.verbose:
print "NEAR", left, right, left+right
offset = self.center-len(arr)/2
if left < right:
offset += 3
else:
offset -= 3
self.directionAngle = math.radians( -offset*step/2.0 )
else:
if self.verbose:
print "OFF", left, right #, left+right, robot.compass
if False: # hacked, ignoring compass self.rowHeading:
self.directionAngle = normalizeAnglePIPI(self.rowHeading-robot.localisation.pose()[2])
if abs(self.directionAngle) > math.radians(90):
self.directionAngle = normalizeAnglePIPI(self.rowHeading-robot.localisation.pose()[2]+math.radians(180))
if self.verbose:
print "DIFF %.1f" % math.degrees(self.directionAngle)
else:
if self.verbose:
print "PATH GAP"
A, B = self.poseHistory[0][:2], self.poseHistory[-1][:2]
viewlog.dumpBeacon( A, color=(0,0,180) )
viewlog.dumpBeacon( B, color=(0,30,255) )
self.line = Line( B, (2*B[0]-A[0], 2*B[1]-A[1]) ) # B+(B-A)
if self.line and self.line.length < 0.5:
self.line = None
else:
self.center = len(arr)/2
# reset center
"""if self.prevLR:
if abs(left-self.prevLR[0]) > 4 and abs(right-self.prevLR[1]) < 3:
left = self.prevLR[0]
self.center += (right-left)/2
offset = self.center-len(arr)/2
self.directionAngle = math.radians( -offset*step/2.0 )
elif abs(right-self.prevLR[1]) > 4 and abs(left-self.prevLR[0]) < 3:
right = self.prevLR[1]
self.center += (right-left)/2
offset = self.center-len(arr)/2
self.directionAngle = math.radians( -offset*step/2.0 )
else:
self.directionAngle = 0.0 # if you do not know, go ahead
else:
self.directionAngle = 0.0 # if you do not know, go ahead"""
self.directionAngle = 0.0 # default
if left >= 17 and right >= 17 or left+right >= 40:
self.endOfRow = True
g_weedReportEnabled = False
if self.verbose and robot.insideField:
print "laser: END OF ROW"
self.prevLR = left, right
pose = robot.localisation.pose()
goal = combinedPose( (pose[0], pose[1], pose[2]+self.directionAngle), (self.radius, 0, 0) )
if self.line == None:
self.line = Line( pose, goal )
self.newData = True
self.cornLeft = min(arr[5:9])
self.cornRight = min(arr[40:44])
def competeRobotemRovne(drone, desiredHeight=1.5):
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
desiredSpeed = MAX_ALLOWED_SPEED
refLine = DEFAULT_REF_LINE
try:
drone.wait(1.0)
drone.setVideoChannel(front=True)
downloadOldVideo(drone, timeout=20.0)
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
# TODO sonar/vision altitude
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR), None))
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
startTime = drone.time
sx, sy, sz, sa = 0, 0, 0, 0
lastUpdate = None
while drone.time < startTime + 600.0:
altitude = desiredHeight
if drone.altitudeData != None:
altVision = drone.altitudeData[0] / 1000.0
altSonar = drone.altitudeData[3] / 1000.0
altitude = (altSonar + altVision) / 2.0
# TODO selection based on history? panic when min/max too far??
if abs(altSonar - altVision) > 0.5:
print altSonar, altVision
altitude = max(
altSonar,
altVision) # sonar is 0.0 sometimes (no ECHO)
sz = max(-0.2, min(0.2, desiredHeight - altitude))
if altitude > 2.5:
# wind and "out of control"
sz = max(-0.5, min(0.5, desiredHeight - altitude))
sx = max(0, min(drone.speed, desiredSpeed - drone.vx))
if drone.lastImageResult:
lastUpdate = drone.time
assert len(drone.lastImageResult) == 2 and len(
drone.lastImageResult[0]) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpVideoFrame(frameNumber, timestamp)
# keep history small
videoTime = correctTimePeriod(timestamp / 1000.,
ref=drone.time)
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max(videoDelay, maxVideoDelay)
toDel = 0
for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight * oldAngles[0],
desiredHeight * oldAngles[1],
0) # TODO real height?
print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
oldAngles[0]), math.degrees(oldAngles[1])),
# print "angle", math.degrees(drone.angleFB-oldAngles[0]), math.degrees(drone.angleLR-oldAngles[1])
if rects != None and len(rects) > 0:
# note, that "rects" could be None, when video processing is OFF (g_processingEnabled==False)
if oldAltitude == None:
oldAltitude = altitude
debugImg = None
if drone.replayLog != None:
debugFilename = "tmp_%04d.jpg" % (frameNumber /
FRAMES_PER_INDEX)
debugImg = cv2.imread(debugFilename)
line = chooseBestWidth(rects,
coord=oldPose[:2],
height=oldAltitude,
heading=oldPose[2],
angleFB=oldAngles[0],
angleLR=oldAngles[1],
debugImg=debugImg)
if line:
rejected = False
if refLine != None:
print "%.1fdeg %.2fm" % (math.degrees(
normalizeAnglePIPI(refLine.angle - line.angle)
), refLine.signedDistance(line.start))
if abs(normalizeAnglePIPI(refLine.angle - line.angle)) < MAX_REF_LINE_ANGLE and \
abs(refLine.signedDistance( line.start )) < MAX_REF_LINE_DIST:
refLine = line
else:
rejected = True
else:
refLine = line
viewlog.dumpBeacon(line.start, index=3)
viewlog.dumpBeacon(line.end, index=3)
if drone.replayLog != None:
if rejected:
# redraw image with yellow color
chooseBestWidth(rects,
coord=oldPose[:2],
height=oldAltitude,
heading=oldPose[2],
angleFB=oldAngles[0],
angleLR=oldAngles[1],
debugImg=debugImg,
color=(0, 255, 255))
cv2.imwrite(debugFilename, debugImg)
else:
print rects
desiredSpeed = MAX_ALLOWED_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
if refLine:
errY = refLine.signedDistance(drone.coord)
errA = normalizeAnglePIPI(drone.heading - refLine.angle)
# get the height first
if drone.coord[
2] < desiredHeight - 0.1 and drone.time - startTime < 5.0:
sx = 0.0
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (
desiredSpeed / 0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA(sx, sy, sz, sa)
maxControlGap = max(drone.time - prevTime, maxControlGap)
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR), altitude))
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel(front=True)
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def competeAirRace(drone, desiredHeight=1.7):
loops = []
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
loc = StripsLocalisation()
remainingFastStrips = 0
desiredSpeed = TRANSITION_SPEED
updateFailedCount = 0
try:
drone.wait(1.0)
drone.setVideoChannel(front=False)
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR)))
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
pathType = PATH_TURN_LEFT
virtualRefCircle = None
startTime = drone.time
sx, sy, sz, sa = 0, 0, 0, 0
lastUpdate = None
while drone.time < startTime + 600.0:
sz = max(-0.2, min(0.2, desiredHeight - drone.coord[2]))
sx = max(0, min(drone.speed, desiredSpeed - drone.vx))
if drone.lastImageResult:
lastUpdate = drone.time
assert len(drone.lastImageResult) == 2 and len(
drone.lastImageResult[0]) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpCamera("tmp_%04d.jpg" % (frameNumber / 15, ), 0)
# keep history small
videoTime = correctTimePeriod(timestamp / 1000.,
ref=drone.time)
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max(videoDelay, maxVideoDelay)
toDel = 0
for oldTime, oldPose, oldAngles in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight * oldAngles[0],
desiredHeight * oldAngles[1],
0) # TODO real height?
print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
oldAngles[0]), math.degrees(oldAngles[1])),
loc.updateFrame(
Pose(*oldPose).add(tiltCompensation),
allStripPoses(rects,
highResolution=drone.videoHighResolution))
if loc.pathType != pathType:
print "TRANS", pathType, "->", loc.pathType
if pathType == PATH_TURN_LEFT and loc.pathType == PATH_STRAIGHT:
if len(loops) > 0:
print "Loop %d, time %d" % (len(loops),
drone.time - loops[-1])
print "-----------------------------------------------"
loops.append(drone.time)
if drone.magneto[:3] == magnetoOnStart:
print "!!!!!!!! COMPASS FAILURE !!!!!!!!"
pathType = loc.pathType
print "----"
remainingFastStrips = NUM_FAST_STRIPS
if loc.crossing:
print "X", True, remainingFastStrips
else:
print pathType, loc.pathUpdated, remainingFastStrips
if not loc.pathUpdated:
updateFailedCount += 1
if updateFailedCount > 1:
print "UPDATE FAILED", updateFailedCount
else:
updateFailedCount = 0
if remainingFastStrips > 0:
remainingFastStrips -= 1
desiredSpeed = MAX_ALLOWED_SPEED
if not loc.pathUpdated and not loc.crossing:
desiredSpeed = STRIPS_FAILURE_SPEED
else:
desiredSpeed = TRANSITION_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
for sp in allStripPoses(
rects, highResolution=drone.videoHighResolution):
sPose = Pose(*oldPose).add(tiltCompensation).add(sp)
viewlog.dumpBeacon(sPose.coord(), index=3)
viewlog.dumpObstacles([[
(sPose.x - 0.15 * math.cos(sPose.heading),
sPose.y - 0.15 * math.sin(sPose.heading)),
(sPose.x + 0.15 * math.cos(sPose.heading),
sPose.y + 0.15 * math.sin(sPose.heading))
]])
refCircle, refLine = loc.getRefCircleLine(
Pose(drone.coord[0], drone.coord[1], drone.heading))
if refCircle == None and refLine == None and virtualRefCircle != None:
refCircle = virtualRefCircle
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
assert refCircle == None or refLine == None # they cannot be both active at the same time
if refCircle:
if pathType == PATH_TURN_LEFT:
errY = refCircle[1] - math.hypot(
drone.coord[0] - refCircle[0][0],
drone.coord[1] - refCircle[0][1])
errA = normalizeAnglePIPI(
-math.atan2(refCircle[0][1] - drone.coord[1],
refCircle[0][0] - drone.coord[0]) -
math.radians(-90) + drone.heading)
if pathType == PATH_TURN_RIGHT:
errY = math.hypot(
drone.coord[0] - refCircle[0][0],
drone.coord[1] - refCircle[0][1]) - refCircle[1]
errA = normalizeAnglePIPI(
-math.atan2(refCircle[0][1] - drone.coord[1],
refCircle[0][0] - drone.coord[0]) -
math.radians(90) + drone.heading)
if refLine:
errY = refLine.signedDistance(drone.coord)
errA = normalizeAnglePIPI(drone.heading - refLine.angle)
# get the height first
if drone.coord[
2] < desiredHeight - 0.1 and drone.time - startTime < 5.0:
sx = 0.0
if refCircle == None and refLine == None and virtualRefCircle == None:
sx = 0.0 # wait for Z-up
if drone.coord[2] > desiredHeight - 0.1:
print "USING VIRTUAL LEFT TURN CIRCLE!"
circCenter = Pose(drone.coord[0], drone.coord[1],
drone.heading).add(
Pose(0.0, REF_CIRCLE_RADIUS,
0)).coord()
viewlog.dumpBeacon(circCenter, index=0)
virtualRefCircle = circCenter, REF_CIRCLE_RADIUS
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (
desiredSpeed / 0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA(sx, sy, sz, sa)
maxControlGap = max(drone.time - prevTime, maxControlGap)
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR)))
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel(front=True)
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def updateExtension(self, robot, id, data):
if id == 'remission' and len(data) > 0:
pass # ignored for FRE 2014
if id == 'laser' and len(data) > 0:
self.poseHistory.append(robot.localisation.pose()[:2])
if len(self.poseHistory) > 20:
self.poseHistory = self.poseHistory[-20:]
step = 10
data2 = [x == 0 and 10000 or x for x in data]
arr = [
min(i) / 1000.0 for i in [
itertools.islice(data2, start, start + step)
for start in range(0, len(data2), step)
]
]
arr.reverse()
robot.preprocessedLaser = arr
limit = self.radius
danger = False
prevCenter = self.center
if self.center != None:
# if center > 0 and center < len(arr)-1 and arr[center] < limit and arr[center-1] < limit and arr[center+1] < limit:
if self.center > 0 and self.center < len(arr) - 1 and arr[
self.center] < limit:
print "!!!DANGER!!! PATH BLOCKED!!!", self.center
danger = True
if self.center == None or danger:
# offset is not set - find nearest
if self.center == None:
self.center = len(arr) / 2
if arr[self.center] < limit:
i = 0
while i < self.center:
if arr[self.center - i] >= limit:
break
i += 1
left = i
i = 0
while i + self.center < len(arr):
if arr[self.center + i] >= limit:
break
i += 1
right = i
print "HACK", left, right
if left < right:
self.center += -left
else:
self.center += right
i = 0
if self.center < 0 or self.center >= len(arr):
self.center = prevCenter
if self.center == None:
self.center = len(arr) / 2
while i <= self.center:
if arr[self.center - i] < limit:
break
i += 1
left = i # arr is already reversed
i = 0
while i + self.center < len(arr):
if arr[self.center + i] < limit:
break
i += 1
right = i
p = robot.localisation.pose()
if self.center - left >= 0:
self.lastLeft = combinedPose(
(p[0], p[1], p[2] + math.radians(135 - 5 *
(self.center - left))),
(arr[self.center - left], 0, 0))
# viewlog.dumpBeacon(self.lastLeft[:2], color=(0,128,0))
if self.center + right < len(arr):
self.lastRight = combinedPose(
(p[0], p[1], p[2] + math.radians(135 - 5 *
(self.center + right))),
(arr[self.center + right], 0, 0))
# viewlog.dumpBeacon(self.lastRight[:2], color=(0,128,50))
if self.verbose:
if danger and left + right < MIN_GAP_SIZE:
print "MIN GAP SIZE danger:", left + right
s = ''
for i in arr:
s += (i < 0.5 and 'X'
or (i < 1.0 and 'x' or (i < 1.5 and '.' or ' ')))
s = "".join(
list(s)[:self.center] + ['C'] + list(s)[self.center:])
print "'" + s + "'"
# print "LRLR\t%d\t%d\t%d" % (left, right, left+right)
# if self.prevLR :
# print "LRDIFF", left-self.prevLR[0], right-self.prevLR[1]
self.line = None # can be defined inside
if left + right <= MAX_GAP_SIZE: # TODO limit based on minSize, radius and sample angle
self.center += (right - left) / 2
offset = self.center - len(arr) / 2
self.directionAngle = math.radians(-offset * step / 2.0)
self.collisionAhead = min(arr[54 / 3:2 * 54 / 3]), min(
arr[4 * 54 / 9:5 * 54 / 9]), (left + right < MIN_GAP_SIZE)
if False: #self.verbose:
if self.collisionAhead[0] < 0.25:
print "!!! COLISSION AHEAD !!!", self.collisionAhead
else:
print "free space", self.collisionAhead
elif left < 3 or right < 3:
# wide row & collision ahead
if self.verbose:
print "NEAR", left, right, left + right
offset = self.center - len(arr) / 2
if left < right:
offset += 3
else:
offset -= 3
self.directionAngle = math.radians(-offset * step / 2.0)
else:
if self.verbose:
print "OFF", left, right #, left+right, robot.compass
if False: # hacked, ignoring compass self.rowHeading:
self.directionAngle = normalizeAnglePIPI(
self.rowHeading - robot.localisation.pose()[2])
if abs(self.directionAngle) > math.radians(90):
self.directionAngle = normalizeAnglePIPI(
self.rowHeading - robot.localisation.pose()[2] +
math.radians(180))
if self.verbose:
print "DIFF %.1f" % math.degrees(self.directionAngle)
else:
if self.verbose:
print "PATH GAP"
A, B = self.poseHistory[0][:2], self.poseHistory[-1][:2]
viewlog.dumpBeacon(A, color=(0, 0, 180))
viewlog.dumpBeacon(B, color=(0, 30, 255))
self.line = Line(
B, (2 * B[0] - A[0], 2 * B[1] - A[1])) # B+(B-A)
if self.line and self.line.length < 0.5:
self.line = None
else:
self.center = len(arr) / 2
# reset center
"""if self.prevLR:
if abs(left-self.prevLR[0]) > 4 and abs(right-self.prevLR[1]) < 3:
left = self.prevLR[0]
self.center += (right-left)/2
offset = self.center-len(arr)/2
self.directionAngle = math.radians( -offset*step/2.0 )
elif abs(right-self.prevLR[1]) > 4 and abs(left-self.prevLR[0]) < 3:
right = self.prevLR[1]
self.center += (right-left)/2
offset = self.center-len(arr)/2
self.directionAngle = math.radians( -offset*step/2.0 )
else:
self.directionAngle = 0.0 # if you do not know, go ahead
else:
self.directionAngle = 0.0 # if you do not know, go ahead"""
self.directionAngle = 0.0 # default
if left >= 17 and right >= 17 or left + right >= 40:
self.endOfRow = True
if self.verbose and robot.insideField:
print "laser: END OF ROW"
self.prevLR = left, right
pose = robot.localisation.pose()
goal = combinedPose(
(pose[0], pose[1], pose[2] + self.directionAngle),
(self.radius, 0, 0))
if self.line == None:
self.line = Line(pose, goal)
self.newData = True
self.cornLeft = min(arr[5:9])
self.cornRight = min(arr[40:44])
def crossRows2(self, row, num, rowsOnLeft):
IGNORE_NEIGHBORS = 2
ROWS_OFFSET = 0.7 #0.5
# if row.lastLeft:
# viewlog.dumpBeacon(row.lastLeft[:2], color=(0,128,0))
# if row.lastRight:
# viewlog.dumpBeacon(row.lastRight[:2], color=(0,128,50))
start = self.robot.localisation.pose()[:2]
viewlog.dumpBeacon(start, color=(255, 128, 0))
goal = combinedPose(self.robot.localisation.pose(), (1.0, 0, 0))
line = Line(self.robot.localisation.pose(), goal)
lastA, lastB = None, None
ends = []
while True:
for cmd in self.driver.followLineG(line):
self.robot.setSpeedPxPa(*cmd)
self.robot.update()
if row.newData:
row.newData = False
break
else:
print "END OF LINE REACHED!"
if self.robot.preprocessedLaser != None:
tmp = self.robot.preprocessedLaser[:]
tlen = len(tmp)
if rowsOnLeft:
tmp = tmp[:tlen / 2] + [3.0] * (tlen - tlen / 2)
else:
tmp = [3.0] * (tlen - tlen / 2) + tmp[tlen / 2:]
sarr = sorted([(x, i) for (i, x) in enumerate(tmp)])[:5]
ax, ai = sarr[0]
for bx, bi in sarr[1:]:
if abs(ai - bi) > IGNORE_NEIGHBORS:
break
else:
print "NO 2nd suitable minimum"
print(ax, ai), (bx, bi)
if rowsOnLeft:
offset = -ROWS_OFFSET
if ai > bi:
(ax, ai), (bx, bi) = (bx, bi), (ax, ai)
else:
offset = ROWS_OFFSET
if ai < bi: # rows on right
(ax, ai), (bx, bi) = (bx, bi), (ax, ai)
p = self.robot.localisation.pose()
A = combinedPose(
(p[0], p[1], p[2] + math.radians(135 - 5 * ai)),
(ax, 0, 0))
B = combinedPose(
(p[0], p[1], p[2] + math.radians(135 - 5 * bi)),
(bx, 0, 0))
if lastA == None:
ends.extend([A, B])
lastA, lastB = A, B
if self.verbose:
print "DIST", distance(lastA,
A), distance(lastB,
B), distance(lastB, A)
if distance(lastB, A) < 0.2:
dist = distance(start, self.robot.localisation.pose())
print "NEXT ROW", dist
if dist > 0.4:
ends.append(B) # new one
lastA, lastB = A, B
line = Line(A, B) # going through the ends of rows
A2 = combinedPose((A[0], A[1], line.angle), (0, offset, 0))
B2 = combinedPose((B[0], B[1], line.angle), (2.0, offset, 0))
line = Line(A2, B2)
viewlog.dumpBeacon(A[:2], color=(200, 0, 0))
viewlog.dumpBeacon(B[:2], color=(200, 128, 0))
viewlog.dumpBeacon(A2[:2], color=(255, 0, 0))
viewlog.dumpBeacon(B2[:2], color=(255, 128, 0))
if len(ends) > num + 1:
break
else:
print "BACKUP solution!!!"
goal = combinedPose(self.robot.localisation.pose(),
(1.0, 0, 0))
line = Line(self.robot.localisation.pose(), goal)
def competeAirRace( drone, desiredHeight = 1.7 ):
loops = []
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
loc = StripsLocalisation()
remainingFastStrips = 0
desiredSpeed = TRANSITION_SPEED
updateFailedCount = 0
try:
drone.wait(1.0)
drone.setVideoChannel( front=False )
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
pathType = PATH_TURN_LEFT
virtualRefCircle = None
startTime = drone.time
sx,sy,sz,sa = 0,0,0,0
lastUpdate = None
while drone.time < startTime + 600.0:
sz = max( -0.2, min( 0.2, desiredHeight - drone.coord[2] ))
sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))
if drone.lastImageResult:
lastUpdate = drone.time
assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpCamera( "tmp_%04d.jpg" % (frameNumber/15,), 0 )
# keep history small
videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max( videoDelay, maxVideoDelay )
toDel = 0
for oldTime, oldPose, oldAngles in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),
loc.updateFrame( Pose( *oldPose ).add(tiltCompensation), allStripPoses( rects, highResolution=drone.videoHighResolution ) )
if loc.pathType != pathType:
print "TRANS", pathType, "->", loc.pathType
if pathType == PATH_TURN_LEFT and loc.pathType == PATH_STRAIGHT:
if len(loops) > 0:
print "Loop %d, time %d" % (len(loops), drone.time-loops[-1])
print "-----------------------------------------------"
loops.append( drone.time )
if drone.magneto[:3] == magnetoOnStart:
print "!!!!!!!! COMPASS FAILURE !!!!!!!!"
pathType = loc.pathType
print "----"
remainingFastStrips = NUM_FAST_STRIPS
if loc.crossing:
print "X", True, remainingFastStrips
else:
print pathType, loc.pathUpdated, remainingFastStrips
if not loc.pathUpdated:
updateFailedCount += 1
if updateFailedCount > 1:
print "UPDATE FAILED", updateFailedCount
else:
updateFailedCount = 0
if remainingFastStrips > 0:
remainingFastStrips -= 1
desiredSpeed = MAX_ALLOWED_SPEED
if not loc.pathUpdated and not loc.crossing:
desiredSpeed = STRIPS_FAILURE_SPEED
else:
desiredSpeed = TRANSITION_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
for sp in allStripPoses( rects, highResolution=drone.videoHighResolution ):
sPose = Pose( *oldPose ).add(tiltCompensation).add( sp )
viewlog.dumpBeacon( sPose.coord(), index=3 )
viewlog.dumpObstacles( [[(sPose.x-0.15*math.cos(sPose.heading), sPose.y-0.15*math.sin(sPose.heading)),
(sPose.x+0.15*math.cos(sPose.heading), sPose.y+0.15*math.sin(sPose.heading))]] )
refCircle,refLine = loc.getRefCircleLine( Pose(drone.coord[0], drone.coord[1], drone.heading) )
if refCircle == None and refLine == None and virtualRefCircle != None:
refCircle = virtualRefCircle
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
assert refCircle == None or refLine == None # they cannot be both active at the same time
if refCircle:
if pathType == PATH_TURN_LEFT:
errY = refCircle[1] - math.hypot( drone.coord[0]-refCircle[0][0], drone.coord[1]-refCircle[0][1] )
errA = normalizeAnglePIPI( - math.atan2( refCircle[0][1] - drone.coord[1], refCircle[0][0] - drone.coord[0] )
- math.radians(-90) + drone.heading )
if pathType == PATH_TURN_RIGHT:
errY = math.hypot( drone.coord[0]-refCircle[0][0], drone.coord[1]-refCircle[0][1] ) - refCircle[1]
errA = normalizeAnglePIPI( - math.atan2( refCircle[0][1] - drone.coord[1], refCircle[0][0] - drone.coord[0] )
- math.radians(90) + drone.heading )
if refLine:
errY = refLine.signedDistance( drone.coord )
errA = normalizeAnglePIPI( drone.heading - refLine.angle )
# get the height first
if drone.coord[2] < desiredHeight - 0.1 and drone.time-startTime < 5.0:
sx = 0.0
if refCircle == None and refLine == None and virtualRefCircle == None:
sx = 0.0 # wait for Z-up
if drone.coord[2] > desiredHeight - 0.1:
print "USING VIRTUAL LEFT TURN CIRCLE!"
circCenter = Pose( drone.coord[0], drone.coord[1], drone.heading ).add( Pose(0.0, REF_CIRCLE_RADIUS, 0 )).coord()
viewlog.dumpBeacon( circCenter, index=0 )
virtualRefCircle = circCenter, REF_CIRCLE_RADIUS
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA( sx, sy, sz, sa )
maxControlGap = max( drone.time - prevTime, maxControlGap )
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel( front=True )
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
