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 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 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 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 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) ):
print "RESET ROW (%0.2f, %0.2f, %0.1f)" % (pose[0], pose[1], math.degrees(pose[2]) )
self.preference = None
self.directionAngle = 0.0
self.offset = 0
goal = combinedPose( (pose[0], pose[1], pose[2]+self.directionAngle), (1.0, 0, 0) ) # was radius
self.line = Line( pose, goal )
self.newData = False
self.traversable = None
self.obstacles = []
self.step = 10 # how many neighboring measurements are merged together
#TODO: get the laser parameters somewhere
h = 310 # Height of the laser above ground. [mm]
alpha = math.radians(81.0) # An angle between a vertical direction and a forward measurement beam.
n = 541 # Number of values provided by the laser
self.laserRight = math.radians(-135.0) # Left-most angle measured by the laser. [rad]
self.laserLeft = math.radians(+135.0) # Right-most angle measured by the laser. [rad]
self.expectation = [] # Expected distances to the ground plane. [mm]
epsilon = 1.0e-3
angle_threshold = math.pi / 3 #math.pi / 2.0 - epsilon
for i in xrange(n):
beta = self.laserRight + (self.laserLeft - self.laserRight) * i / float(n)
if abs(beta) >= angle_threshold:
beta = angle_threshold * (1 if beta > 0 else -1) # Making sure we always have an "intersection" in a reasonable
x = h / (math.cos(alpha) * math.cos(beta))
self.expectation.append(x)
avg_decim = lambda sl: sum(sl) / self.step
self.decimatedExpectation = self.__decimate(self.expectation, decimator = avg_decim)
def cam2waypoint( self, pose, camInfo ):
"estimate from camera info (x,y,width,height) new waypoint"
if camInfo is None:
dir = 0
dist = 10.0
else:
dir = math.radians(0.1* (640/2-(camInfo[1][0]+camInfo[1][2]/2.0)))
dist = 20.0/float(camInfo[1][2]) # TODO calibrate
return combinedPose( (pose[0], pose[1], pose[2]+dir), (dist, 0, 0) )
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 cam2waypoint(self, pose, camInfo):
"estimate from camera info (x,y,width,height) new waypoint"
if camInfo is None:
dir = 0
dist = 10.0
else:
dir = math.radians(0.1 * (640 / 2 -
(camInfo[1][0] + camInfo[1][2] / 2.0)))
dist = 20.0 / float(camInfo[1][2]) # TODO calibrate
return combinedPose((pose[0], pose[1], pose[2] + dir), (dist, 0, 0))
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 updateData( self ):
pose = (0,0,0)
if self.robot and self.robot.localisation:
pose = self.robot.localisation.pose()
step = 10
for i in xrange(0,541,step):
angle = math.radians((i-270)/2.0)
absPose = combinedPose( pose, (0,0,angle) ) # TODO shift of laser
simulatedDist = rayTrace( absPose, self.obstacles )
for j in xrange(step):
if i + j < 541:
self.data[i+j] = simulatedDist*1000
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 updateData(self):
pose = (0, 0, 0)
if self.robot and self.robot.localisation:
pose = self.robot.localisation.pose()
step = 10
for i in xrange(0, 541, step):
angle = math.radians((i - 270) / 2.0)
absPose = combinedPose(pose, (0, 0, angle)) # TODO shift of laser
simulatedDist = rayTrace(absPose, self.obstacles)
for j in xrange(step):
if i + j < 541:
self.data[i + j] = simulatedDist * 1000
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 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 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 reset(self, pose=(0, 0, 0)):
print "RESET ROW (%0.2f, %0.2f, %0.1f)" % (pose[0], pose[1],
math.degrees(pose[2]))
self.preference = None
self.directionAngle = 0.0
self.offset = 0
goal = combinedPose((pose[0], pose[1], pose[2] + self.directionAngle),
(1.0, 0, 0)) # was radius
self.line = Line(pose, goal)
self.newData = False
self.traversable = None
self.obstacles = []
self.step = 10 # how many neighboring measurements are merged together
#TODO: get the laser parameters somewhere
h = 310 # Height of the laser above ground. [mm]
alpha = math.radians(
81.0
) # An angle between a vertical direction and a forward measurement beam.
n = 541 # Number of values provided by the laser
self.laserRight = math.radians(
-135.0) # Left-most angle measured by the laser. [rad]
self.laserLeft = math.radians(
+135.0) # Right-most angle measured by the laser. [rad]
self.expectation = [] # Expected distances to the ground plane. [mm]
epsilon = 1.0e-3
angle_threshold = math.pi / 3 #math.pi / 2.0 - epsilon
for i in xrange(n):
beta = self.laserRight + (self.laserLeft -
self.laserRight) * i / float(n)
if abs(beta) >= angle_threshold:
beta = angle_threshold * (
1 if beta > 0 else -1
) # Making sure we always have an "intersection" in a reasonable
x = h / (math.cos(alpha) * math.cos(beta))
self.expectation.append(x)
avg_decim = lambda sl: sum(sl) / self.step
self.decimatedExpectation = self.__decimate(self.expectation,
decimator=avg_decim)
def updateExtension( self, robot, id, data ):
if id == 0x80:
# virtual bumper
if math.fabs((robot._rampLastLeft+robot._rampLastRight)/2) > 0.2 and math.fabs(robot.currentSpeed) < 0.01:
self.virtualBumper += 1
if self.virtualBumper > 40:
print "VIRTUAL BUMPER", (robot._rampLastLeft+robot._rampLastRight)/2, robot.currentSpeed
self.collision = True
else:
self.virtualBumper = 0
if id == 'laser' and len(data) > 0:
# handle 0 = timeout and find min
near = 10.0
for x in islice(data, 270-90, 270+90):
if x != 0 and x < near*1000:
near = x/1000.0
# TODO for 1m, slow down and define alternative route
if near < 0.5:
self.collision = True
step = 10
data2 = [x == 0 and 10000 or x for x in data]
arr = [min(i)/1000.0 for i in [islice(data2, start, start+step) for start in range(0,len(data2),step)]]
arr.reverse()
limit = self.radius
center = len(arr)/2 + self.offset
center = min(max(center, 0), len(arr)-1)
i = 0
while i < center:
if arr[center - i] < limit:
break
i += 1
left = i # arr is already reversed
i = 0
while i + center < len(arr):
if arr[center + i] < limit:
break
i += 1
right = i
s = ''
for i in arr:
s += (i<1.0 and 'x' or ' ')
s = "".join( list(s)[:center] + ['C'] + list(s)[center:] )
if self.verbose:
print "'" + s + "'"
if left < 5 or right < 5:
# wide row & collision ahead
if self.verbose:
print left, right, left+right
if left < right:
self.offset += 5
else:
self.offset -= 5
self.directionAngle = math.radians( -self.offset*step/2.0 )
pose = robot.localisation.pose()
goal = combinedPose( (pose[0], pose[1], pose[2]+self.directionAngle), (self.radius, 0, 0) )
if not self.collision:
self.generator = self.driver.followLineG( Line( pose, goal ) )
elif left >= 10 or right > 10:
self.offset = 0
if id == 0x186:
assert( len(data) )
sonar = (data[1]*256 + data[0])*340.0/2.0/1000000.0
if sonar < 0.5:
self.collision = True
# if self.verbose:
# print "sonar %.2f" % sonar
if id == 'camera':
if self.verbose:
print data[1]
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 updateExtension(self, robot, id, data):
if id == 'laser' and len(data) > 0:
# Is there an obstacle somewhere very close?
lbound = len(robot.laserData) / 6
rbound = len(robot.laserData) * 5 / 6
# (lbound, rbound) should cover the space ahead of the robot.
obstacle_threshold = 300 # [mm]
for x in islice(robot.laserData, lbound, rbound):
if x > 0 and x < obstacle_threshold:
self.line = None
self.newData = True
self.directionAngle = 0.0
if self.verbose:
s = ''
for start in xrange(0, len(robot.laserData),
self.step):
too_close = False
for x in islice(robot.laserData, start,
start + self.step):
if x > 0 and x < obstacle_threshold:
too_close = True
break
s += '!' if too_close else '-'
print "'" + s[::
-1] + "'" # The reverse is needed to match the printed traversability pattern.
self.traversable = None
self.obstacles = []
return
# Preprocess the data, so that the computation is not repeated multiple times.
prepData = [self.FAR_AWAY if x == 0 else x for x in data]
backData = [
self.expectation[i] - x for (i, x) in enumerate(prepData)
] # distances measured from the expected ground distance
# Traversability stemming from "there is no obstacle close there".
decimatedPrepData = self.__decimate(prepData, decimator=min)
decimatedBackData = self.__decimate(backData, decimator=max)
trav_obs = self.obstacleTraversability(decimatedBackData)
self.traversable = trav_obs
self.obstacles = []
for i in xrange(len(self.traversable)):
if not self.traversable[i]:
phi = self.laserRight + (self.laserLeft -
self.laserRight) * i / (
len(self.traversable) - 1)
dist = decimatedPrepData[i] / 1000.0
self.obstacles.append((phi, dist))
center = len(self.traversable) / 2 + self.offset
if center < 0: center = 0
elif center >= len(self.traversable):
center = len(self.traversable) - 1
# Look for a traversable direction. The more in the expected direction, the better.
for i in range(max(center, len(self.traversable) - center)):
if center - i >= 0 and self.traversable[center - i]:
center -= i
break
elif center + i < len(
self.traversable) and self.traversable[center + i]:
center += i
break
#If the center is not traversable, we should better STOP!!!
if not self.traversable[center]:
self.line = None
self.newData = True
self.directionAngle = 0.0
if self.verbose:
print '+' * len(self.traversable)
self.traversable = None
self.obstacles = []
return
# Find borders of the traversable gap.
i = 0
while i < center:
if not self.traversable[center - i]:
break
i += 1
right = i
i = 0
while i + center < len(self.traversable):
if not self.traversable[center + i]:
break
i += 1
left = i
# Spit the information.
s = ''
for t in self.traversable:
s += (' ' if t else 'x')
s = "".join(list(s)[:center] + ['C'] + list(s)[center:])
s = s[::-1]
if self.verbose:
print "'" + s + "'"
# print center, left, right, left+right
# Update the belief.
if left + right <= 10: # TODO limit based on minSize, radius and sample angle
self.offset += (left - right) / 2
self.directionAngle = math.radians(self.offset * self.step /
2.0)
elif left < 5 or right < 5:
# wide row & collision ahead
print left, right, left + right
if right < left:
self.offset += 5
else:
self.offset -= 5
self.directionAngle = math.radians(self.offset * self.step /
2.0)
else:
# print "OFF", left, right, left+right
self.directionAngle = 0.0 # if you do not know, go ahead
pose = robot.localisation.pose()
goal = combinedPose(
(pose[0], pose[1], pose[2] + self.directionAngle), (1.0, 0, 0))
self.line = Line(pose, goal)
self.newData = True
if id == 'camera':
if self.verbose:
print data[1]
def goToVfhDigit( self, digit, timeout, info = None ):
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
RETREAT = 0.2 # A retreated distance after endurance times out. [m]
pathFinder = LaserPath( verbose=False )
self.robot.addExtension( pathFinder.updateExtension, "PATH" )
turnRadius = None #1.2
# vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
vfh = VFH(self.robot, blockingDistance = 0.2, safetyDistance = 0.3, maxRange = 1.0, turnRadius = None, verbose = False)
self.robot.addExtension(vfh.updateExtension, "VHF")
# Navigate roughly in the suggested direction, avoiding obstacles.
prevDir = 0.0
waypoint = self.cam2waypoint( self.robot.localisation.pose(), info )
goalDir = self.waypoint2dir( self.robot.localisation.pose(), waypoint )
print "GOAL DIR", goalDir
oldCam = self.robot.cameraData
countImages = 0
countVerified = 0
startTime = self.robot.time
happyTime = self.robot.time
while self.robot.time < startTime + timeout:
if oldCam != self.robot.cameraData:
oldCam = self.robot.cameraData
countImages += 1
if self.robot.cameraData and self.robot.cameraData[0]:
info = None
for d in parseCameraData( self.robot.cameraData ):
# TODO switch to goToDigit for large enough number
if d[0] == digit:
info = d
if info != None:
countVerified += 1
waypoint = self.cam2waypoint( self.robot.localisation.pose(), info )
goalDir = self.waypoint2dir( self.robot.localisation.pose(), waypoint )
print "GOAL DIR", goalDir, oldCam, info, digit
dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
if dir is None or not pathFinder.isTraversable(dir):
strategy = zeroCmd()
prevDir = 0.0
self.robot.toDisplay = 'HH'
else:
pose = self.robot.localisation.pose()
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
self.robot.toDisplay = 'OK'
happyTime = self.robot.time
if self.robot.time - happyTime > ENDURANCE:
break # does not have sense to wait - try to turn somewhere else ...
# We are stuck for too long. Let's try to retreat a bit:
# 1) to show we are alive,
# 2) to hope the robot gets out of the blocked state.
# Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
pose = self.robot.localisation.pose()
goal = combinedPose( pose, (-(RETREAT + TOLERATED_MISS), 0, 0) )
strategy = self.driver.goToG( goal , TOLERATED_MISS, backward = True, angleThreshold = ANGULAR_THRESHOLD, angularSpeed = ANGULAR_SPEED)
self.robot.toDisplay = '\/'
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa( speed, angularSpeed )
self.robot.update()
happyTime = self.robot.time
continue # TODO maybe replace by return -> give up
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa( speed, angularSpeed )
self.robot.update()
pose = self.robot.localisation.pose()
# isThere = math.hypot(pose[0] - lastX, pose[1] - lastY) <= TOLERATED_MISS
# if isThere or pathFinder.newData:
if pathFinder.newData:
pathFinder.newData = False
break
self.robot.removeExtension( "VHF" )
self.robot.removeExtension( "PATH" )
def goVfh(self, timeout, maxDist=None):
print "goVfh", maxDist
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
RETREAT = 0.2 # A retreated distance after endurance times out. [m]
pathFinder = LaserPath(verbose=False)
self.robot.addExtension(pathFinder.updateExtension, "PATH")
turnRadius = None #1.2
# vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
vfh = VFH(self.robot,
blockingDistance=0.2,
safetyDistance=0.3,
maxRange=1.0,
turnRadius=None,
verbose=False)
self.robot.addExtension(vfh.updateExtension, "VHF")
# Navigate roughly in the suggested direction, avoiding obstacles.
prevDir = 0.0
goalDir = 0.0
startTime = self.robot.time
happyTime = self.robot.time
distTraveled = 0.0
while self.robot.time < startTime + timeout:
if maxDist is not None and distTraveled > maxDist:
print "goVfh - maxDist reached"
break
dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
if dir is None or not pathFinder.isTraversable(dir):
strategy = zeroCmd()
prevDir = 0.0
self.robot.toDisplay = 'HH'
else:
pose = self.robot.localisation.pose()
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
self.robot.toDisplay = 'OK'
happyTime = self.robot.time
if self.robot.time - happyTime > ENDURANCE:
print "ENDURANCE!"
break # does not have sense to wait - try to turn somewhere else ...
# We are stuck for too long. Let's try to retreat a bit:
# 1) to show we are alive,
# 2) to hope the robot gets out of the blocked state.
# Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
pose = self.robot.localisation.pose()
goal = combinedPose(pose, (-(RETREAT + TOLERATED_MISS), 0, 0))
strategy = self.driver.goToG(goal,
TOLERATED_MISS,
backward=True,
angleThreshold=ANGULAR_THRESHOLD,
angularSpeed=ANGULAR_SPEED)
self.robot.toDisplay = '\/'
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
distTraveled += self.robot.lastDistStep
happyTime = self.robot.time
continue # TODO maybe replace by return -> give up
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
distTraveled += self.robot.lastDistStep
pose = self.robot.localisation.pose()
if pathFinder.newData:
pathFinder.newData = False
break
self.robot.removeExtension("VHF")
self.robot.removeExtension("PATH")
print "... traveled", distTraveled, "time", self.robot.time - startTime
def goVfh( self, timeout, maxDist=None ):
print "goVfh", maxDist
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
RETREAT = 0.2 # A retreated distance after endurance times out. [m]
pathFinder = LaserPath( verbose=False )
self.robot.addExtension( pathFinder.updateExtension, "PATH" )
turnRadius = None #1.2
# vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
vfh = VFH(self.robot, blockingDistance = 0.2, safetyDistance = 0.3, maxRange = 1.0, turnRadius = None, verbose = False)
self.robot.addExtension(vfh.updateExtension, "VHF")
# Navigate roughly in the suggested direction, avoiding obstacles.
prevDir = 0.0
goalDir = 0.0
startTime = self.robot.time
happyTime = self.robot.time
distTraveled = 0.0
while self.robot.time < startTime + timeout:
if maxDist is not None and distTraveled > maxDist:
print "goVfh - maxDist reached"
break
dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
if dir is None or not pathFinder.isTraversable(dir):
strategy = zeroCmd()
prevDir = 0.0
self.robot.toDisplay = 'HH'
else:
pose = self.robot.localisation.pose()
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
self.robot.toDisplay = 'OK'
happyTime = self.robot.time
if self.robot.time - happyTime > ENDURANCE:
print "ENDURANCE!"
break # does not have sense to wait - try to turn somewhere else ...
# We are stuck for too long. Let's try to retreat a bit:
# 1) to show we are alive,
# 2) to hope the robot gets out of the blocked state.
# Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
pose = self.robot.localisation.pose()
goal = combinedPose( pose, (-(RETREAT + TOLERATED_MISS), 0, 0) )
strategy = self.driver.goToG( goal , TOLERATED_MISS, backward = True, angleThreshold = ANGULAR_THRESHOLD, angularSpeed = ANGULAR_SPEED)
self.robot.toDisplay = '\/'
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa( speed, angularSpeed )
self.robot.update()
distTraveled += self.robot.lastDistStep
happyTime = self.robot.time
continue # TODO maybe replace by return -> give up
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa( speed, angularSpeed )
self.robot.update()
distTraveled += self.robot.lastDistStep
pose = self.robot.localisation.pose()
if pathFinder.newData:
pathFinder.newData = False
break
self.robot.removeExtension( "VHF" )
self.robot.removeExtension( "PATH" )
print "... traveled", distTraveled, "time", self.robot.time - startTime
def goToVfhDigit(self, digit, timeout, info=None):
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
RETREAT = 0.2 # A retreated distance after endurance times out. [m]
pathFinder = LaserPath(verbose=False)
self.robot.addExtension(pathFinder.updateExtension, "PATH")
turnRadius = None #1.2
# vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
vfh = VFH(self.robot,
blockingDistance=0.2,
safetyDistance=0.3,
maxRange=1.0,
turnRadius=None,
verbose=False)
self.robot.addExtension(vfh.updateExtension, "VHF")
# Navigate roughly in the suggested direction, avoiding obstacles.
prevDir = 0.0
waypoint = self.cam2waypoint(self.robot.localisation.pose(), info)
goalDir = self.waypoint2dir(self.robot.localisation.pose(), waypoint)
print "GOAL DIR", goalDir
oldCam = self.robot.cameraData
countImages = 0
countVerified = 0
startTime = self.robot.time
happyTime = self.robot.time
while self.robot.time < startTime + timeout:
if oldCam != self.robot.cameraData:
oldCam = self.robot.cameraData
countImages += 1
if self.robot.cameraData and self.robot.cameraData[0]:
info = None
for d in parseCameraData(self.robot.cameraData):
# TODO switch to goToDigit for large enough number
if d[0] == digit:
info = d
if info != None:
countVerified += 1
waypoint = self.cam2waypoint(
self.robot.localisation.pose(), info)
goalDir = self.waypoint2dir(
self.robot.localisation.pose(), waypoint)
print "GOAL DIR", goalDir, oldCam, info, digit
dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
if dir is None or not pathFinder.isTraversable(dir):
strategy = zeroCmd()
prevDir = 0.0
self.robot.toDisplay = 'HH'
else:
pose = self.robot.localisation.pose()
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
self.robot.toDisplay = 'OK'
happyTime = self.robot.time
if self.robot.time - happyTime > ENDURANCE:
break # does not have sense to wait - try to turn somewhere else ...
# We are stuck for too long. Let's try to retreat a bit:
# 1) to show we are alive,
# 2) to hope the robot gets out of the blocked state.
# Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
pose = self.robot.localisation.pose()
goal = combinedPose(pose, (-(RETREAT + TOLERATED_MISS), 0, 0))
strategy = self.driver.goToG(goal,
TOLERATED_MISS,
backward=True,
angleThreshold=ANGULAR_THRESHOLD,
angularSpeed=ANGULAR_SPEED)
self.robot.toDisplay = '\/'
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
happyTime = self.robot.time
continue # TODO maybe replace by return -> give up
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
pose = self.robot.localisation.pose()
# isThere = math.hypot(pose[0] - lastX, pose[1] - lastY) <= TOLERATED_MISS
# if isThere or pathFinder.newData:
if pathFinder.newData:
pathFinder.newData = False
break
self.robot.removeExtension("VHF")
self.robot.removeExtension("PATH")
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 updateExtension( self, robot, id, data ):
if id == 'laser' and len(data) > 0:
# Is there an obstacle somewhere very close?
lbound = len(robot.laserData) / 6
rbound = len(robot.laserData) * 5 / 6
# (lbound, rbound) should cover the space ahead of the robot.
obstacle_threshold = 300 # [mm]
for x in islice(robot.laserData, lbound, rbound):
if x > 0 and x < obstacle_threshold:
self.line = None
self.newData = True
self.directionAngle = 0.0
if self.verbose:
s = ''
for start in xrange(0, len(robot.laserData), self.step):
too_close = False
for x in islice(robot.laserData, start, start+self.step):
if x > 0 and x < obstacle_threshold:
too_close = True
break
s += '!' if too_close else '-'
print "'" + s[::-1] + "'" # The reverse is needed to match the printed traversability pattern.
self.traversable = None
self.obstacles = []
return
# Preprocess the data, so that the computation is not repeated multiple times.
prepData = [ self.FAR_AWAY if x == 0 else x for x in data ]
backData = [ self.expectation[i] - x for (i, x) in enumerate(prepData) ] # distances measured from the expected ground distance
# Traversability stemming from "there is no obstacle close there".
decimatedPrepData = self.__decimate(prepData, decimator = min)
decimatedBackData = self.__decimate(backData, decimator = max)
trav_obs = self.obstacleTraversability( decimatedBackData )
self.traversable = trav_obs
self.obstacles = []
for i in xrange(len(self.traversable)):
if not self.traversable[i]:
phi = self.laserRight + (self.laserLeft - self.laserRight) * i / (len(self.traversable) - 1)
dist = decimatedPrepData[i] / 1000.0
self.obstacles.append( (phi, dist) )
center = len(self.traversable)/2 + self.offset
if center < 0: center = 0
elif center >= len(self.traversable): center = len(self.traversable) - 1
# Look for a traversable direction. The more in the expected direction, the better.
for i in range(max(center, len(self.traversable) - center)):
if center - i >= 0 and self.traversable[center - i]:
center -= i
break
elif center + i < len(self.traversable) and self.traversable[center + i]:
center += i
break
#If the center is not traversable, we should better STOP!!!
if not self.traversable[center]:
self.line = None
self.newData = True
self.directionAngle = 0.0
if self.verbose:
print '+' * len(self.traversable)
self.traversable = None
self.obstacles = []
return
# Find borders of the traversable gap.
i = 0
while i < center:
if not self.traversable[center - i]:
break
i += 1
right = i
i = 0
while i + center < len(self.traversable):
if not self.traversable[center + i]:
break
i += 1
left = i
# Spit the information.
s = ''
for t in self.traversable:
s += (' ' if t else 'x')
s = "".join( list(s)[:center] + ['C'] + list(s)[center:] )
s = s[::-1]
if self.verbose:
print "'" + s + "'"
# print center, left, right, left+right
# Update the belief.
if left+right <= 10: # TODO limit based on minSize, radius and sample angle
self.offset += (left - right) / 2
self.directionAngle = math.radians( self.offset * self.step / 2.0 )
elif left < 5 or right < 5:
# wide row & collision ahead
print left, right, left+right
if right < left:
self.offset += 5
else:
self.offset -= 5
self.directionAngle = math.radians( self.offset * self.step / 2.0 )
else:
# print "OFF", left, right, left+right
self.directionAngle = 0.0 # if you do not know, go ahead
pose = robot.localisation.pose()
goal = combinedPose( (pose[0], pose[1], pose[2]+self.directionAngle), (1.0, 0, 0) )
self.line = Line( pose, goal )
self.newData = True
if id == 'camera':
if self.verbose:
print data[1]
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):
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 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 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):
if id == 0x80:
# virtual bumper
if math.fabs((robot._rampLastLeft + robot._rampLastRight) /
2) > 0.2 and math.fabs(robot.currentSpeed) < 0.01:
self.virtualBumper += 1
if self.virtualBumper > 40:
print "VIRTUAL BUMPER", (
robot._rampLastLeft +
robot._rampLastRight) / 2, robot.currentSpeed
self.collision = True
else:
self.virtualBumper = 0
if id == 'laser' and len(data) > 0:
# handle 0 = timeout and find min
near = 10.0
for x in islice(data, 270 - 90, 270 + 90):
if x != 0 and x < near * 1000:
near = x / 1000.0
# TODO for 1m, slow down and define alternative route
if near < 0.5:
self.collision = True
step = 10
data2 = [x == 0 and 10000 or x for x in data]
arr = [
min(i) / 1000.0 for i in [
islice(data2, start, start + step)
for start in range(0, len(data2), step)
]
]
arr.reverse()
limit = self.radius
center = len(arr) / 2 + self.offset
center = min(max(center, 0), len(arr) - 1)
i = 0
while i < center:
if arr[center - i] < limit:
break
i += 1
left = i # arr is already reversed
i = 0
while i + center < len(arr):
if arr[center + i] < limit:
break
i += 1
right = i
s = ''
for i in arr:
s += (i < 1.0 and 'x' or ' ')
s = "".join(list(s)[:center] + ['C'] + list(s)[center:])
if self.verbose:
print "'" + s + "'"
if left < 5 or right < 5:
# wide row & collision ahead
if self.verbose:
print left, right, left + right
if left < right:
self.offset += 5
else:
self.offset -= 5
self.directionAngle = math.radians(-self.offset * step / 2.0)
pose = robot.localisation.pose()
goal = combinedPose(
(pose[0], pose[1], pose[2] + self.directionAngle),
(self.radius, 0, 0))
if not self.collision:
self.generator = self.driver.followLineG(Line(pose, goal))
elif left >= 10 or right > 10:
self.offset = 0
if id == 0x186:
assert (len(data))
sonar = (data[1] * 256 + data[0]) * 340.0 / 2.0 / 1000000.0
if sonar < 0.5:
self.collision = True
# if self.verbose:
# print "sonar %.2f" % sonar
if id == 'camera':
if self.verbose:
print data[1]
