def move(plane, communicator, method): timer = 0 stop = False # Todo: Make an option for centralized/decentralized collision avoidance & movement # Move the plan in a straight line to the direction of the target waypoint while not stop and not plane.dead: # Simulate time by delaying planes update. Will need this for GUI. if defaultValues.SIMULATE_TIME: time.sleep(defaultValues.DELAY) timer += defaultValues.DELAY # find distance to target location clat = plane.cLoc.latitude clon = plane.cLoc.longitude calt = plane.cLoc.altitude tlat = plane.tLoc.latitude tlon = plane.tLoc.longitude talt = plane.tLoc.altitude # Todo: with each adjustment, calculate a distance travled plane.cBearing = plane.tBearing plane.cElevation = plane.tElevation if plane.tdistance <= 20: pass straightLine.straightline(plane) uav_positions = communicator.read(plane) for elem in uav_positions: distance = standardFuncs.totalDistance(plane.cLoc, elem["cLoc"]) if (distance < defaultValues.CRASH_DISTANCE and elem["ID"] != plane.id and elem["dead"] == False): # time.sleep(random.uniform(0, .001)) # Just so that the console doesn't get screwed up plane.dead = True plane.killedBy = elem["ID"] stop = True elif elem["ID"] == plane.id and elem["dead"] == True: plane.dead = True plane.killedBy = elem["killedBy"] stop = True # Move through queue to next waypoint, or if done stop thread. if (plane.tdistance < defaultValues.WAYPOINT_DISTANCE) and (plane.wpAchieved <= plane.numWayPoints): plane.wpAchieved += 1 if plane.wpAchieved < plane.numWayPoints: plane.nextwp() if plane.wpAchieved >= plane.numWayPoints: stop = True communicator.update(plane) # Todo: make this pretty if plane.dead: print "\r%-80s" % "UAV #%3i has crashed!!" % plane.id if plane.wpAchieved == plane.numWayPoints: print "\r%-80s" % "UAV #%3i reached all waypoints." % plane.id
def generate_planes( numPlanes, numWayPoints, gridSize, communicator, location=OUR_LOCATION, ): plane = [] # Create list of planes # Creates a set number of planes for i in range(0, numPlanes): plane.append(Plane()) plane[i].numWayPoints = numWayPoints logging.info("UAV #%3i generated." % plane[i].id) for j in range(0, plane[i].numWayPoints + 2): # +2 to git inital and previous location waypoint = randomLocation(gridSize, location) plane[i].wayPoints.append(waypoint) plane[i].queue.put(plane[i].wayPoints[j]) logging.info("UAV #%3i generated waypoint #%i at: %s" % (plane[i].id, (j + 1), waypoint)) plane[i].set_cLoc(plane[i].queue.get_nowait() ) # Set current location to first generated waypoint plane[i].set_cLoc(plane[i].queue.get_nowait()) plane[i].nextwp() # and removes it from the queue # Calculate current and target bearing (both set to equal initially) plane[i].tBearing = standardFuncs.find_bearing(plane[i].cLoc, plane[i].tLoc) plane[i].cBearing = plane[i].tBearing # Calculate current and target elevation angles (also equa) plane[i].tElevation = standardFuncs.elevation_angle( plane[i].cLoc, plane[i].tLoc) plane[i].cElevation = plane[i].tElevation # Calculate the three dimensional and two dimensional distance to target plane[i].distance = standardFuncs.findDistance(plane[i].cLoc, plane[i].tLoc) plane[i].tdistance = standardFuncs.totalDistance( plane[i].cLoc, plane[i].tLoc) if IS_TEST: print "Plane ID is", plane[i].id, "and has", plane[ i].numWayPoints, "waypoints" print plane[i].wayPoints communicator.startUp(plane[i]) return plane
def updateTelemetry(self, newLoc): # Set the current location self.pLoc = self.cLoc # Move current location to previous location self.cLoc = newLoc # Set new current location # Calculate new elevation self.tBearing = standardFuncs.find_bearing(self.pLoc, self.tLoc) self.tElevation = standardFuncs.elevation_angle(self.pLoc, self.tLoc) # haversine's horizontal distance self.distance = standardFuncs.findDistance(newLoc, self.tLoc) # haversine's horizontal distance w/ vertical distance taken into account self.tdistance = standardFuncs.totalDistance(newLoc, self.tLoc) self.distanceTraveled += self.speed * self.args.DELAY
def straightline (plane): if plane.avoid: target = plane.avoidanceWaypoint logging.info ("UAV #3i is moving toward an avoidance waypoint" % plane.id) else: target = plane.tLoc speed = plane.speed # Get speed from plane distanceTraveled = plane.speed * defaultValues.DELAY #Get frequency of updates. plane.distanceTraveled += distanceTraveled #The total distance travelled. # Calculate new position position = vMath.vector(distanceTraveled, plane.cBearing, plane.cElevation) new_lat = plane.cLoc.latitude + (position.x / standardFuncs.LATITUDE_TO_METERS) new_lon = plane.cLoc.longitude + (position.y / standardFuncs.LONGITUDE_TO_METERS) new_alt = plane.cLoc.altitude + position.z newLoc = standardFuncs.loc(new_lat,new_lon,new_alt) # Update current location, distance, total distance, target bearing, newloc = standardFuncs.loc(new_lat,new_lon,new_alt) plane.set_cLoc(newloc) # Calculate new bearing plane.cBearing = standardFuncs.find_bearing(plane.pLoc, plane.cLoc) plane.cElevation = standardFuncs.elevation_angle(plane.pLoc, plane.cLoc) # Calculate new elevation plane.tBearing = standardFuncs.find_bearing(newLoc, plane.tLoc) plane.tElevation = standardFuncs.elevation_angle(newLoc,plane.tLoc) # haversine's horizontal distance plane.distance = standardFuncs.findDistance(newLoc, plane.tLoc) # haversine's horizontal distance w/ vertical distance taken into account plane.tdistance = standardFuncs.totalDistance(newLoc, plane.tLoc)
def generate_planes(numPlanes, numWayPoints, gridSize, communicator, location=OUR_LOCATION, ): plane = [] # Create list of planes # Creates a set number of planes for i in range(0, numPlanes): plane.append(Plane()) plane[i].numWayPoints = numWayPoints logging.info("UAV #%3i generated." % plane[i].id) for j in range(0, plane[i].numWayPoints + 2): # +2 to git inital and previous location waypoint = randomLocation(gridSize, location) plane[i].wayPoints.append(waypoint) plane[i].queue.put(plane[i].wayPoints[j]) logging.info("UAV #%3i generated waypoint #%i at: %s" % (plane[i].id, (j + 1), waypoint)) plane[i].set_cLoc(plane[i].queue.get_nowait()) # Set current location to first generated waypoint plane[i].set_cLoc(plane[i].queue.get_nowait()) plane[i].nextwp() # and removes it from the queue # Calculate current and target bearing (both set to equal initially) plane[i].tBearing = standardFuncs.find_bearing(plane[i].cLoc, plane[i].tLoc) plane[i].cBearing = plane[i].tBearing # Calculate current and target elevation angles (also equa) plane[i].tElevation = standardFuncs.elevation_angle(plane[i].cLoc, plane[i].tLoc) plane[i].cElevation = plane[i].tElevation # Calculate the three dimensional and two dimensional distance to target plane[i].distance = standardFuncs.findDistance(plane[i].cLoc, plane[i].tLoc) plane[i].tdistance = standardFuncs.totalDistance(plane[i].cLoc, plane[i].tLoc) if IS_TEST: print "Plane ID is", plane[i].id, "and has", plane[i].numWayPoints, "waypoints" print plane[i].wayPoints communicator.startUp(plane[i]) return plane
def move(plane, communicator, method): timer = 0 stop = False # Todo: Make an option for centralized/decentralized collision avoidance & movement # Move the plan in a straight line to the direction of the target waypoint while not stop and not plane.dead: # Simulate time by delaying planes update. Will need this for GUI. if defaultValues.SIMULATE_TIME: time.sleep(defaultValues.DELAY) timer += defaultValues.DELAY # find distance to target location clat = plane.cLoc.latitude clon = plane.cLoc.longitude calt = plane.cLoc.altitude tlat = plane.tLoc.latitude tlon = plane.tLoc.longitude talt = plane.tLoc.altitude # Todo: with each adjustment, calculate a distance travled plane.cBearing = plane.tBearing plane.cElevation = plane.tElevation if plane.tdistance <= 20: pass straightLine.straightline(plane) uav_positions = communicator.read(plane) for elem in uav_positions: distance = standardFuncs.totalDistance(plane.cLoc, elem["cLoc"]) if (distance < defaultValues.CRASH_DISTANCE and elem["ID"] != plane.id and elem["dead"] == False): # time.sleep(random.uniform(0, .001)) # Just so that the console doesn't get screwed up plane.dead = True plane.killedBy = elem["ID"] stop = True elif elem["ID"] == plane.id and elem["dead"] == True: plane.dead = True plane.killedBy = elem["killedBy"] stop = True # Move through queue to next waypoint, or if done stop thread. if (plane.tdistance < defaultValues.WAYPOINT_DISTANCE) and ( plane.wpAchieved <= plane.numWayPoints): plane.wpAchieved += 1 if plane.wpAchieved < plane.numWayPoints: plane.nextwp() if plane.wpAchieved >= plane.numWayPoints: stop = True communicator.update(plane) # Todo: make this pretty if plane.dead: print "\r%-80s" % "UAV #%3i has crashed!!" % plane.id if plane.wpAchieved == plane.numWayPoints: print "\r%-80s" % "UAV #%3i reached all waypoints." % plane.id