def BringMeCenterRotate(self): """ A test method that only rotates the drone """ # Reset flags self.stopTask = False self.wasTagCentered = False self.findTagController.setPoint(0) while not self.stopTask and ( self.recentNavdata.tags_xc[self.GetFrontTagIndex(self.recentNavdata)] > 480 or self.recentNavdata.tags_xc[self.GetFrontTagIndex(self.recentNavdata)] < 520 ): # Create a copy of the current navdata, so it doesn't get refreshed while we work on it workingNavdata = self.recentNavdata if workingNavdata.tags_count == 0: # No tag visible # We want the drone to turn in one place steeringCommand = matrix33( 0.0, 0.0, 0.0, self.constants.FIND_TAG_TURN_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0 ) else: # Receive the necessary actions steeringCommand = self.TellMeFindTag(workingNavdata) self.ExecuteCommand(steeringCommand) print "Done"
def keyReleaseEvent(self,event): key = event.key() steering_matrix = matrix33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # If we have constructed the drone and the key is not generated from an auto-repeating key if not event.isAutoRepeat(): # Note that we don't handle the release of emergency/takeoff/landing keys here, there is no need. # Now we handle moving, notice that this section is the opposite (-=) of the keypress section if key == KeyMapping.YawLeft: self.yaw_velocity -= 1 elif key == KeyMapping.YawRight: self.yaw_velocity -= -1 elif key == KeyMapping.PitchForward: self.pitch -= 1 elif key == KeyMapping.PitchBackward: self.pitch -= -1 elif key == KeyMapping.RollLeft: self.roll -= 1 elif key == KeyMapping.RollRight: self.roll -= -1 elif key == KeyMapping.IncreaseAltitude: self.z_velocity -= 1 elif key == KeyMapping.DecreaseAltitude: self.z_velocity -= -1 # finally we set the command to be sent. The controller handles sending this at regular intervals steering_matrix.m11 = self.pitch steering_matrix.m12 = self.roll steering_matrix.m13 = self.z_velocity steering_matrix.m21 = self.yaw_velocity pub_steering.publish(steering_matrix)
def __init__(self): # How long the drone should carry out the action # Unit is Hz self.COMMAND_PUBLISH_RATE = 5 # The velocity with which the drone turns, between 0 and 1, 1 is # what a keyboard press sends self.FIND_TAG_TURN_VELOCITY = 0.2 # The command to approach the tag is sent for this duration # Unit is Hz, so it moves for only 1/10th of a second self.TAG_APPROACH_RATE = 2 # The size of the angle the drone will make sideways, between 0 and 1, 1 is # what a keyboard press sends self.TAG_CENTER_VELOCITY = 0.15 # The velocity with which the drone approaches a tag, between 0 and 1, # 1 is equivalent to a keypress self.TAG_APPROACH_VELOCITY = 0.05 # A matrix with an empty command, this is sent whenever the drone needs to stop # moving self.STOP_MOVING = matrix33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # The code for the 3 striped tag: self.frontTagType = 0 # The code for the oriented A4 tag: self.bottomTagType = 131072 # The distance, up to which the tag should be approached, measured in cm self.desiredDistance = 100.0 # The distance, from which point on the controller will regulate the approach speed, measured in cm self.controllerDistance = 250.0 # The distance, from which on the sidewards movements to center the tag will be reduced linearly # (See LandingNavigator.CenterTag()) self.reduceDistance = 250.0 # A factor to further reduce sidewards movements #self.reduceFactor = 0.5 # How fast the drone should move laterally to center the bottom tag in the x direction self.CENTER_BOTTOM_X_VELOCITY = 0.02 # How fast the drone should move laterally to center the bottom tag in the y direction self.CENTER_BOTTOM_Y_VELOCITY = 0.02 # How fast the drone should turn on the spot to orient itself properly over the bottom tag self.ALIGN_BOTTOM_TAG_VELOCITY = 0.1
def AlignBottomTag(self, navdata): """ Returns the command to orient the drone in a 180° angle over the bottom tag """ tagIndex = self.GetBottomTagIndex(navdata) controller_input = navdata.tags_orientation[tagIndex] - 180.0 / 360.0 controller_output = self.alignBottomController.update(controller_input) controller_output = self.alignBottomController.avoid_drastic_corrections(controller_output) return matrix33( 0.0, 0.0, 0.0, controller_output * self.constants.ALIGN_BOTTOM_TAG_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0 )
def TellMeFindTag(self, navdata): """ Returns a matrix33 with appropriate commands to center the front tag in the field of view, where those commands will turn the drone on the spot """ tagIndex = self.GetFrontTagIndex(navdata) # We feed the controller with values between -1 and 1, and we receive factors, with which we multiply the speed at which the # drone is turned controller_input = (navdata.tags_xc[tagIndex] - 500.0) / 500.0 controller_output = self.findTagController.update(controller_input) # Sometimes the controller might want to do some drastic actions, which we want to avoid controller_output = self.findTagController.avoid_drastic_corrections(controller_output) return matrix33( 0.0, 0.0, 0.0, controller_output * self.constants.FIND_TAG_TURN_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0 )
def keyPressEvent(self, event): key = event.key() steering_matrix = matrix33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # If we have constructed the drone controller and the key is not generated from an auto-repeating key if not event.isAutoRepeat(): # Handle the important cases first! if key == KeyMapping.Emergency: steering_matrix.m33 = 1.0 elif key == KeyMapping.Takeoff: steering_matrix.m31 = 1.0 elif key == KeyMapping.Land: steering_matrix.m32 = 1.0 else: # Now we handle moving, notice that this section is the opposite (+=) of the keyrelease section if key == KeyMapping.YawLeft: self.yaw_velocity += 1 elif key == KeyMapping.YawRight: self.yaw_velocity += -1 elif key == KeyMapping.PitchForward: self.pitch += 1 elif key == KeyMapping.PitchBackward: self.pitch += -1 elif key == KeyMapping.RollLeft: self.roll += 1 elif key == KeyMapping.RollRight: self.roll += -1 elif key == KeyMapping.IncreaseAltitude: self.z_velocity += 1 elif key == KeyMapping.DecreaseAltitude: self.z_velocity += -1 # finally we set the command to be sent. The controller handles sending this at regular intervals steering_matrix.m11 = self.pitch steering_matrix.m12 = self.roll steering_matrix.m13 = self.z_velocity steering_matrix.m21 = self.yaw_velocity pub_steering.publish(steering_matrix)
def TellMeApproach(self, navdata): """ Returns a matrix33 whith commands to approach the tag up to a certain distance """ tagIndex = self.GetFrontTagIndex(navdata) # We want the drone to stop at a certain point in front of the tag # Also, we want it to go full speed up to controllerDistance away from that point, followed by a # distance, where the controller handles the speed (The controller isn't given the actual distance but a float value representing how # far away we are. It then returns a factor, with which we multiply our TAG_APPROACH_VELOCITY) controller_input = ( navdata.tags_distance[tagIndex] - self.constants.desiredDistance ) / self.constants.controllerDistance controller_output = self.approachFrontController.update(controller_input) # The output value needs to be inverted, avoid drastic controller outputs controller_output = -self.approachFrontController.avoid_drastic_corrections(controller_output) return matrix33( controller_output * self.constants.TAG_APPROACH_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 )
def TellMeCenter(self, navdata): """ Returns a matrix33 with appropriate commands to center the front tag in the field of view, where those commands will move the drone laterally """ tagIndex = self.GetFrontTagIndex(navdata) controller_input = (navdata.tags_xc[tagIndex] - 500.0) / 500.0 controller_output = self.centerFrontController.update(controller_input) controller_output = self.centerFrontController.avoid_drastic_corrections(controller_output) # Thing is, the corrections done by the controller will have a pretty huge impact once we are # close to the tag, therefore we reduce those corrections depending on the distance from the tag # reducingFactor = self.constants.reduceFactor * navdata.tags_distance[tagIndex] / self.constants.reduceDistance reducingFactor = navdata.tags_distance[tagIndex] / self.constants.reduceDistance if reducingFactor > 1: reducingFactor = 1 sidewardsMovement = reducingFactor * controller_output * self.constants.TAG_CENTER_VELOCITY return matrix33(0.0, sidewardsMovement, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def LandingPreparations(self, navdata): """ Returns the appropriate commands to center the bottom tag. Once it is centered, it returns the command to land """ tagIndex = self.GetBottomTagIndex(navdata) tagXPosition = navdata.tags_xc[tagIndex] tagYPosition = navdata.tags_yc[tagIndex] controller_input_y = (tagYPosition - 500.0) / 500.0 controller_input_x = (tagXPosition - 500.0) / 500.0 if not (tagYPosition > 400 and tagYPosition < 600) or not (tagXPosition > 400 and tagXPosition < 600): # We aren't centered, now we determine in which direction we are more off, so we can deal with that one first if (controller_input_y * controller_input_y) > (controller_input_x * controller_input_x): controller_output_y = self.centerBottomYController.update(controller_input_y) # controller_output_y = self.centerBottomYController.avoid_drastic_corrections(controller_output_y) print "Y" print controller_input_y print controller_output_y return matrix33( controller_output_y * self.constants.CENTER_BOTTOM_Y_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ) else: controller_output_x = self.centerBottomXController.update(controller_input_x) # controller_output_x = self.centerBottomXController.avoid_drastic_corrections(controller_output_x) print "X" print controller_input_x print controller_output_x return matrix33( 0.0, controller_output_x * self.constants.CENTER_BOTTOM_X_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ) else: print "READY" # return self.constants.STOP_MOVING # Tag is centered in both directions, we are done, we can land! self.success = True return matrix33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0)
def TellMeWhatToDo(self, navdata): """ Gets the current navdata and returns a matrix33 with appropriate steering commands Automatically decides which commands to send, based on the available tags """ # We got 3 cases: No tag visible, front tag visble, bottom tag visible # Check where the tags are in the arrays in the navdata (You can check which information the navdata contains from the commandline with "rosmsg show ardrone/Navdata") bottomTagIndex = self.GetBottomTagIndex(navdata) frontTagIndex = self.GetFrontTagIndex(navdata) if navdata.tags_count == 0: # No tag visible # We want the drone to turn in one place print "Searching" self.searchCounter += 1 steeringCommand = matrix33(0.0, 0.0, 0.0, self.constants.FIND_TAG_TURN_VELOCITY, 0.0, 0.0, 0.0, 0.0, 0.0) return steeringCommand # return self.constants.STOP_MOVING elif bottomTagIndex > -1: # Sweet, the bottom tag is visible! print "Centering bottom tag" self.bottomCounter += 1 # We need to check if we are turned the right way above the tag currentAngle = navdata.tags_orientation[bottomTagIndex] # Actually this step doesn't seem to be necessary so we skip it # It should have led to a bigger precision, but mostly the drone is oriented the right way return self.LandingPreparations(navdata) # if currentAngle > 170.0 and currentAngle < 190.0: # # Ok, so we are turned correctly, we need to center the bottom tag now. If it is centered # # the command to land will be returned # return self.LandingPreparations(navdata) # else: # # We need to turn the drone on the spot # return self.AlignBottomTag(navdata) else: # Only the front tag is visble # Now we got 2 cases: # - We are still in the spot we started and we are trying to center it (Meaning we are turning on the spot) # - We are already on our way towards that tag # The x position is represented in values between 0 - 1000 from left to right x = navdata.tags_xc[frontTagIndex] if x > 450 and x < 550: # It is pretty centered, set the flag and start approaching self.wasTagCentered = True print "Approaching" self.approachCounter += 1 return self.TellMeApproach(navdata) else: # Check if we already centered it once if self.wasTagCentered: # We did? Ok, then lets move laterally print "Centering laterally" self.centerCounter += 1 return self.TellMeCenter(navdata) else: # The tag hasn't been centered yet, turn on the spot print "Turning" self.rotateCounter += 1 return self.TellMeFindTag(navdata)
def BringMeHome(self): """ Core function to land the drone. It will turn the drone till it finds a 3-colored tag witht the front camera, approach this tag until it sees a A4-tag on the bottom and will land the drone on this tag """ # This is the big loop for all the functions. It resets all flags and periodically asks for instructions to be sent to the drone. if self.recentNavdata == None: print "No navdata available, exiting" return # Reset flags self.stopTask = False self.wasTagCentered = False self.success = False # Reset the controllers self.findTagController.setPoint(0) self.centerFrontController.setPoint(0) self.approachFrontController.setPoint(0) self.centerBottomXController.setPoint(0) self.centerBottomYController.setPoint(0) self.alignBottomController.setPoint(0) # Some counters to evaluate how we are doing self.searchCounter = 0 self.rotateCounter = 0 self.approachCounter = 0 self.centerCounter = 0 self.bottomCounter = 0 # Get the drone up to a good height: workingNavdata = self.recentNavdata while workingNavdata.altd < 1450: if self.stopTask: # In case of keyboard interruption break workingNavdata = self.recentNavdata self.ExecuteCommand(matrix33(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)) print "Gaining Altitude" # Now we are at a proper height and we can start finding & approaching the tag while not self.stopTask and not self.success: # Create a copy of the current navdata, so it doesn't get refreshed while we work on it workingNavdata = self.recentNavdata # Receive the necessary actions steeringCommand = self.TellMeWhatToDo(workingNavdata) # Send them to the drone self.ExecuteCommand(steeringCommand) # In case we got stopped via the keyboard, we want to make sure that our last command is to stop the drone from # doing anything steeringCommand = self.constants.STOP_MOVING self.ExecuteCommand(steeringCommand) print "Steps searching: %i" % self.searchCounter print "Steps rotating: %i" % self.rotateCounter print "Steps approaching: %i" % self.approachCounter print "Steps centering: %i" % self.centerCounter print "Steps centering bottom tag %i " % self.bottomCounter