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!")
