def init(): global tko global land global linX global linY global Alt global Hdg rospy.init_node('mambo_node',anonymous=True) mamboAdd = rospy.get_param('~Mambo_Address',str("e0:14:7d:11:3d:c7")) wifi = rospy.get_param('~mambo_wifi',False) retries = rospy.get_param('~mambo_retries',3) rospy.loginfo("\n" + rospy.get_name() + "\nParameters:\n" + mamboAdd + "\n" + str(wifi) + "\n" + str(retries) +"\n") s_cmd_vel = rospy.Subscriber('cmd_vel',Twist,cb_cmd_vel) s_take_off = rospy.Subscriber('take_off',Empty,cb_take_off) s_land = rospy.Subscriber('land',Empty,cb_land) mambo = Mambo(mamboAdd,use_wifi=wifi) success = mambo.connect(retries) if(success): mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) mambo.flat_trim() rate = rospy.Rate(60) while not rospy.is_shutdown(): # mambo.ask_for_state_update() if tko == True: mambo.safe_takeoff(7) tko = False if land == True: mambo.safe_land(4) land = False mambo.fly_direct(roll = (linY * 100), pitch = (linX*100),yaw = (Hdg *100), vertical_movement = (Alt*100), duration=0.001) rate.sleep()
def run(self): # you will need to change this to the address of YOUR mambo mamboAddr = "e0:14:d0:63:3d:d0" # make my mambo object mambo = Mambo(mamboAddr, use_wifi=True) print("trying to connect") success = mambo.connect(num_retries=3) print("connected: %s" % success) print("sleeping") mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) print("taking off!") mambo.safe_takeoff(3) # from IPython import embed;embed() # mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=50, duration=1) while self._running: message = self.read() self.logger.info("message:%s", str(message)) data = message.get("data") if data: if data == "turn around": mambo.turn_degrees(90) if data == "up": # self.logger.info("up!!") # ok # https://jingtaozf.gitbooks.io/crazepony-gitbook/content/wiki/pitch-yaw-roll.html mambo.fly_direct( roll=0, pitch=0, yaw=0, vertical_movement=50, duration=0.5) if data == "down": # self.logger.info("down!!") # ok mambo.fly_direct( roll=0, pitch=0, yaw=0, vertical_movement=-50, duration=0.5) if data == "flip": success = mambo.flip(direction="back") mambo.safe_land(5)
# if you are using Wifi, this can be ignored mamboAddr = "e0:14:3f:cd:3d:fc" # make my mambo object # remember to set True/False for the wifi depending on if you are using the wifi or the BLE to connect mambo = Mambo(mamboAddr, use_wifi=False) print("trying to connect") success = mambo.connect(num_retries=3) print("connected: %s" % success) if (success): # get the state information print("sleeping") mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) print("taking off!") mambo.safe_takeoff(5) if (mambo.sensors.flying_state != "emergency"): print("flying state is %s" % mambo.sensors.flying_state) #insert movement code here #pitch=forward backward movement #roll=left right tilt #yaw= left right turn #vertical movement = hieght, negatives go towards ground but not too close, -20 is fine for chair #duration= the length of the command
from pyparrot.Minidrone import Mambo from pprint import pprint bt_mac_01 = "d0:3a:de:8a:e6:37" # bt_mac_01 = "d0:3a:82:0a:e6:21" drone_01 = Mambo(address=bt_mac_01, use_wifi=False) print("trying to connect") success_01 = drone_01.connect(num_retries=3) print("drone_01 connected: %s " % success_01) if (success_01): #& success_02: # get the state information print("test stats") print("taking off!") drone_01.safe_takeoff(5) for i in range(10): drone_01.ask_for_state_update() pprint(vars(drone_01.sensors)) drone_01.smart_sleep(0.1) drone_01.safe_land(5) drone_01.disconnect()
class Drone: def __init__(self, test_flying, mambo_addr, use_wifi=True, use_vision=True): """ Constructor for the Drone Superclass. When writing your code, you may redefine __init__() to have additional attributes for use in processing elsewhere. If you do this, be sure to call super().__init__() with relevant args in order properly initialize all mambo-related things. test_flying: bool : run flight code if True mambo_addr: str : BLE address of drone use_wifi: bool : connect with WiFi instead of BLE if True use_vision: bool : turn on DroneVisionGUI module if True """ self.test_flying = test_flying self.use_wifi = use_wifi self.use_vision = use_vision self.mamboAddr = mambo_addr self.mambo = Mambo(self.mamboAddr, use_wifi=self.use_wifi) self.mambo.set_user_sensor_callback(self.sensor_cb, args=None) if self.use_vision: self.mamboVision = DroneVisionGUI( self.mambo, is_bebop=False, buffer_size=200, user_code_to_run=self.flight_func, user_args=None) self.mamboVision.set_user_callback_function( self.vision_cb, user_callback_args=None) def execute(self): """ Connects to drone and executes flight_func as well as any vision handling when needed. Run this after initializing your subclass in your main method to start the test/script. """ print("trying to connect to self.mambo now") success = self.mambo.connect(num_retries=3) print("connected: %s" % success) if (success): self.mambo.smart_sleep(1) self.mambo.ask_for_state_update() self.mambo.smart_sleep(1) if self.use_vision: print("starting vision") self.mamboVision.open_video() else: print("starting flight without vision") self.flight_func(None, None) if self.use_vision: print("ending vision") self.mamboVision.close_video() self.mambo.smart_sleep(3) self.mambo.disconnect() def flight_func(self, mamboVision, args): """ Method to me run for flight. This is defined by the user outside of this class. When writing your code, define a new class for each test that inherits this class. Redefine your flight_func in your subclass to match your flight plan. your redefinition of flight_func must include the mamboVision and args arguments to properly work. NOTE: after takeoff it is smart to do the following: print("sensor calib") while self.mambo.sensors.speed_ts == 0: continue This gives the sensors time to fully initialize and send back proper readings for use in your sensor callback method. Cannot be done before takeoff; sensors send nothing at this time. """ pass def vision_cb(self, args): """ Callback function for every vision-handle update Again, defined by the user outside of the class in a specific subclass for every test script. Your vision_cb must include the self and args arguments to work. """ pass def sensor_cb(self, args): """ Callback function for every sensor update. Again, defined by the user outside of the class in a specific subclass for every test script. Your sensor_cb must include the self and args arguments to work. """ pass
def init_controller(addr=None): """ Initiate connection to Mambo via BLE and control it via keyboard (W, A, S, D, Q, E, F) :param addr: MAC address of Mambo to connect to, default: None :return: - """ if addr is None: mamboAddr = "D0:3A:58:76:E6:22" else: mamboAddr = addr mambo = Mambo(mamboAddr, use_wifi=False) print("trying to connect") success = mambo.connect(num_retries=3) print("connected: %s" % success) if success: orig_settings = termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin) x = 0 sending = False battery = mambo.sensors.battery print("Battery on take off:", battery) if not mambo.is_landed(): mambo.safe_land(1) while x != chr(27): # ESC x = sys.stdin.read(1)[0] if x == 'f': if mambo.is_landed(): mambo.ask_for_state_update() print("taking off!") mambo.safe_takeoff(1) else: print("landing with key") mambo.safe_land(1) break elif x == 'l': print("direct landing") mambo.safe_land(1) elif x == 'e': print("upwards!") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=15, duration=1) elif x == 'q': print("downwards!") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-15, duration=1) elif x == 'w': print("forwards!") mambo.fly_direct(roll=0, pitch=10, yaw=0, vertical_movement=0, duration=1) elif x == 's': print("backwards!") mambo.fly_direct(roll=0, pitch=-10, yaw=0, vertical_movement=0, duration=1) elif x == 'a': print("leftwards! :)") mambo.fly_direct(roll=0, pitch=0, yaw=-15, vertical_movement=0, duration=1) elif x == 'd': print("rightwards! :)") mambo.fly_direct(roll=0, pitch=0, yaw=15, vertical_movement=0, duration=1) elif x == 'c': if sending: print("stopping cam feed") sending = False else: print("activate cam feed") sending = True else: print("testing battery:") if mambo.sensors.battery != battery: battery = mambo.sensors.battery if battery < 7: print("landing because battery is low :)", battery) mambo.safe_land(1) break print("battery:", battery) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings) print("disconnect") mambo.disconnect()
def init(): global tko global land global cannon global auto_tko global linX global linY global Alt global Hdg global p_mode global need_to_change_mode global need_to_toggle_mode rospy.init_node('mambo_node', anonymous=True) mamboAdd = rospy.get_param('~bt', str("e0:14:60:5c:3d:c7")) wifi = rospy.get_param('~mambo_wifi', False) retries = rospy.get_param('~mambo_retries', 3) rospy.loginfo("\n" + rospy.get_name() + "\nParameters:\n" + mamboAdd + "\n" + str(wifi) + "\n" + str(retries) + "\n") s_cmd_vel = rospy.Subscriber('cmd_vel', Twist, cb_cmd_vel) s_take_off = rospy.Subscriber('take_off', Empty, cb_take_off) s_land = rospy.Subscriber('land', Empty, cb_land) s_cannon = rospy.Subscriber('cannon', Empty, cb_shoot_cannon) s_auto_tko = rospy.Subscriber('auto_tko', Empty, cb_auto_take_off) s_piloting_mode = rospy.Subscriber('piloting_mode', UInt8, cb_pilotmode) s_toggle_mode = rospy.Subscriber('toggle_mode', Empty, cb_toggle_mode) p_ready = rospy.Publisher('ready', String, queue_size=30) mambo = Mambo(mamboAdd, use_wifi=wifi) success = mambo.connect(retries) if (success): mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) mambo.flat_trim() rate = rospy.Rate(100) while not rospy.is_shutdown(): if tko == True: mambo.safe_takeoff(2) p_ready.publish("ok") tko = False if land == True: mambo.safe_land(2) land = False if cannon == True: mambo.fire_gun() cannon = False if auto_tko == True: mambo.turn_on_auto_takeoff() auto_tko = False if need_to_change_mode == True: if pilotmode(mambo, p_mode): print("Changed mode successfully") else: print("Failed changing mode") need_to_change_mode = False if need_to_toggle_mode == True: if togglemode(mambo): print("Activating preffered mode...") else: print("Failed activating preferred mode") need_to_toggle_mode = False r_y = round(linY, 2) p_x = round(linX, 2) v_z = round(Alt, 2) y_z = round(Hdg, 2) r_y = sat(r_y, 0.98) p_x = sat(p_x, 0.98) v_z = sat(v_z, 0.98) y_z = sat(y_z, 0.98) mambo.fly_direct(roll=(-r_y * 100), pitch=(p_x * 100), yaw=(-y_z * 100), vertical_movement=(v_z * 100), duration=0.01) # linY = 0 # linX = 0 # Hdg = 0 # Alt = 0 rate.sleep()
class Processor(object): """ consumes commands and executes them """ def __init__(self, command_queue, **kwargs): self.queue = command_queue self.is_flying = False self.connected = False self.droneAddr = kwargs.get('droneAddr', None) self.maxPitch = int(kwargs.get('maxPitch') or 50) self.maxVertical = int(kwargs.get('maxVertical') or 50) if self.droneAddr is None: sys.exit() # always connect with BT self.client = Mambo(self.droneAddr, use_wifi=False) # # set max horizontal/vertical speed # self.client.set_max_tilt(self.maxPitch) # self.client.set_max_vertical_speed(self.maxVertical) def connect(self): # if already connected, ignore command if self.connected: return True if self.client.connect(3): self.client.ask_for_state_update() self.connected = True return True return False def disconnect(self): # if already disconnected, ignore command if not self.connected: return True if self.client.disconnect(): self.connected = False return True return False def fly(self, roll, pitch, yaw, vertical_movement): if not self.is_flying: print("Cannot fly when drone is not in flight mode", file=sys.stderr) return True duration = None roll = float(roll) pitch = float(pitch) yaw = float(yaw) vertical_movement = float(vertical_movement) self.client.fly_direct(roll, pitch, yaw, vertical_movement, duration) return True def land(self, timeout=10): if not self.is_flying: print("Drone already landed", file=sys.stderr) return True self.client.safe_land(int(timeout)) self.is_flying = False return True def emergency(self): self.client.safe_emergency(10) self.client.disconnect() return False def takeoff(self, timeout=10): if self.is_flying: print("Drone already flying", file=sys.stderr) return True self.client.safe_takeoff(int(timeout)) self.is_flying = True return True def process_commands(self): seconds_idle = 0 while True: try: message = self.queue.get_nowait() print("received: " + message, file=sys.stderr) except queue.Empty: if self.connected: if seconds_idle > 1: print("sleeping", file=sys.stderr) self.client.smart_sleep(0.00001) seconds_idle += 1 # stop flying and disconnect if idle for too long if seconds_idle >= 300: print("client timed out. Landing and exiting", file=sys.stderr) self.client.safe_land(10) self.client.disconnect() print("client timed out. Landing and exiting") continue # Ignore blank commands if len(message) <= 0: continue if message == STOPSIGNAL: if self.is_flying: self.client.safe_land(10) self.client.disconnect() break command = message.split() if not self.__getattribute__(command[0])(*command[1:]): self.client.safe_emergency(10) self.client.disconnect() complain("Command {} failed!".format(message)) break # reset idle time seconds_idle = 0
class drawMov: def __init__(self): self.tx, self.ty, self.bx, self.by = None, None, None, None self.top = None self.bottom = None self.left = None self.right = None self.width = None self.height = None self.center = None self.mamboAddr, self.mamboName = None, None self.mambo = None self.droneCheck = False self.droneBattery = None self.inBox = (220, 300) #(130,300)->(250,300)->(260,380) width height self.outBox = (340, 370) #(200,380)->(330,380)->(330,450) width height self.inBoxPos = [] self.outBoxPos = [] # 드론의 상태를 업데이트 한다 def update(self): self.mambo.smart_sleep(0.01) # 드론의 배터리 상태를 갱신한다 self.droneBattery = int(self.mambo.sensors.battery) print("Battery:", self.droneBattery, "% State:", self.mambo.sensors.flying_state) # 타겟의 좌표를 갱신한다 def setTarget(self, target): self.tx, self.ty, self.bx, self.by = int(target[0]), int( target[1]), int(target[2]), int(target[3]) self.top = self.ty self.bottom = self.by self.left = self.tx self.right = self.bx self.width = self.right - self.left self.height = self.bottom - self.top self.center = self.getCenter(target) # 드론 연결 def droneConnect(self): # 일반 패키지의 경우에만 사용하며, 블루투스를 통해 드론을 찾는다 self.mamboAddr, self.mamboName = findMinidrone.getMamboAddr() # FPV의 경우 use_wifi=True, 일반 패키지의 경우 use_wifi=False self.mambo = Mambo(self.mamboAddr, use_wifi=False) self.droneCheck = self.mambo.connect(num_retries=3) print("Drone Connect: ", self.droneCheck) self.mambo.smart_sleep(2) # 드론의 상태를 받아온다 최초 1회만 하면 됨 self.mambo.ask_for_state_update() self.mambo.set_max_tilt(1) # 드론 이륙 def droneStart(self): print('take off') self.mambo.safe_takeoff(5) # 이미지의 크기를 기준으로 드론 움직임의 기준이 되는 InBOX, OutBOX 표현 def getInOutBoxPos(self, img): standardCenter = self.getStandardCenter(img) self.inBoxPos = [ int(standardCenter[0] - self.inBox[0] / 2), int(standardCenter[1] - self.inBox[1] / 2), int(standardCenter[0] + self.inBox[0] / 2), int(standardCenter[1] + self.inBox[1] / 2) ] self.outBoxPos = [ int(standardCenter[0] - self.outBox[0] / 2), int(standardCenter[1] - self.outBox[1] / 2), int(standardCenter[0] + self.outBox[0] / 2), int(standardCenter[1] + self.outBox[1] / 2) ] return standardCenter # 드론 착륙 및 연결 해제 def droneStop(self): if not self.mambo.sensors.flying_state == 'landed': self.mambo.safe_land(5) try: self.mambo.disconnect() except: print("No Ground Cam!!") print("Complete to Stop the Drone!") def getCenter(self, bbox): return [int((bbox[2] + bbox[0]) / 2), int((bbox[3] + bbox[1]) / 2)] def drawLine(self, img): #moveCenter=self.getCenter(bbox) moveCenter = self.getStandardCenter(img) cv2.line(img, (self.center[0], self.center[1]), (moveCenter[0], moveCenter[1]), (255, 0, 0), 2) # 타겟의 위치와 이미지의 중앙점을 선으로 잇고 -Y축 기준으로 드론의 회전 각을 계산한다 def getAngle(self, img): moveCenter = self.getStandardCenter(img) distance = math.sqrt((moveCenter[0] - self.center[0])**2 + (moveCenter[1] - self.center[1])**2) cTheta = (moveCenter[1] - self.center[1]) / (distance + 10e-5) angle = math.degrees(math.acos(cTheta)) return angle def drawCenter(self, img): cv2.circle(img, tuple(self.center), 2, (255, 0, 0), -1) # 드론의 수직이동 값 계산 def adjustVertical(self): vertical = 0 ih = self.inBoxPos[1] oh = self.outBoxPos[1] vt = self.top if vt < oh: vertical = 10 elif vt > ih: vertical = -10 return vertical # 드론의 수평이동, Yaw, Yaw 횟수 계산 def adjustCenter(self, img, stack, yawTime): # right + , front +, vertical roll, yaw = 0, 0 angle = 0 yawCount = yawTime stackLR = stack standardCenter = self.getStandardCenter(img) #cv2.putText(img,"center",tuple(standardCenter),cv2.FONT_HERSHEY_COMPLEX_SMALL,1,(0,0,255),2) roll = self.center[0] - standardCenter[0] if roll < -1: roll = -20 stackLR -= 1 elif roll > 1: roll = 20 stackLR += 1 if yawCount < -1: yaw = -50 yawCount += 1 elif yawCount > 1: yaw = 50 yawCount -= 1 if stackLR > 20: angle = self.getAngle(img) stackLR = 0 print('angle: ', angle) yawCount = int(angle / 7) elif stackLR < -20: angle = -(self.getAngle(img)) stackLR = 0 print('angle: ', angle) yawCount = int(angle / 7) return roll, yaw, stackLR, yawCount def getStandardCenter(self, img): return [int(img.shape[1] / 2), int(img.shape[0] / 2 + 120)] #80->150->0->80 def drawStandardBox(self, img): standardCenter = self.getInOutBoxPos(img) cv2.circle(img, tuple(standardCenter), 2, (0, 0, 255), -1) cv2.rectangle(img, (self.inBoxPos[0], self.inBoxPos[1]), (self.inBoxPos[2], self.inBoxPos[3]), (0, 0, 255), 1) cv2.rectangle(img, (self.outBoxPos[0], self.outBoxPos[1]), (self.outBoxPos[2], self.outBoxPos[3]), (0, 0, 255), 1) def adjustBox(self, img): pitch = 0 if self.width * self.height < self.inBox[0] * self.inBox[1]: pitch = 30 elif self.width * self.height > self.outBox[0] * self.outBox[1]: pitch = -30 return pitch # 타겟의 위치에 따른 드론의 직접적인 움직임 제어 def adjPos(self, img, target, angleStack, yawTime): roll, pitch, yaw, vertical, duration = 0, 0, 0, 0, 0.1 angle = 0 self.drawStandardBox(img) stack = angleStack cv2.putText(img, "Following The Target", (5, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) self.setTarget(target) vertical = self.adjustVertical() if vertical == 0: pitch = self.adjustBox(img) roll, yaw, stack, yawTime = self.adjustCenter(img, stack, yawTime) pos = [roll, pitch, yaw, vertical] if pos == [0, 0, 0, 0]: stack = 0 else: self.mambo.fly_direct(roll=roll, pitch=pitch, yaw=yaw, vertical_movement=vertical, duration=duration) print('Roll:', roll, ' Pitch:', pitch, ' Yaw:', yaw, ' Vertical:', vertical) return stack, yawTime
mamboAddr2 = "d0:3a:bf:42:e6:3a" mambo1 = Mambo(mamboAddr1, use_wifi=False) mambo2 = Mambo(mamboAddr2, use_wifi=False) print("trying to connect") success1 = mambo1.connect(num_retries=3) success2 = mambo2.connect(num_retries=3) print("connected: %s" % success1) print("connected: %s" % success2) if (success1 and success2 == True): # get the state information print("sleeping") mambo1.smart_sleep(2) mambo2.smart_sleep(2) mambo1.ask_for_state_update() mambo2.ask_for_state_update() mambo1.smart_sleep(2) mambo2.smart_sleep(2) print("taking off!") mambo1.safe_takeoff(1) mambo2.safe_takeoff(1) mambo1.turn_degrees(180) mambo2.turn_degrees(180) mambo1.safe_land(5) mambo2.safe_land(5) mambo1.smart_sleep(5) mambo2.smart_sleep(5) mambo1.disconnect() mambo2.disconnect()
class DroneColorSegTest: def __init__(self, testFlying, mamboAddr, use_wifi): self.bb = [0, 0, 0, 0] self.bb_threshold = 4000 self.bb_trigger = False self.testFlying = testFlying self.mamboAddr = mamboAddr self.use_wifi = use_wifi self.mambo = Mambo(self.mamboAddr, self.use_wifi) self.mamboVision = DroneVisionGUI( self.mambo, is_bebop=False, buffer_size=200, user_code_to_run=self.mambo_fly_function, user_args=None) def color_segmentation(self, args): img = self.mamboVision.get_latest_valid_picture() if img is not None: [((x1, y1), (x2, y2)), ln_color] = cd_color_segmentation(img) self.bb = [x1, y1, x2, y2] bb_size = self.get_bb_size() print('bb_size:', bb_size) # cv2.imwrite('test_file.png', img) # uncomment to save latest pic if bb_size >= self.bb_threshold: print('orange detected') self.bb_trigger = True # else: # self.bb_trigger = False else: print('image is None') def get_bb_size(self): ''' returns area of self.bb (bounding box) ''' return (self.bb[2] - self.bb[0]) * (self.bb[3] - self.bb[1]) def mambo_fly_function(self, mamboVision, args): """ self.mambo takes off and yaws slowly in a circle until the vision processing detects a large patch of orange. It then performs a flip and lands. """ if (self.testFlying): print("taking off!") self.mambo.safe_takeoff(5) if (self.mambo.sensors.flying_state != "emergency"): print("flying state is %s" % self.mambo.sensors.flying_state) print("Flying direct: going up") self.mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=15, duration=2) print("flying state is %s" % self.mambo.sensors.flying_state) print("yawing slowly") for deg in range(36): self.mambo.turn_degrees(10) if self.bb_trigger: break self.mambo.smart_sleep(1) print("flying state is %s" % self.mambo.sensors.flying_state) print("flip left") success = self.mambo.flip(direction="left") print("self.mambo flip result %s" % success) self.mambo.smart_sleep(2) print("landing") print("flying state is %s" % self.mambo.sensors.flying_state) self.mambo.safe_land(5) else: print("Sleeeping for 15 seconds - move the self.mambo around") self.mambo.smart_sleep(15) # done doing vision demo print("Ending the sleep and vision") self.mamboVision.close_video() self.mambo.smart_sleep(5) print("disconnecting") self.mambo.disconnect() def run_test(self): print("trying to connect to self.mambo now") success = self.mambo.connect(num_retries=3) print("connected: %s" % success) if (success): # get the state information print("sleeping") self.mambo.smart_sleep(1) self.mambo.ask_for_state_update() self.mambo.smart_sleep(1) print("Preparing to open vision") self.mamboVision.set_user_callback_function( self.color_segmentation, user_callback_args=None) self.mamboVision.open_video()
def main(): tc = tracker.createTrackerByName("KCF") # 일반모드에서 블루투스를 사용하여 드론을 찾는다, 드론의 연결 주소와 이름이 반환된다 # addr,name = findMinidrone.getMamboAddr() addr = None # 드론 객체 생성 FPV의 경우는 WIFI를 사용하며, use_wifi = True가 된다 mambo = Mambo(addr, use_wifi=True) # 드론을 연결한다 success = mambo.connect(3) print("Connect: %s" % success) mambo.smart_sleep(0.01) # 드론의 상태를 업데이트 한다 mambo.ask_for_state_update() # 드론의 움직임 기울기를 1로 설정한다 mambo.set_max_tilt(1) # 드론의 배터리를 확인 할 수 있다 battery = mambo.sensors.battery # FPV 연결 mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30) userVision = UserVision(mamboVision) mamboVision.set_user_callback_function(userVision.get_image, user_callback_args=None) cv2.namedWindow('Vision') cv2.setMouseCallback('Vision', draw_bbox) success = mamboVision.open_video() img = cv2.imread(mamboVision.filename, cv2.IMREAD_COLOR) mask = np.ones_like(img, dtype=np.uint8) tc = tracker.createTrackerByName("KCF") # Tracking Mode mode = False bbox = [] angleStack, yawTime = 0, 0 mov = drawMov() mov.mambo = mambo while (True): # OpenCV를 바탕으로 드론을 제어 하기위해 마스킹이미지를 생성한다 # mask = np.ones((480,600,3),dtype=np.uint8) img = cv2.imread(mamboVision.filename, cv2.IMREAD_COLOR) print(img.shape) # 드론과 연결이 끊기지 않기 위해 매 프레임마다 상태 확인 신호를 보낸다 mambo.smart_sleep(0.01) # 드론의 배터리를 확인 할 수 있다 battery = mambo.sensors.battery print("Battery: %s" % battery) #img=cv2.add(img,mask) #if mode==True: # bbox=tracker.updateTracker(img,tc) # angleStack,yawTime=mov.adjPos(mask,bbox,angleStack,yawTime) cv2.imshow("Vision", mask) key = cv2.waitKey(10) # 'q' 키를 누르면 드론을 착륙하고 프로세스를 종료한다 if ord('q') == key: mambo.safe_land(5) mamboVision.close_video() exit(0) # 'p' 키를 누르면 드론이 이륙한다 elif ord('p') == key: mambo.safe_takeoff(5) # 'w' 키를 누르면 드론에 앞으로 100 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('w') == key: mambo.fly_direct(roll=0, pitch=100, yaw=0, vertical_movement=0, duration=0.1) # 's' 키를 누르면 드론에 뒤로 100 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('s') == key: mambo.fly_direct(roll=0, pitch=-100, yaw=0, vertical_movement=0, duration=0.1) # 'a' 키를 누르면 드론에 왼쪽으로 50 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('a') == key: mambo.fly_direct(roll=-50, pitch=0, yaw=0, vertical_movement=0, duration=0.1) # 'd' 키를 누르면 드론에 오른쪽으로 50 만큼 움직이라는 신호를 0.1초간 보낸다 elif ord('d') == key: mambo.fly_direct(roll=50, pitch=0, yaw=0, vertical_movement=0, duration=0.1) elif ord('i') == key: mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=50, duration=0.1) elif ord('k') == key: mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-50, duration=0.1) elif ord('j') == key: mambo.fly_direct(roll=0, pitch=0, yaw=-50, vertical_movement=0, duration=0.1) elif ord('l') == key: mambo.fly_direct(roll=0, pitch=0, yaw=50, vertical_movement=0, duration=0.1) elif ord('c') == key: mask = np.ones((480, 600, 3), dtype=np.uint8) elif ord('v') == key: print("tracker start") bbox = [tx, ty, bx - tx, by - ty] mode = True ok = tc.init(img, bbox) mov.setBox([tx, ty, bx, by])