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 testVFH(self, verbose=False):
turnRadius = None # 1.2
vfh = VFH(
self.robot, blockingDistance=0.35, safetyDistance=0.6, maxRange=1.4, turnRadius=turnRadius, verbose=verbose
)
self.robot.addExtension(vfh.updateExtension)
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
D = 20.0 # [m]
waypoints = [(D, 0.0), (D, D), (0.0, D), (0.0, 0.0)]
prevDir = 0.0 # [rad]
while True:
for (x, y) in waypoints:
isThere = False
while not isThere:
pose = self.robot.localisation.pose()
if vfh.laserMeasurements is None:
strategy = self.driver.stopG()
prevDir = 0.0
else:
phi = math.atan2(y - pose[1], x - pose[0])
goalDir = normalizeAnglePIPI(phi - pose[2])
dir = vfh.navigate(goalDir, prevDir)
if dir is None:
strategy = self.driver.stopG()
prevDir = 0.0
else:
goal = combinedPose((pose[0], pose[1], pose[2] + dir), (1.0, 0, 0))
strategy = self.driver.goToG(
goal, TOLERATED_MISS, angleThreshold=ANGULAR_THRESHOLD, angularSpeed=ANGULAR_SPEED
)
prevDir = dir
vfh.laserMeasurements = None
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
pose = self.robot.localisation.pose()
isThere = math.hypot(pose[0] - x, pose[1] - y) <= TOLERATED_MISS
if isThere or vfh.laserMeasurements is not None:
break
def compassCheck():
return (self.robot.compass is not None and abs(
normalizeAnglePIPI(backHeading - startHeading)) > math.pi / 2)
def waypoint2dir( self, pose, waypoint ): "return direction to waypoint for current pose" absAngle = math.atan2(waypoint[1]-pose[1], waypoint[0]-pose[0]) relAngle = absAngle - pose[2] return normalizeAnglePIPI(relAngle)
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 ver2( self, code, detectWeeds = True, detectBlockedRow = True ):
print "Field Robot - LASER & CAMERA"
try:
# start GPS sooner to get position fix
# self.robot.gps.start()
self.robot.laser.start()
self.waitForStart()
if not self.robot.switchBlueSelected:
print "RED -> mirroring code directions!!!"
code = [-x for x in code]
print code
if self.robot.compass:
self.rowHeading = compassHeading(self.robot.compass)
self.robot.camera.start()
laserRow = LaserRow( verbose = self.verbose, rowHeading = self.robot.localisation.pose()[2] )
self.robot.addExtension( laserRow.updateExtension )
cameraRow = CameraRow( verbose = self.verbose )
self.robot.addExtension( cameraRow.updateExtension )
angularSpeed = 0.0
self.robot.maxSpeed = 0.2
speed = 0 # wait for first camera image (maybe beep?? if no route to host)
startTime = self.robot.time
row = laserRow
row.reset( offsetDeg=None ) # just for test, try to find the gap (kitchen test)
# row = cameraRow
for action in code:
self.robot.insideFiled = True
print "================= ACTION ", action, "================="
print "battery:", self.robot.battery
while not row.endOfRow:
# sprayer( self.robot, True, True )
for (speed, angularSpeed) in self.driver.followLineG( row.line ):
# print "TEST %.2f" % (math.degrees(normalizeAnglePIPI(row.line.angle - self.robot.localisation.pose()[2])))
if abs(normalizeAnglePIPI(row.line.angle - self.robot.localisation.pose()[2])) > SLOW_DOWN_ANGLE:
speed *= 0.9
if self.robot.laserData:
self.robot.toDisplay = 'OK'
else:
self.robot.toDisplay = '--'
speed, angularSpeed = 0.0, 0.0
# if detectBlockedRow and row.collisionAhead[1] < 0.25:
blockedCamCount = 0
if detectBlockedRow and self.robot.cameraData:
camDat, fileName = self.robot.cameraData
if camDat:
blockedCamCount = int(camDat.split()[0])
if blockedCamCount > RED_ALERT:
print "CAMERA BLOCKED!!", blockedCamCount
# print self.robot.cameraData
# if detectBlockedRow and row.collisionAhead[2]:
if detectBlockedRow and row.collisionAhead[3] and blockedCamCount > RED_ALERT:
# if detectBlockedRow and blocked:
print "---------- COLLISON -> TURN 180 -------------"
self.robot.beep = 1
self.driver.stop()
# self.driver.goStraight( -0.5 )
self.driver.turn( math.radians(90), radius = 0.12, angularSpeed=math.radians(120), withStop=False )
self.driver.turn( math.radians(90), radius = -0.12, angularSpeed=math.radians(120) )
self.robot.beep = 0
# self.driver.turn( math.radians(180), radius = 0.075 )
# self.driver.turn( math.radians(180), radius = 0.0 )
# self.driver.turn( math.radians(20), radius = 0.37, angularSpeed=math.radians(20) )
# self.driver.turn( math.radians(70), radius = 0.08, angularSpeed=math.radians(20) )
# self.driver.turn( math.radians(70), radius = -0.08, angularSpeed=math.radians(20) )
# self.driver.turn( math.radians(20), radius = -0.37, angularSpeed=math.radians(20) )
self.robot.setSpeedPxPa( 0.0, 0.0 )
row.reset( self.robot.localisation.pose(), offsetDeg = None ) # instead of turning shift search
else:
# if detectBlockedRow and (row.collisionAhead[0] < 0.5 or row.collisionAhead[1] < 0.7):
# if self.verbose:
# print "!!SLOW DOWN!!"
# speed, angularSpeed = speed/2.0, angularSpeed/2.0 # slow down
self.robot.setSpeedPxPa( speed, angularSpeed )
self.robot.update()
if row.newData:
row.newData = False
break
# sprayer( self.robot, False, False )
# handle action after row termination
self.robot.insideField = False
self.robot.beep = 1
self.driver.stop()
self.robot.beep = 0
if action == 0:
self.driver.goStraight( 0.2 )
prevAngle = self.robot.localisation.pose()[2]
if not self.driver.turn( math.radians(180), timeout=7.0 ):
self.driver.goStraight( -0.2 )
currAngle = self.robot.localisation.pose()[2]
print "TIMEOUT", math.degrees(normalizeAnglePIPI(math.radians(180)-currAngle+prevAngle))
if not self.driver.turn( normalizeAnglePIPI(math.radians(180)-currAngle+prevAngle), timeout=5.0 ):
currAngle = self.robot.localisation.pose()[2]
print "TIMEOUT2 - GIVING UP", math.degrees(normalizeAnglePIPI(math.radians(180)-currAngle-prevAngle))
elif action == -1:
self.driver.turn( math.radians(160), radius = 0.75/2.0, angularSpeed=math.radians(40) )
elif action == 1:
self.driver.turn( math.radians(-160), radius = 0.75/2.0, angularSpeed=math.radians(40) )
else:
if action < 0:
self.driver.turn( math.radians(90), radius = self.rowWidth/2.0, angularSpeed=math.radians(40) )
else:
self.driver.turn( math.radians(-90), radius = self.rowWidth/2.0, angularSpeed=math.radians(40) )
# self.driver.goStraight( (math.fabs(action)-1) * (self.rowWidth+self.rowPotsWidth)+self.rowPotsWidth )
self.crossRows( row, math.fabs(action)-1, rowsOnLeft = (action < 0) )
self.driver.stop()
if action < 0:
self.driver.turn( math.radians(90), radius = self.rowWidth/2.0, angularSpeed=math.radians(40) )
# self.driver.turn( math.radians(90), radius = 0.0, angularSpeed=math.radians(40) )
else:
self.driver.turn( math.radians(-90), radius = self.rowWidth/2.0, angularSpeed=math.radians(40) )
# self.driver.turn( math.radians(-90), radius = 0.0, angularSpeed=math.radians(40) )
# clear flag for detection
# row.reset( self.robot.localisation.pose() )
row.reset( self.robot.localisation.pose(), offsetDeg=None ) # init with search for center
self.robot.setSpeedPxPa( 0.0, 0.0 )
self.robot.update()
except EmergencyStopException, e:
print "EmergencyStopException"
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 waypoint2dir(self, pose, waypoint):
"return direction to waypoint for current pose"
absAngle = math.atan2(waypoint[1] - pose[1], waypoint[0] - pose[0])
relAngle = absAngle - pose[2]
return normalizeAnglePIPI(relAngle)
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 compassCheck():
return self.robot.compass is not None and abs(normalizeAnglePIPI(backHeading - startHeading)) > math.pi / 2
def ver2(self, code, detectWeeds=True, detectBlockedRow=True):
print "Field Robot - LASER & CAMERA"
try:
# start GPS sooner to get position fix
# self.robot.gps.start()
self.robot.laser.start()
self.waitForStart()
if not self.robot.switchBlueSelected:
print "RED -> mirroring code directions!!!"
code = [-x for x in code]
print code
if self.robot.compass:
self.rowHeading = compassHeading(self.robot.compass)
self.robot.camera.start()
laserRow = LaserRow(verbose=self.verbose,
rowHeading=self.robot.localisation.pose()[2])
self.robot.addExtension(laserRow.updateExtension)
cameraRow = CameraRow(verbose=self.verbose)
self.robot.addExtension(cameraRow.updateExtension)
angularSpeed = 0.0
self.robot.maxSpeed = 0.2
speed = 0 # wait for first camera image (maybe beep?? if no route to host)
startTime = self.robot.time
row = laserRow
row.reset(offsetDeg=None
) # just for test, try to find the gap (kitchen test)
# row = cameraRow
for action in code:
self.robot.insideFiled = True
print "================= ACTION ", action, "================="
print "battery:", self.robot.battery
while not row.endOfRow:
# sprayer( self.robot, True, True )
for (speed,
angularSpeed) in self.driver.followLineG(row.line):
# print "TEST %.2f" % (math.degrees(normalizeAnglePIPI(row.line.angle - self.robot.localisation.pose()[2])))
if abs(
normalizeAnglePIPI(
row.line.angle -
self.robot.localisation.pose()[2])
) > SLOW_DOWN_ANGLE:
speed *= 0.9
if self.robot.laserData:
self.robot.toDisplay = 'OK'
else:
self.robot.toDisplay = '--'
speed, angularSpeed = 0.0, 0.0
# if detectBlockedRow and row.collisionAhead[1] < 0.25:
blockedCamCount = 0
if detectBlockedRow and self.robot.cameraData:
camDat, fileName = self.robot.cameraData
if camDat:
blockedCamCount = int(camDat.split()[0])
if blockedCamCount > RED_ALERT:
print "CAMERA BLOCKED!!", blockedCamCount
# print self.robot.cameraData
# if detectBlockedRow and row.collisionAhead[2]:
if detectBlockedRow and row.collisionAhead[
3] and blockedCamCount > RED_ALERT:
# if detectBlockedRow and blocked:
print "---------- COLLISON -> TURN 180 -------------"
self.robot.beep = 1
self.driver.stop()
# self.driver.goStraight( -0.5 )
self.driver.turn(math.radians(90),
radius=0.12,
angularSpeed=math.radians(120),
withStop=False)
self.driver.turn(math.radians(90),
radius=-0.12,
angularSpeed=math.radians(120))
self.robot.beep = 0
# self.driver.turn( math.radians(180), radius = 0.075 )
# self.driver.turn( math.radians(180), radius = 0.0 )
# self.driver.turn( math.radians(20), radius = 0.37, angularSpeed=math.radians(20) )
# self.driver.turn( math.radians(70), radius = 0.08, angularSpeed=math.radians(20) )
# self.driver.turn( math.radians(70), radius = -0.08, angularSpeed=math.radians(20) )
# self.driver.turn( math.radians(20), radius = -0.37, angularSpeed=math.radians(20) )
self.robot.setSpeedPxPa(0.0, 0.0)
row.reset(self.robot.localisation.pose(),
offsetDeg=None
) # instead of turning shift search
else:
# if detectBlockedRow and (row.collisionAhead[0] < 0.5 or row.collisionAhead[1] < 0.7):
# if self.verbose:
# print "!!SLOW DOWN!!"
# speed, angularSpeed = speed/2.0, angularSpeed/2.0 # slow down
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
if row.newData:
row.newData = False
break
# sprayer( self.robot, False, False )
# handle action after row termination
self.robot.insideField = False
self.robot.beep = 1
self.driver.stop()
self.robot.beep = 0
if action == 0:
self.driver.goStraight(0.2)
prevAngle = self.robot.localisation.pose()[2]
if not self.driver.turn(math.radians(180), timeout=7.0):
self.driver.goStraight(-0.2)
currAngle = self.robot.localisation.pose()[2]
print "TIMEOUT", math.degrees(
normalizeAnglePIPI(
math.radians(180) - currAngle + prevAngle))
if not self.driver.turn(normalizeAnglePIPI(
math.radians(180) - currAngle + prevAngle),
timeout=5.0):
currAngle = self.robot.localisation.pose()[2]
print "TIMEOUT2 - GIVING UP", math.degrees(
normalizeAnglePIPI(
math.radians(180) - currAngle - prevAngle))
elif action == -1:
self.driver.turn(math.radians(160),
radius=0.75 / 2.0,
angularSpeed=math.radians(40))
elif action == 1:
self.driver.turn(math.radians(-160),
radius=0.75 / 2.0,
angularSpeed=math.radians(40))
else:
if action < 0:
self.driver.turn(math.radians(90),
radius=self.rowWidth / 2.0,
angularSpeed=math.radians(40))
else:
self.driver.turn(math.radians(-90),
radius=self.rowWidth / 2.0,
angularSpeed=math.radians(40))
# self.driver.goStraight( (math.fabs(action)-1) * (self.rowWidth+self.rowPotsWidth)+self.rowPotsWidth )
self.crossRows(row,
math.fabs(action) - 1,
rowsOnLeft=(action < 0))
self.driver.stop()
if action < 0:
self.driver.turn(math.radians(90),
radius=self.rowWidth / 2.0,
angularSpeed=math.radians(40))
# self.driver.turn( math.radians(90), radius = 0.0, angularSpeed=math.radians(40) )
else:
self.driver.turn(math.radians(-90),
radius=self.rowWidth / 2.0,
angularSpeed=math.radians(40))
# self.driver.turn( math.radians(-90), radius = 0.0, angularSpeed=math.radians(40) )
# clear flag for detection
# row.reset( self.robot.localisation.pose() )
row.reset(self.robot.localisation.pose(),
offsetDeg=None) # init with search for center
self.robot.setSpeedPxPa(0.0, 0.0)
self.robot.update()
except EmergencyStopException, e:
print "EmergencyStopException"
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])
