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)
from pyparrot.Minidrone import Mambo from pprint import pprint bt_mac_01 = "d0:3a:de:8a:e6:37" #"90:3a:e6:21:82:0a" bt_mac_02 = "d0:3a:82:0a:e6:21" #"90:3a:e6:21:82:0a" drone = Mambo(address=bt_mac_01, use_wifi=False) print("trying to connect") success_01 = drone.connect(num_retries=3) print("drone_01 connected: %s " % success_01) if (success_01): print("taking off!") drone.safe_takeoff(5) if (drone.sensors.flying_state != "emergency"): print("flying state is %s" % drone.sensors.flying_state) print("Flying direct: going up") drone.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=20, duration=1) print("flip left") print("flying state is %s" % drone.sensors.flying_state) success = drone.flip(direction="left") print("mambo flip result %s" % success) drone.smart_sleep(2)
if __name__ == "__main__": mamboAddr = "D0:3A:08:89:E6:37" # mamboAddr = "6C:29:95:08:40:3A" # mamboAddr = "D0:3A:15:7D:E6:3B" mambo = Mambo(mamboAddr, use_wifi=True) print("[*] Trying to connect") success = mambo.connect(num_retries=3) print("[!] Connected : %s" % success) if (success): print("[*] Sleeping") mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) print("taking off") mambo.safe_takeoff(2) mambo.smart_sleep(2) print("[*] Flying direct : going forward") mambo.fly_direct(roll=0, pitch=10, yaw=0, vertical_movement=0) print("[*] Flying direct : going backward") mambo.fly_direct(roll=0, pitch=-10, yaw=0, vertical_movement=0) rint("[*] Flying direct :turning") mambo.fly_direct(roll=0, pitch=0, yaw=90, vertical_movement=0)
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 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)
#mamboAddr = "8c:85:90:79:78:49" #wifi mamboAddr = "D0:3A:08:D3:E6:26" #bluetooth # 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) success = mambo.connect(num_retries=3) print("connected: %s" % success) if (success): mambo.smart_sleep(5) print("Flying: taking off!") mambo.safe_takeoff(4) mambo.ask_for_state_update() mambo.smart_sleep(4) print("Flying: going up") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=30, duration=1) mambo.smart_sleep(4) print("Flying: going down") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-30, duration=1) mambo.smart_sleep(4) print("Flying: positive roll") mambo.fly_direct(roll=20, pitch=0, yaw=0, vertical_movement=0, duration=1) mambo.smart_sleep(4)
def sw(r,d): mambo.fly_direct(roll=r, pitch=0, yaw=0, vertical_movement=0, duration=d) cr =r*-2 cd =d*.25 mambo.fly_direct(roll=cr, pitch=0, yaw=0, vertical_movement=0, duration=cd) 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(1) mambo.smart_sleep(2) #going through the bottom of the chair mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-5, duration=2) fw(35,4) #going around rope #right mambo.smart_sleep(2) sw(20,1.75) #forward mambo.smart_sleep(2) fw(35,2) #left
# 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 #mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=2) mambo.fly_direct(roll=0, pitch=0, yaw=0,
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
print("trying to connect") success = mambo.connect(num_retries=3) print("connected: %s" % success) if (success): velocity = 10 duration = 0.3 # get the state information mambo.smart_sleep(2) mambo.ask_for_state_update() mambo.smart_sleep(2) print("taking off!") mambo.safe_takeoff(duration*5) print("Flying direct: going forward (positive pitch)") mambo.fly_direct(roll=0, pitch=50, yaw=0, vertical_movement=0, duration=duration) print("Showing turning (in place) using turn_degrees") mambo.turn_degrees(90) mambo.smart_sleep(duration*4) mambo.turn_degrees(-90) mambo.smart_sleep(duration*4) print("Flying direct: yaw") mambo.fly_direct(roll=0, pitch=0, yaw=velocity, vertical_movement=0, duration=duration)
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() """ 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)
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()
mamboAddr = "D0:3A:CD:C4:E6:21" mambo = Mambo(mamboAddr, use_wifi=True) 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(1) mambo.ask_for_state_update() mambo.smart_sleep(1) import beep mambo.safe_takeoff(3) print("up") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=20, duration=1) mambo.smart_sleep(3) print("down") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-20, duration=1) mambo.smart_sleep(3) print("land") mambo.safe_land(2) mambo.smart_sleep(2) picture_names = mambo.groundcam.get_groundcam_pictures_names()
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()
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 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
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])