def waypoints2dirDist(waypoints):
"convert GPS waypoints to desired heading and distance"
conv = Convertor(waypoints[0])
ret = []
for start, goal in zip(waypoints[:-1], waypoints[1:]):
ret.append((angleTo(conv.geo2planar(start), conv.geo2planar(goal)),
distance(conv.geo2planar(start), conv.geo2planar(goal))))
return ret
def rayTraceSingleLine( pose, wallStart, wallEnd, maxRadius ):
a = wallStart[1] - wallEnd[1]
b = wallEnd[0] - wallStart[0]
c = - a * wallStart[0] - b * wallStart[1]
frac = a * math.cos( pose[2] ) + b * math.sin( pose[2] )
if math.fabs( frac ) < 0.0001:
return maxRadius
t = -(a * pose[0] + b * pose[1] + c)/frac;
if t < 0:
return maxRadius
# check that crossing belongs to the wall and is not outside
crossing = ( pose[0] + t * math.cos( pose[2] ), pose[1] + t * math.sin( pose[2] ) )
if distance( crossing, wallStart ) + distance( crossing, wallEnd ) > distance( wallStart, wallEnd ) + 0.01:
return maxRadius
return t < maxRadius and t or maxRadius
def waypoints2dirDist( waypoints ):
"convert GPS waypoints to desired heading and distance"
conv = Convertor( waypoints[0] )
ret = []
for start,goal in zip(waypoints[:-1],waypoints[1:]):
ret.append( (
angleTo( conv.geo2planar(start), conv.geo2planar(goal) ),
distance( conv.geo2planar(start), conv.geo2planar(goal) )
) )
return ret
def rayTraceSingleLine(pose, wallStart, wallEnd, maxRadius):
a = wallStart[1] - wallEnd[1]
b = wallEnd[0] - wallStart[0]
c = -a * wallStart[0] - b * wallStart[1]
frac = a * math.cos(pose[2]) + b * math.sin(pose[2])
if math.fabs(frac) < 0.0001:
return maxRadius
t = -(a * pose[0] + b * pose[1] + c) / frac
if t < 0:
return maxRadius
# check that crossing belongs to the wall and is not outside
crossing = (pose[0] + t * math.cos(pose[2]),
pose[1] + t * math.sin(pose[2]))
if distance(crossing, wallStart) + distance(crossing, wallEnd) > distance(
wallStart, wallEnd) + 0.01:
return maxRadius
return t < maxRadius and t or maxRadius
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 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 ver0(metalog, waypoints=None):
assert metalog is not None
assert waypoints is not None # for simplicity (first is start)
conv = Convertor(refPoint = waypoints[0])
waypoints = waypoints[1:-1] # remove start/finish
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
# mount_sensor(GPS, robot, metalog)
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# mount_sensor(VelodyneThread, robot, metalog)
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30*60: # RO timelimit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, robot.velodyne_data
prev_gps = robot.gps_data
if robot.gps_data is not None:
dist_arr = [distance( conv.geo2planar((robot.gps_data[1], robot.gps_data[0])),
conv.geo2planar(destination) ) for destination in waypoints]
dist = min(dist_arr)
print "DIST-GPS", dist
if prev_destination_dist is not None:
if prev_destination_dist < dist and dist < 10.0:
robot.drop_ball = True
# remove nearest
i = dist_arr.index(dist) # ugly, but ...
print "INDEX", i
del waypoints[i]
center(robot)
moving = False
robot.wait(1.0)
robot.drop_ball = False
robot.wait(3.0)
dist = None
prev_destination_dist = dist
dist = None
if robot.velodyne_data is not None:
index, dist = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!! -", robot.velodyne_data
center(robot)
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num/2]), min(arr[num/2:])
print "DECIDE", left, right, robot.velodyne_data
if left <= right:
robot.pulse_right(RIGHT_TURN_TIME)
robot.steering_angle = math.radians(-30) # TODO replace by autodetect
else:
robot.pulse_left(LEFT_TURN_TIME)
robot.steering_angle = math.radians(30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
if robot.steering_angle < 0:
robot.pulse_left(LEFT_TURN_TIME)
else:
robot.pulse_right(RIGHT_TURN_TIME)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", robot.velodyne_data
go(robot)
moving = True
if not robot.buttonGo:
print "STOP!"
break
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
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 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
