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