class DroneController(): def __init__(self, mac_address, debug=False, mock=False): self.flying = False self.commands = Queue() if not mock: self.drone = Mambo(mac_address) self.debug = debug self.mock = mock def done(): self.drone.safe_land(5) self.flying = False self.code_to_command = { 0: (lambda: done()), 1: (lambda: self.drone.safe_takeoff(5)), # takeoff 2: (lambda: self.drone.safe_land(5)), # land 3: (lambda: self.drone.fly_direct(0, 10, 0, 0, 1)), # forward 4: (lambda: self.drone.fly_direct(0, -10, 0, 0, 1)), # backward 5: (lambda: self.drone.fly_direct(-10, 0, 0, 0, 1)), # left 6: (lambda: self.drone.fly_direct(10, 0, 0, 0, 1)), # right 7: (lambda: self.drone.fly_direct(0, 0, 0, 10, 1)), # up 8: (lambda: self.drone.fly_direct(0, 0, 0, -10, 1)), # down 9: (lambda: self.drone.turn_degrees(-45)), # turn left 10: (lambda: self.drone.turn_degrees(45)), # turn right 11: (lambda: self.drone.flip("front")), # flip forward 12: (lambda: self.drone.flip("back")), # flip backward 13: (lambda: self.drone.flip("right")), # flip right 14: (lambda: self.drone.flip("left")), # flip left } if mock: self.process_command = lambda c: print(F"Mock execute: {c}") else: self.process_command = lambda c: self.code_to_command[ c.command_code]() def start_flight(self): self.flying = True def fly(): self.flying = True if not self.mock: self.drone.connect(5) self.drone.smart_sleep(5) while self.flying: try: c = self.commands.get(block=False) except: if not self.mock: #self.drone.hover() self.drone.smart_sleep(2) continue if self.debug: print(F"Debug: {c}") self.process_command(c) if not self.mock: self.drone.smart_sleep(3) if not self.mock: self.drone.disconnect() t = Thread(target=fly) t.start() def send_command(self, command): if not self.flying: return False self.commands.put(command) return True def stop_flight(self): # self.send_command(<STOP COMMAND>) pass
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()
pitch=-25, yaw=0, vertical_movement=0, duration=3.25) mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=2) #mambo.fly_direct(roll=-30, pitch=10, yaw=-30, vertical_movement=0, duration=4) #mambo.fly_direct(roll=-40, pitch=10, yaw=-40, vertical_movement=0, duration=2) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=3) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-15, duration=1) #mambo.fly_direct(roll=0, pitch=50, yaw=0, vertical_movement=0, duration=3) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1) #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1) success = mambo.flip(direction="front") print("landing") print("flying state is %s" % mambo.sensors.flying_state) mambo.safe_land(5) mambo.smart_sleep(5) print("disconnect") mambo.disconnect()
print("flying state is %s" % drone.sensors.flying_state) success = drone.flip(direction="left") print("mambo flip result %s" % success) drone.smart_sleep(2) print("flip right") print("flying state is %s" % drone.sensors.flying_state) success = drone.flip(direction="right") print("mambo flip result %s" % success) drone.smart_sleep(2) print("flip front") print("flying state is %s" % drone.sensors.flying_state) success = drone.flip(direction="front") print("mambo flip result %s" % success) drone.smart_sleep(2) print("flip back") print("flying state is %s" % drone.sensors.flying_state) success = drone.flip(direction="back") print("mambo flip result %s" % success) drone.smart_sleep(2) print("landing") print("flying state is %s" % drone.sensors.flying_state) drone.safe_land(5) drone.smart_sleep(2) print("disconnect") drone.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()
class MamboHost: def __init__(self): self.host_name = rospy.get_param('~name') self.host_mac = rospy.get_param('~mac') self.service_connect = rospy.Service( '/mambo_srv/' + self.host_name + '/connect', Connect, self.handle_connect) self.service_take_off = rospy.Service( '/mambo_srv/' + self.host_name + '/takeoff', Connect, self.handle_takeoff) self.service_land = rospy.Service( '/mambo_srv/' + self.host_name + '/land', Connect, self.handle_land) self.sub_disconnect = rospy.Subscriber( '/mambo_srv/' + self.host_name + '/disconnect', String, self.callback_disconnect) self.sub_fly_command = rospy.Subscriber( '/mambo_srv/' + self.host_name + '/fly_command', AttitudeTarget, self.callback_fly_command) self.mambo = Mambo(self.host_mac, use_wifi=False) def handle_connect(self, req): rospy.loginfo(rospy.get_caller_id() + ', I heard %s', req.connect) if (req.connect): success = self.mambo.connect(num_retries=3) rospy.loginfo(rospy.get_caller_id() + ' connected: %s', success) return success else: rospy.loginfo(rospy.get_caller_id() + ' connection not requested.') return False def handle_takeoff(self, req): rospy.loginfo(rospy.get_caller_id() + ', I heard %s', req.connect) if (req.connect): success_msg = self.mambo.takeoff() rospy.loginfo(rospy.get_caller_id() + " take off requested: %s", success_msg) time.sleep(0.5) success = self.mambo.is_takeoff() rospy.loginfo(rospy.get_caller_id() + " has taken off: %s", success) return success else: rospy.loginfo(rospy.get_caller_id() + ' take off not requested.') return False def handle_land(self, req): rospy.loginfo(rospy.get_caller_id() + ', I heard %s', req.connect) if (req.connect): success = self.mambo.safe_land(5) rospy.loginfo(rospy.get_caller_id() + " landing: %s", success) return True else: rospy.loginfo(rospy.get_caller_id() + ' landing not requested.') return False def callback_disconnect(self, data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data) success = self.mambo.disconnect() # rospy.loginfo(rospy.get_caller_id() + ' disconnected: %s', success) def callback_fly_command(self, data): rospy.loginfo(rospy.get_caller_id() + ' flight command: %f' % data.thrust + 'successful.') success = self.mambo.fly_direct(roll=data.orientation.x, pitch=data.orientation.y, yaw=0, vertical_movement=data.thrust, duration=0.2)
class Fly(Visitor): def __init__(self, mac_addr, rs): self.mambo = Mambo(mac_addr, use_wifi=False) self.requirements = rs # positive is east self.x = 0 # positive is up self.y = 0 # positive is south self.z = 0 # Angle of drone from the x axis, so starts north self.theta = 90 info("Trying to connect") success = self.mambo.connect(num_retries=3) info("Connected: %s" % success) def __del__(self): info("Disconnecting") self.mambo.safe_land(5) self.mambo.disconnect() def takeoff(self, tree): info('Taking off') self.mambo.safe_takeoff(5) self.mambo.smart_sleep(1) self.y = TAKE_OFF_HEIGHT def land(self, tree): info('Landing at x={}, y={}, ={}'.format(self.x, self.y, self.z)) self.mambo.safe_land(5) self.y = 0 for r in self.requirements: r.update_on_land(self.x, self.y, self.z) def up(self, tree): duration = toFloat(tree) self.mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=10, duration=duration) self.mambo.smart_sleep(2) self.y += VERTICAL_CALIBRATION * duration def down(self, tree): duration = toFloat(tree) self.mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-10, duration=duration) self.mambo.smart_sleep(2) self.y -= VERTICAL_CALIBRATION * duration def move_in_steps(self, roll, pitch, yaw, v_m, duration): for _ in range(floor(duration)): self.mambo.fly_direct(roll, pitch, yaw, v_m, 1) self.mambo.smart_sleep(2) if floor(duration) is not duration: self.mambo.fly_direct(roll, pitch, yaw, v_m, duration - floor(duration)) self.mambo.smart_sleep(2) def left(self, tree): duration = toFloat(tree) self.move_in_steps(roll=-10, pitch=0, yaw=0, v_m=0, duration=duration) self.x += HORIZONTAL_CALIBRATION * duration * cos( radians(self.theta) + pi / 2) self.z -= HORIZONTAL_CALIBRATION * duration * sin( radians(self.theta) + pi / 2) for r in self.requirements: r.update_on_move(self.x, self.y, self.z) def right(self, tree): duration = toFloat(tree) self.move_in_steps(roll=10, pitch=0, yaw=0, v_m=0, duration=duration) self.x += HORIZONTAL_CALIBRATION * duration * cos( radians(self.theta) - pi / 2) self.z -= HORIZONTAL_CALIBRATION * duration * sin( radians(self.theta) - pi / 2) for r in self.requirements: r.update_on_move(self.x, self.y, self.z) def forward(self, tree): duration = toFloat(tree) self.move_in_steps(roll=0, pitch=10, yaw=0, v_m=0, duration=duration) self.x += HORIZONTAL_CALIBRATION * duration * cos(radians(self.theta)) self.z -= HORIZONTAL_CALIBRATION * duration * sin(radians(self.theta)) for r in self.requirements: r.update_on_move(self.x, self.y, self.z) def backward(self, tree): duration = toFloat(tree) self.move_in_steps(roll=0, pitch=-10, yaw=0, v_m=0, duration=duration) self.x += HORIZONTAL_CALIBRATION * duration * cos( radians(self.theta) + pi) self.z -= HORIZONTAL_CALIBRATION * duration * sin( radians(self.theta) + pi) for r in self.requirements: r.update_on_move(self.x, self.y, self.z) def rotatel(self, tree): degrees = toFloat(tree) self.mambo.turn_degrees(-degrees) self.mambo.smart_sleep(3) self.theta += degrees def rotater(self, tree): degrees = toFloat(tree) self.mambo.turn_degrees(degrees) self.mambo.smart_sleep(3) self.theta -= degrees def wait(self, tree): duration = toFloat(tree) info('Waiting {} seconds'.format(duration)) self.mambo.smart_sleep(duration) def action(self, tree): self.mambo.smart_sleep(2) info('Performed action at ({}, {}, {})'.format(self.x, self.y, self.z)) for r in self.requirements: r.update_on_action(self.x, self.y, self.z) def abort(self): self.mambo.safe_land(5)
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
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() """ if (mambo.sensors.flying_state != "emergency"): print("flying state is %s" % mambo.sensors.flying_state) print("Flying direct: going up") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=20, duration=1) print("Showing turning (in place) using turn_degrees") mambo.turn_degrees(180) mambo.smart_sleep(2) mambo.turn_degrees(-180) mambo.smart_sleep(2) print("landing") print("flying state is %s" % mambo.sensors.flying_state)
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()