diff --git a/src/game_master/scripts/autonomous_controller.py b/src/game_master/scripts/autonomous_controller.py index 1c75a80..a50fe92 100755 --- a/src/game_master/scripts/autonomous_controller.py +++ b/src/game_master/scripts/autonomous_controller.py @@ -50,9 +50,9 @@ class AutonomousController: INTENT_PREPARE_TO_LAND = 8 INTENT_LAND = 9 - SEARCH_HEIGHT = 1.5 + SEARCH_HEIGHT = 1.6 TAKE_OFF_HEIGHT = 1.3 - LANDING_HEIGHT = 1.0 + LANDING_HEIGHT = 0.8 MIN_BATTERY_LEVEL = 15 def __init__(self): @@ -175,7 +175,7 @@ def begin_search_swipe(self, func, height): ''' self.groundSwipeCount = 0 if callable(func): - self.goalTracker.setHeightTarget(height, False, func) + self.goalTracker.setHeightTarget(height, True, func) def take_a_photo(self, name): home = expanduser("~") @@ -187,7 +187,10 @@ def return_to_base(self): if self.intent < self.INTENT_RETURN_TO_BASE: self.intent = self.INTENT_RETURN_TO_BASE print("Looking for gate\n\n") - self.begin_search_swipe(self.look_for_landing_gate, self.SEARCH_HEIGHT) + self.goalTracker.setOrientationTarget(self.visualScale.southWall, False, self.begin_look_for_landing_gate) + + def begin_look_for_landing_gate(self): + self.begin_search_swipe(self.look_for_landing_gate, self.SEARCH_HEIGHT) #----------------------------------------------------------------------- # End Common Functions #----------------------------------------------------------------------- @@ -293,17 +296,31 @@ def look_for_landing_gate(self): self.drone.cameraControl(-15, 0) elif self.groundSwipeCount == 1: self.drone.cameraControl(-30, 0) - elif self.groundSwipeCount == 2: - self.drone.cameraControl(-50, 0) - if self.groundSwipeCount < 3: + if self.groundSwipeCount < 2: self.goalTracker.setSwipeTarget(self.look_for_landing_gate) else: + self.landing_gate_was_missing() + self.groundSwipeCount += 1 + + def landing_gate_was_missing(self): + ''' + Handle special case of landing + ''' + # first cancle all previous targets + self.goalTracker.reset() + self.intent = self.INTENT_RETURNING_TO_BASE + + if self.vision.eastGateDistance is None and self.vision.northGateDistance is None: print("Couldn't find landing gate\n\n") # If gate marker isn't visible it's safe to just land where you are self.drone.land() - - self.groundSwipeCount += 1 + elif self.vision.eastGateDistance is None: + print("Navigating to north gate\n\n") + self.goalTracker.setOrientationTarget(self.vision.northGateAngle, False, self.wait_and_navigate_to_north) + else: + print("Navigating to east gate\n\n") + self.goalTracker.setOrientationTarget(self.vision.eastGateAngle, False, self.wait_and_navigate_to_east) def wait_and_navigate_to_north(self): self.landingGateTarget = 'north' @@ -618,19 +635,12 @@ def run(self): target.angular.z = self.vision.landingPadOrientation self.target_pub.publish(target) - elif self.intent == self.INTENT_RETURN_TO_BASE and (self.vision.eastGateVisible or self.vision.northGateVisible): + elif self.intent == self.INTENT_RETURN_TO_BASE and (self.vision.eastGateDistance is not None and self.vision.northGateDistance is not None): # first cancle all previous targets self.goalTracker.reset() self.intent = self.INTENT_RETURNING_TO_BASE - if not self.vision.eastGateVisible: - print("Navigating to north gate\n\n") - self.goalTracker.setOrientationTarget(self.vision.northGateAngle, False, self.wait_and_navigate_to_north) - elif not self.vision.northGateVisible: - print("Navigating to east gate\n\n") - self.goalTracker.setOrientationTarget(self.vision.eastGateAngle, False, self.wait_and_navigate_to_east) - # Both gates are visible - elif self.vision.eastFrameDistance > self.vision.northFrameDistance: + if self.vision.eastGateDistance > self.vision.northGateDistance: print("Navigating to north gate\n\n") self.goalTracker.setOrientationTarget(self.vision.northGateAngle, False, self.wait_and_navigate_to_north) else: diff --git a/src/game_master/scripts/target_tracker.py b/src/game_master/scripts/target_tracker.py index cff69b7..862b48f 100644 --- a/src/game_master/scripts/target_tracker.py +++ b/src/game_master/scripts/target_tracker.py @@ -209,7 +209,7 @@ def adjustPointTarget(self): errorVector = self.getPointError(currentPoint, targetPoint) errorMagnitude = np.linalg.norm(errorVector) - #Lets try to set a 150 pixel error max we can make it configureable + #Lets try to set a 100 pixel error max we can make it configureable if errorMagnitude > 100 or self.drone.camera_tilt > -70: if abs(errorVector[0]) > 40: @@ -218,21 +218,21 @@ def adjustPointTarget(self): #self.drone.moveY(moveY) self.drone.turn(moveY) else: # Lets control the angle and forward seperately - if errorVector[1] > 0: + if errorVector[1] > 0 or self.drone.camera_tilt <= -70: signX = errorVector[1]/abs(errorVector[1]) moveX = signX * min([0.06, abs(errorVector[1])]) self.drone.moveX(moveX) - else: - if self.drone.camera_tilt > -70: - if self.lastTiltChanged > 10: - # print(targetPoint) - # print(currentPoint) - # print(errorVector) - # print("Looking further down\n\n") - self.drone.cameraControl(self.drone.camera_tilt - 5, self.drone.camera_pan) - self.lastTiltChanged = 0 - else: - self.lastTiltChanged += 1 + elif self.drone.camera_tilt > -70: + if self.lastTiltChanged > 10: + # print(targetPoint) + # print(currentPoint) + # print(errorVector) + # print("Looking further down\n\n") + self.drone.cameraControl(self.drone.camera_tilt - 5, self.drone.camera_pan) + self.lastTiltChanged = 0 + else: + self.lastTiltChanged += 1 + else: if not self.pointAchived: @@ -259,7 +259,7 @@ def adjustCentreLock(self): errorVector = self.getPointError(currentPoint, targetPoint) errorMagnitude = np.linalg.norm(errorVector) - #Lets try to set a 150 pixel error max we can make it configureable + #Lets try to set a 100 pixel error max we can make it configureable if errorMagnitude > 100: if abs(errorVector[0]) > 40: signY = (errorVector[0]/abs(errorVector[0])) diff --git a/src/game_master/scripts/vision.py b/src/game_master/scripts/vision.py index 24251ad..495f859 100644 --- a/src/game_master/scripts/vision.py +++ b/src/game_master/scripts/vision.py @@ -71,7 +71,7 @@ def __init__(self, drone, vfov=45, hfov=80, expectedDistance= 6.50, northToSouth self.bearFrameDistance = None # New frame flag self.newFrameProcessed = False - self.processLine = False + self.processLine = True self.processMarker = False self.processBear = False