def qr_tracking(droneVision:DroneVisionGUI, bebop:Bebop): cv2.namedWindow('qr') while cv2.getWindowProperty('qr', 0) >= 0: img = droneVision.get_latest_valid_picture() x,y,w,h = None,None,None,None try: rect = zbar.decode(img, symbols=[zbar.ZBarSymbol.QRCODE])[0][2] poly = zbar.decode(img, symbols=[zbar.ZBarSymbol.QRCODE])[0][3] x,y,w,h = rect p1,p2,p3,p4 = poly except IndexError: pass if x is not None: # cv2.rectangle(img,(x,y),(x+w,y+h),(0,0,255)) pts = np.array([[p1[0],p1[1]], [p2[0],p2[1]], [p3[0],p3[1]], [p4[0],p4[1]]], np.int32) pts.reshape(-1,1,2) cv2.polylines(img, [pts], True, (255, 0, 255), 5) area_t1 = abs((p1[0]*(p2[1]-p4[1])+p2[0]*(p4[1]-p1[1])+p4[0]*(p1[1]-p2[1]))/2.0) area_t2 = abs((p3[0] * (p2[1] - p4[1]) + p2[0] * (p4[1] - p3[1]) + p4[0] * (p3[1] - p2[1])) / 2.0) area = area_t2+area_t1 backup_threshold = 20000 fwd_threshold = 10000 if w is not None and area > backup_threshold: print("GOING BACK") bebop.fly_direct(roll=0, pitch=-20, yaw=0, vertical_movement=0, duration=0.07) elif w is not None and area < fwd_threshold: print("GOING FORWARD") bebop.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=0.07) if x is not None and x + (w / 2.0) > 550: print("GOING RIGHT") bebop.fly_direct(roll=0, pitch=0, yaw=100, vertical_movement=0, duration=0.1) elif x is not None and x + (w / 2.0) < 300: print("GOING LEFT") bebop.fly_direct(roll=0, pitch=0, yaw=-100, vertical_movement=0, duration=0.1) if x is not None and y + (h / 2.0) > 380: print("GOING DOWN") bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-40, duration=0.1) elif x is not None and y + (h / 2.0) < 100: print("GOING UP") bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=40, duration=0.1) cv2.imshow('qr', img) cv2.waitKey(10) bebop.safe_land(10) cv2.destroyAllWindows()
class ARDrone(Drone): def __init__(self): super().__init__() self.bebop = Bebop() print("connecting to bebop drone") self.connection.emit("progress") self.success = self.bebop.connect(5) if self.success: self.connection.emit("on") self.bebop.set_max_altitude(20) self.bebop.set_max_distance(20) self.bebop.set_max_rotation_speed(180) self.bebop.set_max_vertical_speed(2) self.bebop.enable_geofence(1) self.bebop.set_hull_protection(1) # todo: battery signal to emit (look in sensors) #TODO test this piece of code self.bebop.set_user_sensor_callback(print, self.bebop.sensors.battery) else: print("refresh....") self.connection.emit("off") def take_off(self): self.bebop.safe_takeoff(5) def land(self): self.bebop.safe_land(5) def stop(self): self.bebop.disconnect() def fly_direct(self, roll, pitch, yaw, vertical_movement): my_roll = self.bebop._ensure_fly_command_in_range(roll) my_pitch = self.bebop_ensure_fly_command_in_range(pitch) my_yaw = self.bebop_ensure_fly_command_in_range(yaw) my_vertical = self.bebop_ensure_fly_command_in_range(vertical_movement) command_tuple = self.bebop.command_parser.get_command_tuple( "ardrone3", "Piloting", "PCMD") self.bebop.drone_connection.send_single_pcmd_command( command_tuple, my_roll, my_pitch, my_yaw, my_vertical) def process_motion(self, _up, _rotate, _front, _right): velocity_up = _up * self.max_vert_speed velocity_yaw = _rotate * self.max_rotation_speed velocity_pitch = _front * self.max_horiz_speed velocity_roll = _right * self.max_horiz_speed #print("PRE", velocity_roll, velocity_pitch, velocity_up, velocity_yaw) self.fly_direct(velocity_roll, velocity_pitch, velocity_yaw, velocity_up)
def face_tracking(droneVision:DroneVisionGUI,bebop:Bebop): face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml') cv2.namedWindow("face_tracking") frame = droneVision.get_latest_valid_picture() while cv2.getWindowProperty('face_tracking', 0) >= 0: frame = droneVision.get_latest_valid_picture() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, 1.3, 5) (x,y,w,h) = None, None, None, None if len(faces) > 0: (x,y,w,h) = faces[0] cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 255), 1) print("Face Size: "+ str(w*h)) backup_threshold = 3600 fwd_threshold = 2000 if w is not None and w*h > backup_threshold: print("GOING BACK") bebop.fly_direct(roll=0,pitch=-20,yaw=0,vertical_movement=0,duration=0.1) elif w is not None and w*h < fwd_threshold: print("GOING FORWARD") bebop.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=0.1) if x is not None and x+(w/2.0) > 650: print("GOING RIGHT") bebop.fly_direct(roll=0,pitch=0,yaw=70,vertical_movement=0,duration=0.1) elif x is not None and x+(w/2.0) < 200: print("GOING LEFT") bebop.fly_direct(roll=0, pitch=0, yaw=-70, vertical_movement=0, duration=0.1) cv2.imshow("face_tracking",frame) cv2.waitKey(10) bebop.safe_land(10) cv2.destroyAllWindows()
print("move conclockwise") bebop.fly_direct(roll=0, pitch=0, yaw=-50, vertical_movement=0, duration=0.1) c.send(("controller received " + ch).encode('utf-8')) elif ch == "f": print("flip") bebop.flip(direction="front") c.send(("controller received " + ch).encode('utf-8')) elif ch == "l": print("land") bebop.safe_land(10) c.send(("controller received " + ch).encode('utf-8')) elif ch == "q" or len(ch) == 0: # ソケットがSIGPIPEになったときも着地 print("end") bebop.safe_land(10) c.send(("controller received " + ch).encode('utf-8')) c.close() break print("DONE - disconnecting") bebop.smart_sleep(5) print(bebop.sensors.battery) bebop.disconnect() except:
class myoBebop(object): def __init__(self , indoorFly = True): '''类初始化函数''' self.bebop = None self.indoorFlyFlag = indoorFly self.actionType = "rest" #当前的动作类型 self.startSuccessFlag = False #初始化是否成功标志位 self.takeOfFlag = False #是否起飞标志位 self.landFlag = True #是否降落标志位 self.startFlag = False #程序开始标志位 self.executeCommondFlag = True #命令执行成功标志位 #动作映射字典 self.actionMapDict = { "front" : "fist" , "back" : "open_hand" , "left" : "one" , "right" : "two" , "up" : "good", "down" : "low" , "rotateLeft" : "three" , "rotateRight" : "four" , "takeOf": "ok" , "land" : "love"} self.tempCount = 0 #计数 self.excuteFlag1 = True #读取动作类别标志位 self.excuteFlag2 = True #读取按键值的标志位 self.flyType = 0 #飞机飞行类别(0:正常飞行 , 1:特技飞行) self.moveStepTime = 1 #飞行器水平运行步长时间 self.rotateStepTime = 1 #飞行器旋转运行步长时间 def start(self): '''开始函数''' #初始化飞机对象 self.bebop = Bebop() #连接飞机 print("connecting") self.success = self.bebop.connect(10) #与飞机进行连接(最大尝试次数为10次) if self.success == True: print("sleeping") self.bebop.smart_sleep(5) self.bebop.ask_for_state_update() #获取飞机的状态 if self.indoorFlyFlag == True: self.setIndoorFly() print("set indoor fly sucess!!!") print("The aircraft was successfully initialized!") self.startSuccessFlag = True #开始成功 else: print("The connection failed. Please check again!") self.startSuccessFlag = False #开始失败 self.removeExitFile() def bebopFly(self , actionType): '''根据动作类别进行相应的动作''' if actionType == self.actionMapDict["front"]: #向前飞 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll=0, pitch=50, yaw=0, vertical_movement=0, duration=self.moveStepTime) print("fly front") elif self.flyType == 1: self.success = self.bebop.flip(direction="front") print("flip front") print("mambo flip result %s" % self.success) self.bebop.smart_sleep(5) self.executeCommondFlag = True elif actionType == self.actionMapDict["back"]: #向后飞 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll=0, pitch=-50, yaw=0, vertical_movement=0, duration=self.moveStepTime) print("fly back") elif self.flyType == 1: self.success = self.bebop.flip(direction="back") print("flip back") print("mambo flip result %s" % self.success) self.bebop.smart_sleep(5) self.executeCommondFlag = True elif actionType == self.actionMapDict["left"]: #左飞 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll=50, pitch=0, yaw=0, vertical_movement=0, duration=self.moveStepTime) print("fly left") elif self.flyType == 1: self.success = self.bebop.flip(direction="left") print("flip left") print("mambo flip result %s" % self.success) self.bebop.smart_sleep(5) self.executeCommondFlag = True elif actionType == self.actionMapDict["right"]: #右飞 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll=-50, pitch=0, yaw=0, vertical_movement=0, duration=self.moveStepTime) print("fly right") elif self.flyType == 1: self.success = self.bebop.flip(direction="right") print("flip right") print("mambo flip result %s" % self.success) self.bebop.smart_sleep(5) self.executeCommondFlag = True elif actionType == self.actionMapDict["up"]: #上飞 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll = 0, pitch = 0, yaw = 0, vertical_movement=50, duration = self.moveStepTime) print("fly up") elif self.flyType == 1: pass self.executeCommondFlag = True elif actionType == self.actionMapDict["down"]: #下飞 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll = 0, pitch = 0, yaw = 0, vertical_movement= - 50, duration = self.moveStepTime) print("fly down") elif self.flyType == 1: pass self.executeCommondFlag = True elif actionType == self.actionMapDict["rotateLeft"]: #向左旋转 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll=0, pitch=0, yaw=50, vertical_movement=0, duration=self.moveStepTime) print("fly rotateLeft") elif self.flyType == 1: pass self.executeCommondFlag = True elif actionType == self.actionMapDict["rotateRight"]: #向右旋转 self.executeCommondFlag = False print("flying state is %s" % self.bebop.sensors.flying_state) if self.flyType == 0: self.bebop.fly_direct(roll=0, pitch=0, yaw = -50, vertical_movement=0, duration=self.moveStepTime) print("fly rotateRight") elif self.flyType == 1: pass self.executeCommondFlag = True elif actionType == self.actionMapDict["takeOf"]: #起飞 self.startFlag = True if self.landFlag == True: self.landFlag = False self.executeCommondFlag = False print("take of ") self.bebop.safe_takeoff(10) self.bebop.smart_sleep(5) self.executeCommondFlag = True self.takeOfFlag = True elif actionType == self.actionMapDict["land"]: #降落 if self.takeOfFlag == True: self.takeOfFlag = False self.executeCommondFlag = False print("land ") self.bebop.safe_land(10) self.bebop.smart_sleep(5) self.landFlag = True self.executeCommondFlag = True #myo失去连接后,飞机自动着落 def safeAction(self): if self.startFlag == True: if self.flyType == 0: if self.tempCount > 100: self.bebop.safe_land(10) self.bebop.smart_sleep(5) print("DONE - disconnecting") self.bebop.disconnect() self.mThread.forcedStopThread() elif self.flyType == 1: if self.tempCount > 300: self.bebop.safe_land(10) self.bebop.smart_sleep(5) print("DONE - disconnecting") self.bebop.disconnect() self.mThread.forcedStopThread() def setTheActionMain(self): '''设置动作类别线程函数''' if self.excuteFlag1 == True: self.excuteFlag1 = False os.system("python2 myoMain.py") time.sleep(0.01) def removeExitFile(self , fileName = "actionTempData.dat" ): '''删除已经存在的文件''' if os.path.exists(fileName) == True: os.remove(fileName) def setIndoorFly(self): '''设置室内飞行的参数''' self.bebop.set_max_tilt(5) self.bebop.set_max_vertical_speed(1) def getKeyValueMain(self): '''获取按键值的线程函数''' if self.excuteFlag2 == True: self.excuteFlag2 = False os.system("python2 quit.py" ) time.sleep(0.01) #按键退出函数 def quitMain(self): print("quit") self.bebop.safe_land(10) self.bebop.smart_sleep(5) self.bebop.disconnect() self.mThread.forcedStopThread() def getTheActionMain(self): '''得到动作类别线程函数''' global quitFlag while True: self.actionType = getTheCurrentAction() getTheKeyValue() if quitFlag == True: self.quitMain() else: if self.actionType == None: self.tempCount += 1 else: self.tempCount = 0 self.safeAction() if self.executeCommondFlag == True: self.bebopFly(self.actionType) time.sleep(0.01) def run(self): '''运行主程序''' try: self.mThread = myThread() self.mThread.addThread('setTheActionMain' , self.setTheActionMain , 0) self.mThread.addThread('getTheActionMain' , self.getTheActionMain , 0) self.mThread.addThread('getKeyValueMain' , self.getKeyValueMain , 0) self.mThread.runThread() except KeyboardInterrupt: print("DONE") self.bebop.disconnect() self.mThread.forcedStopThread()
bebop.fly_direct(roll=-50, pitch=0, yaw=0, vertical_movement=0, duration=1.5) bebop.smart_sleep(3) #go back bebop.smart_sleep(3) bebop.fly_direct(roll=0, pitch=-50, yaw=0, vertical_movement=0, duration=1.4) bebop.smart_sleep(3) #moving right bebop.fly_direct(roll=50, pitch=0, yaw=0, vertical_movement=0, duration=1.2) bebop.smart_sleep(3) bebop.smart_sleep(3) bebop.flip(direction="front") bebop.smart_sleep(3) print("landing") bebop.safe_land(5) bebop.smart_sleep(5) print("disconnect") bebop.disconnect()
class Drone: def __init__(self): self.drone = Bebop() def take_off(self): ### Connect to Bebop drone and take off ### success = self.drone.connect1(10) print(success) self.drone.safe_takeoff(2) def step(self, roll, pitch, yaw, vertical_movement, duration): ### Moves the Bebop drone given input parameters ### self.drone.fly_direct(self, roll, pitch, yaw, vertical_movement, duration) def square(self): ### Move the drone in a square pattern### #move left and wait 1 second self.drone.fly_direct(self, roll=-15, pitch=0, yaw=0, vertical_movement=0, duration=1) self.drone.smart_sleep(1) #move forward and wait 1 second self.drone.fly_direct(self, roll=0, pitch=15, yaw=0, vertical_movement=0, duration=1) self.drone.smart_sleep(1) #move right and wait 1 second self.drone.fly_direct(self, roll=15, pitch=0, yaw=0, vertical_movement=0, duration=1) self.drone.smart_sleep(1) #move back and wait 1 second self.drone.fly_direct(self, roll=0, pitch=-15, yaw=0, vertical_movement=0, duration=1) self.drone.smart_sleep(1) def zigzag(self): ### Move the drone in a 3D zigzag pattern### #move left-forward-up and wait 1 second self.drone.fly_direct(self, roll=-15, pitch=15, yaw=0, vertical_movement=15, duration=1) self.drone.smart_sleep(1) #move right-forward-down and wait 1 second self.drone.fly_direct(self, roll=15, pitch=15, yaw=0, vertical_movement=-15, duration=1) self.drone.smart_sleep(1) #move left and wait 1 second self.drone.fly_direct(self, roll=-15, pitch=0, yaw=0, vertical_movement=0, duration=1) self.drone.smart_sleep(1) #move right-back-up and wait 1 second self.drone.fly_direct(self, roll=15, pitch=-15, yaw=0, vertical_movement=15, duration=1) self.drone.smart_sleep(1) #move left-back-down and wait 1 second self.drone.fly_direct(self, roll=-15, pitch=-15, yaw=0, vertical_movement=-15, duration=1) self.drone.smart_sleep(1) def disconnect(self): ### Disconect Bebop drone ### self.drone.disconnect() def land(self): ### Disconnect Bebop drone and land ### success = self.drone.connect1(10) print(success) self.drone.ask_for_state_update() self.drone.safe_land(5) self.drone.disconnect()
from pyparrot.Bebop import Bebop import math bebop = Bebop() print("connecting") success = bebop.connect(20) print(success) print("sleeping") bebop.smart_sleep(5) bebop.ask_for_state_update() bebop.safe_takeoff(1) bebop.smart_sleep(1) bebop.safe_land(1) print("DONE - disconnecting") bebop.disconnect()
class PyParrot(gui.MyFrame1): def __init__(self, parent): gui.MyFrame1.__init__(self, parent) self.statusBar.SetStatusWidths([100, -1]) self.statusBar.SetStatusText('Not Connected') self.lc_commands.InsertColumn(0, 'command', width=300) self.bebop = Bebop() # load saved commands from file self._loadJsonFile() def OnClose( self, event ): if wx.MessageBox("Commands are not yet saved. Do you want to save now?", "Save Changes", wx.ICON_QUESTION | wx.YES_NO) == wx.YES: # Retrive commands from lc_commands cmdList = [] for i in range(self.lc_commands.GetItemCount()): cmdList.append(self.lc_commands.GetItemText(i)) self._saveJsonFile(cmdList) self.OnDisconnect(None) event.Skip() # calls the parent close method def OnConnect( self, event ): isConnected = self.bebop.connect(10) statusText = 'Connected' if not isConnected: statusText = 'Not ' + statusText self.statusBar.SetStatusText(statusText) def OnDisconnect( self, event ): self.bebop.disconnect() self.statusBar.SetStatusText('Disconnected') self.statusBar.SetStatusText(f'Battery: {self.bebop.sensors.battery}%', 1) def OnTakeOff( self, event ): self.bebop.safe_takeoff(10) # By default, setting the status text only sets the first # one. So we must specify the second status text (index 1 # , instead of index 0) self.statusBar.SetStatusText('Taking off', 1) def OnLand( self, event ): self.bebop.safe_land(10) self.statusBar.SetStatusText('Landing', 1) def OnAddTakeOff( self, event ): self._addCommand(f'{CmdType.TakeOff.value},10') def OnAddLand( self, event ): self._addCommand(f'{CmdType.Land.value},10') def OnAddRelative( self, event ): x = int(self.fld_x.GetValue()) y = int(self.fld_y.GetValue()) z = int(self.fld_z.GetValue()) rads = int(self.fld_radians.GetValue()) self._addCommand(f'{CmdType.Relative.value},{x},{y},{z},{rads}') def OnAddDirect( self, event ): roll = int(self.fld_roll.GetValue()) pitch = int(self.fld_pitch.GetValue()) yaw = int(self.fld_yaw.GetValue()) vertical = int(self.fld_vertical.GetValue()) duration = int(self.fld_duration.GetValue()) self._addCommand(f'{CmdType.Direct.value},{roll},{pitch},{yaw},{vertical},{duration}') def OnAddSleep( self, event ): sleep = int(self.fld_sleep.GetValue()) self._addCommand(f'{CmdType.Sleep.value},{sleep}') def OnAddFlip( self, event ): dir = str(self.fld_direction.GetValue()) self._addCommand(f'{CmdType.Flip.value},{dir}') def OnAddFlyGrid(self, event): print("Make sure you take off first!") heightGain = int(self.fld_gridHeight.GetValue()) lengthForward = int(self.fld_gridLength.GetValue()) width = int(self.fld_gridWidth.GetValue()) biLines = int(self.fld_gridLines.GetValue()) widthSection = width/(biLines+1) # Rise self._addCommand(f'{CmdType.Relative.value},0,0,{heightGain},0') # Fly forward and turn right self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly right and turn right self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly back and turn left self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') while (biLines >= 2): biLines -= 2 # Fly right and turn left self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly forward and turn right self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly right and turn right self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly back and turn left self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') if (biLines == 1): biLines -= 1 # Fly right and turn left self._addCommand(f'{CmdType.Relative.value},{widthSection},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly forward self._addCommand(f'{CmdType.Relative.value},{lengthForward},0,0,0') # Flip self._addCommand(f'{"flip"},{"up"}') # Fly back left to home self._addCommand(f'{CmdType.Relative.value},{-lengthForward},{-width},0,0') # Flip self._addCommand(f'{"flip"},{"up"}') # Descend self._addCommand(f'{CmdType.Relative.value},0,0,{-heightGain},0') else: # Fly left and turn left self._addCommand(f'{CmdType.Relative.value},{-width},0,0,-90') # Flip self._addCommand(f'{"flip"},{"up"}') # Descend self._addCommand(f'{CmdType.Relative.value},0,0,{-heightGain},0') print("Make sure you land!") def OnRemove( self, event ): self.lc_commands.DeleteItem(self.lc_commands.GetFocusedItem()) def OnUp( self, event ): """Swap the selected item with the one above it""" index = self.lc_commands.GetFocusedItem() if index >= 1: selItemStr = self.lc_commands.GetItemText(index) aboveItemStr = self.lc_commands.GetItemText(index-1) self.lc_commands.SetItemText(index, aboveItemStr) self.lc_commands.SetItemText(index-1, selItemStr) self.lc_commands.Focus(index-1) def OnDown( self, event ): """Swap the selected item with the one below it""" index = self.lc_commands.GetFocusedItem() if index < self.lc_commands.GetItemCount() - 1: selItemStr = self.lc_commands.GetItemText(index) belowItemStr = self.lc_commands.GetItemText(index+1) self.lc_commands.SetItemText(index, belowItemStr) self.lc_commands.SetItemText(index+1, selItemStr) self.lc_commands.Focus(index+1) def OnClear( self, event ): self.lc_commands.DeleteAllItems() def OnRunFromSelection( self, event ): index = self.lc_commands.GetFocusedItem() while index > 0: self.lc_commands.DeleteItem(index - 1) index = self.lc_commands.GetFocusedItem() self.OnRunCommands(event) def OnRunCommands( self, event ): """Go through each item in lc_commands and convert the string to a list. Then use the first item in the list to determine the command type, and the rest of the items are the params""" # Retrive commands from lc_commands cmdList = [] for i in range(self.lc_commands.GetItemCount()): cmdList.append(self.lc_commands.GetItemText(i)) self._saveJsonFile(cmdList) # Abort running attempt if not connected if not self.bebop.drone_connection.is_connected: print("No Connection. Aborting process.") return # === Run Commands === for i in range(self.lc_commands.GetItemCount()): args = self.lc_commands.GetItemText(i).split(',') self.statusBar.SetStatusText(f'Executing command: {args}', 1) try: if (args[0] == CmdType.Sleep.value): self.bebop.smart_sleep(int(args[1])) elif args[0] == CmdType.TakeOff.value: self.bebop.safe_takeoff(int(args[1])) elif args[0] == CmdType.Land.value: self.bebop.safe_land(int(args[1])) elif args[0] == CmdType.Direct.value: self.bebop.fly_direct(int(args[1]), int(args[2]), int(args[3]), int(args[4]), int(args[5])) elif args[0] == CmdType.Relative.value: self.bebop.move_relative(int(args[1]), int(args[2]), int(args[3]), math.radians(int(args[4]))) elif args[0] == CmdType.Flip.value: self.bebop.flip(str(args[1])) except Exception as e: print("Error occurred: ") print(e) continue def _loadJsonFile(self): # Open up JSON file with last run commands filePath = os.getcwd() + os.sep + "cmd_data.json" if os.path.isfile(filePath): f = open(filePath,"r") s = f.read() commands = json.loads(s) # Input commands into GUI interface for c in commands: self._addCommand(c) def _saveJsonFile(self, cmdList): # Place all commands into JSON file and write them to the disk jsonStr = json.dumps(cmdList) filePath = os.getcwd() + os.sep + "cmd_data.json" with open(filePath,"w") as f: f.write(jsonStr) def _addCommand(self, cmd: str): self.lc_commands.InsertItem(self.lc_commands.GetItemCount(), cmd) self.statusBar.SetStatusText(f'Added command: {cmd}', 1)
def color_tracking(drone_vision:DroneVisionGUI, bebop:Bebop): def show_hist(hist): """Takes in the histogram, and displays it in the hist window.""" bin_count = hist.shape[0] bin_w = 24 img = np.zeros((256, bin_count * bin_w, 3), np.uint8) for i in range(bin_count): h = int(hist[i]) cv2.rectangle(img, (i * bin_w + 2, 255), ((i + 1) * bin_w - 2, 255 - h), (int(180.0 * i / bin_count), 255, 255), -1) img = cv2.cvtColor(img, cv2.COLOR_HSV2BGR) cv2.imshow('hist', img) showBackProj = False showHistMask = False frame = drone_vision.get_latest_valid_picture() if frame is not None: (hgt, wid, dep) = frame.shape cv2.namedWindow('camshift') cv2.namedWindow('hist') cv2.moveWindow('hist', 700, 100) # Move to reduce overlap # Initialize the track window to be the whole frame track_window = (0, 0, wid, hgt) # # Initialize the histogram from the stored image # Here I am faking a stored image with just a couple of blue colors in an array # you would want to read the image in from the file instead histImage = np.array([[[110, 70, 50]], [[111, 128, 128]], [[115, 100, 100]], [[117, 64, 50]], [[117, 200, 200]], [[118, 76, 100]], [[120, 101, 210]], [[121, 85, 70]], [[125, 129, 199]], [[128, 81, 78]], [[130, 183, 111]]], np.uint8) histImage = cv2.imread('orange.jpg') histImage = cv2.cvtColor(histImage,cv2.COLOR_BGR2HSV) maskedHistIm = cv2.inRange(histImage, np.array((0., 60., 32.)), np.array((180., 255., 255.))) cv2.imshow("masked",maskedHistIm) cv2.imshow("histim",histImage) hist = cv2.calcHist([histImage], [0], maskedHistIm, [16], [0, 180]) cv2.normalize(hist, hist, 0, 255, cv2.NORM_MINMAX) hist = hist.reshape(-1) show_hist(hist) # start processing frames while cv2.getWindowProperty('camshift', 0) >= 0: frame = drone_vision.get_latest_valid_picture() vis = frame.copy() hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # convert to HSV mask = cv2.inRange(hsv, np.array((0., 60., 32.)), np.array((180., 255., 255.))) # eliminate low and high saturation and value values # The next line shows which pixels are being used to make the histogram. # it sets to black all the ones that are masked away for being too over or under-saturated if showHistMask: vis[mask == 0] = 0 prob = cv2.calcBackProject([hsv], [0], hist, [0, 180], 1) prob &= mask term_crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1) track_box, track_window = cv2.CamShift(prob, track_window, term_crit) print(track_box[1][0]*track_box[1][1]) if showBackProj: vis[:] = prob[..., np.newaxis] try: cv2.ellipse(vis, track_box, (0, 0, 255), 2) area = track_box[1][0]*track_box[1][1] if area > 7000: print("GOING BACK") bebop.fly_direct(roll=0,pitch=-20,yaw=0,vertical_movement=0,duration=0.5) #bebop.smart_sleep(1) elif area < 4000: print("GOING FORWARD") bebop.fly_direct(roll=0,pitch=20,yaw=0,vertical_movement=0,duration=0.5) #bebop.smart_sleep(1) except: pass # print("Track box:", track_box) cv2.imshow('camshift', vis) ch = chr(0xFF & cv2.waitKey(5)) if ch == 'q': break elif ch == 'b': showBackProj = not showBackProj elif ch == 'v': showHistMask = not showHistMask bebop.safe_land(10) cv2.destroyAllWindows()
class drone: def __init__(self, home): self.rango_largo = properties.RANGO_LARGO self.rango_ancho = properties.RANGO_ANCHO self.mapa_largo = properties.MAPA_LARGO / self.rango_largo self.mapa_ancho = properties.MAPA_ANCHO / self.rango_ancho self.ip = None self.port = None self.search_map = [[0 for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] self.current_position = home self.current_rotation = math.pi / 2 self.mutex_search_map = threading.Lock() self.poi_position = None self.home = home self.bebop = Bebop() self.init_time = None self.obstaculos = properties.OBSTACLES self.max_altitude = properties.MAX_ALTITUDE self.pathToFollow = None self.destinationZone = None self.countIter = 0 self.logMapTimestamp = None self.logMap = None def initialize(self, ip, port): self.initSearchMapWithObstacles() init_time = time.time() self.init_time = init_time if properties.ALGORITHM == SH_ORIGINAL: self.search_map[self.home[0]][self.home[1]] = 1 elif properties.ALGORITHM == SH_TIMESTAMP or ALGORITHM == SH_NO_GREEDY_TIMESTAMP or ALGORITHM == RANDOM: self.search_map = [[init_time for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] # elif properties.ALGORITHM == RANDOM: # self.search_map[self.home[0]][self.home[1]] = 1 self.ip = ip self.port = port # success = self.bebop.connect(10) # print(success) self.bebop.set_max_altitude(self.max_altitude) self.bebop.ask_for_state_update() if properties.STREAMING_MODE_ON: self.initializeStreaming() self.logMap = [[0 for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] self.logMapTimestamp = [[init_time for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] def initSearchMapWithObstacles(self): for obstacle in self.obstaculos: self.search_map[obstacle[0]][obstacle[1]] = -1 def take_off(self): self.bebop.safe_takeoff(10) def land(self): self.bebop.safe_land(10) def move(self, new_position): verticalMove = self.getDronVerticalAlignment() if properties.ROTATE: rotation_diff = utils.angleDifference(self.current_position, new_position, self.current_rotation) distance_diff = utils.cartesianDistance(self.current_position, new_position) self.bebop.move_relative(0, 0, 0, rotation_diff) time.sleep(2) self.bebop.move_relative(distance_diff, 0, verticalMove, 0) self.current_rotation -= rotation_diff time.sleep(2) else: dx, dy = new_position[0] - self.current_position[0], new_position[1] - self.current_position[1] real_dx, real_dy = dx * self.rango_ancho, dy * self.rango_largo self.bebop.move_relative(real_dx, real_dy, verticalMove, 0) time.sleep(2) self.current_position = new_position self.mutex_search_map.acquire() utils.printMatrix(self.search_map) self.mutex_search_map.release() def setPoiPosition(self, poiPosition): self.poi_position = poiPosition def explore(self, forcePosition): if properties.ALGORITHM == SH_ORIGINAL: return self.explore_sh_original(forcePosition) elif properties.ALGORITHM == SH_TIMESTAMP: return self.explore_sh_timestamp(forcePosition) elif properties.ALGORITHM == RANDOM: return self.explore_random(forcePosition) elif properties.ALGORITHM == SH_NO_GREEDY or properties.ALGORITHM == SH_NO_GREEDY_TIMESTAMP: return self.explore_sh_no_greedy2(forcePosition) def explore_sh_original(self, forcePosition): firstTime = True x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): if firstTime: self.mutex_search_map.acquire() val = self.search_map[x3][y3] self.mutex_search_map.release() firstTime = False # print("x3: " + str(x3) + " y3: " + str(y3) + " self.mapa_ancho: " + str(self.mapa_ancho) + " self.mapa_largo: " + str(self.mapa_largo)) self.mutex_search_map.acquire() if (self.search_map[x3][y3] == val): best_values.append((x3, y3)) val = self.search_map[x3][y3] elif self.search_map[x3][y3] < val: best_values = [(x3, y3)] val = self.search_map[x3][y3] self.mutex_search_map.release() selected = self.selectBestValue(best_values) return selected def explore_sh_timestamp(self, forcePosition): firstTime = True x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): if firstTime: self.mutex_search_map.acquire() val = self.search_map[x3][y3] self.mutex_search_map.release() firstTime = False self.mutex_search_map.acquire() if (self.search_map[x3][y3] == val): best_values.append((x3, y3)) val = self.search_map[x3][y3] elif self.search_map[x3][y3] < val: best_values = [(x3, y3)] val = self.search_map[x3][y3] self.mutex_search_map.release() selected = self.selectBestValue(best_values) return selected def explore_random(self, forcePosition): x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): best_values.append((x3, y3)) selected = self.selectBestValue(best_values) return selected # primer intento de algoritmo no greedy, si se lo trabaja un poco puede funcionar def explore_sh_no_greedy(self, forcePosition): firstTime = True x = self.current_position[0] y = self.current_position[1] best_values = [] for y2 in range(-1, 2): for x2 in range(-1, 2): x3 = x + x2 y3 = y + y2 if self.validatePosition(x3, y3, forcePosition): if firstTime: self.mutex_search_map.acquire() val = self.search_map[x3][y3] self.mutex_search_map.release() firstTime = False self.mutex_search_map.acquire() currentRegion = self.getCurrentRegion() bestRegion = self.selectUnexploredRegion() if self.isClosestToBestRegion(currentRegion, bestRegion, x2, y2): if (self.search_map[x3][y3] == val): best_values.append((x3, y3)) val = self.search_map[x3][y3] elif self.search_map[x3][y3] < val: best_values = [(x3, y3)] val = self.search_map[x3][y3] self.mutex_search_map.release() selected = self.selectBestValue(best_values) return selected def explore_sh_no_greedy2(self, forcePosition): if self.pathToFollow is not None and len(self.pathToFollow) > 0: return self.getNewPositionFromPath() else: newZone = self.isChangeZone() if newZone is not None: self.destinationZone = newZone self.getZonePath(newZone) # self.selectNewZone(newZone) return self.getNewPositionFromPath() else: if ALGORITHM == SH_NO_GREEDY: return self.explore_sh_original(forcePosition) else: return self.explore_sh_timestamp(forcePosition) def explore_sh_no_greedy_timestamp(self, forcePosition): if self.pathToFollow is not None and len(self.pathToFollow) > 0: return self.getNewPositionFromPath() else: newZone = self.isChangeZone() if newZone is not None: self.destinationZone = newZone self.getZonePath(newZone) # self.selectNewZone(newZone) return self.getNewPositionFromPath() else: return self.explore_sh_timestamp(forcePosition) def getNewPositionFromPath(self): new_position = self.pathToFollow[0] del self.pathToFollow[0] if self.getPositionZone(new_position) == self.destinationZone and self.isUnexploredPosition(new_position): self.pathToFollow = None self.destinationZone = None return new_position def isUnexploredPosition(self, new_position): if ALGORITHM == SH_NO_GREEDY_TIMESTAMP or ALGORITHM == SH_TIMESTAMP: timeBetweenLastVisit = time.time() - self.search_map[new_position[0]][new_position[1]] firstTime = self.search_map[new_position[0]][new_position[1]] == self.init_time return timeBetweenLastVisit > TIME_COVERAGE_REFRESH or firstTime else: return self.search_map[new_position[0]][new_position[1]] == self.countIter def isChangeZone(self): currentZone = self.getCurrentRegion() zonesCoverage = self.getZonesCoverage() currentZoneCoverage = zonesCoverage[currentZone - 1] minZoneCoverage = min(zonesCoverage) if minZoneCoverage >= MIN_ACCEPTABLE_COVERAGE: self.countIter += 1 # self.updateCoverageCondition() zonesCoverage = self.getZonesCoverage() currentZoneCoverage = zonesCoverage[currentZone - 1] minZoneCoverage = min(zonesCoverage) if currentZoneCoverage >= MIN_ACCEPTABLE_COVERAGE or currentZoneCoverage - minZoneCoverage > COVERAGE_THRESHOLD: newZone = self.selectUnexploredRegion() return newZone return None def updateCoverageCondition(self): if self.init_time is not None: self.init_time = time.time() else: self.countIter += 1 def getZonePath(self, newZone): zonePosition = self.selectPositionInZone(newZone) pathfinder = Pathfinder(self.current_position, zonePosition) pathToFollow = pathfinder.findPath() self.pathToFollow = pathfinder.parsePathToCoordinates(pathToFollow) def selectPositionInZone(self, zone): if zone == 1: position = (0, 0) elif zone == 2: position = (0, int(self.mapa_largo - 1)) elif zone == 3: position = (int(self.mapa_ancho - 1), 0) else: position = (int(self.mapa_ancho - 1), int(self.mapa_largo - 1)) # if zone == 1 or zone == 2: # while position[0] < self.mapa_ancho and self.search_map[position[0], position[1]] == -1: # position = (position[0] + 1, 0) # elif zone == 3 or zone == 4: # while position[0] < self.mapa_largo and self.search_map[position[0], position[1]] == -1: # position = (0, position[1] + 1) # asumo que encuentro posicion valida return position def selectNewZone(self, currentZone): newZone = random.randint(0, 3) while newZone == currentZone: newZone = random.randint(0, 3) return newZone def isClosestToBestRegion(currentRegion, bestRegion, x, y): if currentRegion == bestRegion: return True bestCoordinates = getBestCoordinates(currentRegion, bestRegion) if x in bestCoordinates[0] and y in bestCoordinates[1]: return True return False def getBestCoordinates(currentRegion, bestRegion): if currentRegion == 1: if bestRegion == 2: return [(-1, 1), (0, 1), (1, 1)] elif bestRegion == 3: return [(1, -1), (1, 0), (1, 1)] else: return [(0, 1), (1, 1), (1, 0)] elif currentRegion == 2: if bestRegion == 1: return [(-1, -1), (0, -1), (1, 1)] elif bestRegion == 3: return [(1, -1), (0, -1), (1, 0)] else: return [(1, -1), (1, 0), (1, 1)] elif currentRegion == 3: if bestRegion == 1: return [(-1, -1), (-1, 0), (-1, 1)] elif bestRegion == 2: return [(-1, 0), (-1, 1), (0, 1)] else: return [(-1, 1), (0, 1), (1, 1)] else: if bestRegion == 1: return [(-1, -1), (-1, 0), (0, -1)] elif bestRegion == 2: return [(-1, -1), (-1, 0), (-1, 1)] else: return [(-1, -1), (0, -1), (1, -1)] def getCurrentRegion(self): return self.getPositionZone(self.current_position) def getPositionZone(self, position): if self.mapa_largo / 2 > position[1] and self .mapa_ancho / 2 > position[0]: return 1 elif self.mapa_largo / 2 <= position[1] and self .mapa_ancho / 2 > position[0]: return 2 elif self.mapa_largo / 2 > position[1] and self .mapa_ancho / 2 <= position[0]: return 3 elif self.mapa_largo / 2 <= position[1] and self .mapa_ancho / 2 <= position[0]: return 4 def selectUnexploredRegion(self): regionCoverage = [] regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, self.mapa_largo / 2, self.mapa_largo)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, self.mapa_largo / 2, self.mapa_largo)) selectedZone = regionCoverage.index(min(regionCoverage)) + 1 if selectedZone == self.getCurrentRegion(): return None return selectedZone def getZonesCoverage(self): regionCoverage = [] regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, 0, self.mapa_largo / 2)) regionCoverage.append(getMapCoverage(self, 0, self.mapa_ancho / 2, self.mapa_largo / 2, self.mapa_largo)) regionCoverage.append(getMapCoverage(self, self.mapa_ancho / 2, self.mapa_ancho, self.mapa_largo / 2, self.mapa_largo)) return regionCoverage def selectBestValue(self, best_values): lenght = len(best_values) # print("selectBestValue: " + str(lenght)) if(lenght == 1): return best_values[0] else: selected = random.randint(0, lenght - 1) return best_values[selected] def validatePosition(self, x3, y3, forcePosition): condition = x3 >= 0 and y3 >= 0 and x3 < self.mapa_ancho and y3 < self.mapa_largo tupla = (x3, y3) if self.pointIsObstacule(x3, y3): return False elif (forcePosition is not None): return (condition and self.minDistanceToTarget(self.home, self.current_position, tupla)) elif self.poi_position is not None: return (condition and self.minDistanceToTarget(self.poi_position, self.current_position, tupla)) elif self.checkBatteryStatus() == NORMAL: return condition else: return (condition and self.minDistanceToTarget(self.home, self.current_position, tupla)) def minDistanceToTarget(self, target, positionA, positionB): # print("minDistanceToTarget") distance2 = self.calculateDistance(target, positionA) distance1 = self.calculateDistance(target, positionB) # print("distance1: " + str(distance1) + " distance2: " + str(distance2)) return (distance1 <= distance2) def pointIsObstacule(self, x1, x2): isObstacule = False for obs in self.obstaculos: if obs[0] == x1 and obs[1] == x2: isObstacule = True return isObstacule def calculateDistance(self, tuple1, tuple2): return math.sqrt((tuple2[1] - tuple1[1])**2 + (tuple2[0] - tuple1[0])**2) def updateSearchMap(self, tupla): self.mutex_search_map.acquire() if properties.ALGORITHM == SH_ORIGINAL or properties.ALGORITHM == SH_NO_GREEDY: self.search_map[tupla[0]][tupla[1]] += 1 elif properties.ALGORITHM == SH_TIMESTAMP or ALGORITHM == SH_NO_GREEDY_TIMESTAMP or ALGORITHM == RANDOM: self.search_map[tupla[0]][tupla[1]] = time.time() self.logMap[tupla[0]][tupla[1]] += 1 self.logMapTimestamp[tupla[0]][tupla[1]] = time.time() self.mutex_search_map.release() def getSearchMap(self): # self.mutex_search_map.acquire() return [[self.search_map[i][j] for j in range(int(self.mapa_largo))]for i in range(int(self.mapa_ancho))] # self.mutex_search_map.release() def getDroneAltitude(self): return self.bebop.sensors.sensors_dict["AltitudeChanged_altitude"] def checkDroneAltitudeStatus(self): altitude = self.getDroneAltitude() if (altitude < properties.MIN_ALTITUDE): return TOO_LOW elif (altitude > properties.MAX_ALTITUDE): return TOO_HIGH else: return ALTITUDE_OK def getDronVerticalAlignment(self): droneAltitudeStatus = self.checkDroneAltitudeStatus() verticalAlignment = 0 if (droneAltitudeStatus == TOO_LOW or droneAltitudeStatus == TOO_HIGH): # negative goes up, positive goes down verticalAlignment = self.getDroneAltitude() - properties.OPTIMAL_ALTITUDE else: verticalAlignment = 0 return verticalAlignment def getBatteryPercentage(self): return self.bebop.sensors.battery def checkBatteryStatus(self): batteryPercentage = self.getBatteryPercentage() if batteryPercentage < 5: return CRITICAL elif batteryPercentage < 10: return LOW else: return NORMAL def goHome(self): if self.current_position != self.home: pathfinder = Pathfinder(self.current_position, self.home) pathToFollow = pathfinder.findPath() pathfinder.printFinalMap() for nextPosition in pathfinder.parsePathToCoordinates(pathToFollow): self.move(nextPosition) def getClosestCoordinateToTarget(self, target, pos): res = self.current_position[pos] if res < target[pos]: res = res + 1 elif res > target[pos]: res = res - 1 return res def moveToPoiCritico(self, path): for nextPosition in path: self.move(nextPosition) def moveNextPositionPOICritico(self, new_position): dx, dy = new_position[0] - self.current_position[0], new_position[1] - self.current_position[1] real_dx, real_dy = dx * self.rango_ancho, dy * self.rango_largo verticalMove = self.getDronVerticalAlignment() self.bebop.move_relative(real_dx, real_dy, verticalMove, 0) time.sleep(2) self.current_position = new_position def disconnect(self): if properties.STREAMING_MODE_ON: self.closeStreaming() self.bebop.disconnect() def initializeStreaming(self): print("-- Starting Streaming... --") self.bebop.set_video_resolutions('rec720_stream720') self.bebop.start_video_stream() print("-- Streaming Started! --") def closeStreaming(self): print("-- Stopping Streaming... --") self.bebop.stop_video_stream() print("-- Streaming stopped!")