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 BLE: you will need to change this to the address of YOUR mambo # if you are using Wifi, this can be ignored mamboAddr = "e0:14:d0:63:3d:d0" # 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=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(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) # print("Flying direct: going up") # mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=20, duration=1) # # print("flip left") # print("flying state is %s" % mambo.sensors.flying_state) # success = mambo.flip(direction="left") # print("mambo flip result %s" % success)
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)
time.sleep(0.5) msg = pickle.loads(msg) #msg = str(msg) time.sleep(0.5) print(f"recebido {msg}") if (msg != "abort"): msg = msg.split(",") if (len(msg) < 2): print(type(msg[0])) # CODIGO PARA CLIENTE QUE APENAS OPERA 1 DRONE mambo = Mambo(msg[0].rstrip(), use_wifi=False) success = mambo.connect(num_retries=5) print(success) if (success): #mambo.takeoff() mambo.takeoff() mambo.smart_sleep(5) mambo.land() else: print("is a list ||%s ||| %s|| " % (msg[0].rstrip(), msg[1].rstrip())) mambo1 = Mambo(msg[0].rstrip(), use_wifi=False) mambo2 = Mambo(msg[1].rstrip(), use_wifi=False) success = mambo1.connect(num_retries=5) print(f"manbo 1 - {success}") if (success): success = mambo2.connect(num_retries=5) print(f"manbo 2 - {success}") if (success): mambo1.takeoff() mambo2.takeoff() mambo1.smart_sleep(1)
# If you are using BLE: you will need to change this to the address of YOUR mambo # if you are using Wifi, this can be ignored #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)
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()
# If you are using BLE: you will need to change this to the address of YOUR mambo # 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
GPIO.setmode(GPIO.BCM) GPIO.setup(14, GPIO.OUT) from pyparrot.Minidrone import Mambo 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)
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
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
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) 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)
from pyparrot.Minidrone import Mambo mamboAddr = "e0:14:c1:6c:3d:cf" mambo = Mambo(mamboAddr, use_wifi=False) print("trying to connect") success = mambo.connect(num_retries=5) print("connected: %s" % success) if (success): # get the state information print("sleeping") mambo.smart_sleep(5) print("taking off!") mambo.safe_takeoff(5) print("Flying direct: going forward (positive pitch)") mambo.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=4) mambo.smart_sleep(0) print("landing") mambo.safe_land(5) mambo.smart_sleep(5) print("disconnect") mambo.disconnect()
#from Minidrone import Mambo from pyparrot.Minidrone import Mambo mamboAddr = "e0:14:ef:39:3d:d1" mambo = Mambo(mamboAddr, use_wifi=False) 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)
import cv2 import cv2.aruco as aruco mambo = Mambo(None, use_wifi=True) # address is None since it only works with WiFi anyway print("trying to connect to mambo now") success = mambo.connect(num_retries=3) print("connected: %s" % success) # Aruco parameters for their respective methods ARUCO_PARAMETERS = aruco.DetectorParameters_create() ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_7X7_50) if success: # get the state information print("sleeping") mambo.smart_sleep(1) mambo.ask_for_state_update() mambo.smart_sleep(1) mambo.safe_takeoff(5) found = True pictures = [] # loop looking for marker while found: # back-up deletion of mambo picture files in case there are any lingering in memory pictures = mambo.groundcam.get_groundcam_pictures_names() if len(pictures) > 0: for i in range(0, len(pictures)): mambo.groundcam._delete_file(pictures[i])
mamboAddr1 = "D0:3A:90:EB:E6:21" 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)
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()
# you will need to change this to the address of YOUR mambo mamboAddr = "fe80::a5b2:6fea:a148:4e25%14" # 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=True) print("trying to connect to mambo now") 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) print("Preparing to open vision") mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30) userVision = UserVision(mamboVision) mamboVision.set_user_callback_function(userVision.save_pictures, user_callback_args=None) success = mamboVision.open_video() print("Success in opening vision is %s" % success) if (success): print("Vision successfully started!") #removed the user call to this function (it now happens in open_video()) #mamboVision.start_video_buffering()
# you will need to change this to the address of YOUR mambo mamboAddr = "e0:14:d0:63:3d:d0" # 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=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(2) mambo.ask_for_state_update() mambo.smart_sleep(2) print("taking off!") mambo.safe_takeoff(5) print("Flying direct: going up") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=50, duration=0.5) mambo.smart_sleep(2) print("Flying direct: going forward (positive pitch)")
from pyparrot.Minidrone import Mambo mamboAddr = "e0:14:c1:6c:3d:cf" mambo = Mambo(mamboAddr, use_wifi=False) print("trying to connect") success = mambo.connect(num_retries=5) print("connected: %s" % success) if (success): # get the state information print("sleeping") mambo.smart_sleep(5) print("taking off!") mambo.safe_takeoff(5) print("Flying direct: going forward (positive pitch)") mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-10, duration=2) mambo.smart_sleep(0) print("Flying direct: going forward (positive pitch)") mambo.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=4) mambo.smart_sleep(0) print("Showing turning (in place) using turn_degrees") mambo.turn_degrees(-45) mambo.smart_sleep(0) print("Showing turning (in place) using turn_degrees")
# 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=True) print("trying to connect") success = mambo.connect(num_retries=3) print("connected: %s" % success) state_estimator = NewStateEstimator(mambo) print("Initial state:", state_estimator.get_current_drone_state()[0]) if (success): try: # get the state information print("sleeping") mambo.smart_sleep(2) # sleeps the requested number of seconds but wakes up for notifications mambo.ask_for_state_update() mambo.smart_sleep(2) print("taking off!") mambo.safe_takeoff(5) reference_value = state_estimator.has_taken_off() print("Initial state:", reference_value) if mambo.sensors.flying_state != "emergency": dur = 0.5 # state estimator update period controller = SmoothController(mambo, state_estimator)
# you will need to change this to the address of YOUR mambo mamboAddr = "e0:14:d0:63:3d:d0" # 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=True) print("trying to connect to mambo now") 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) print("Preparing to open vision") mamboVision = DroneVision(mambo, is_bebop=False, buffer_size=30) userVision = UserVision(mamboVision) # mamboVision.set_user_callback_function(userVision.follow_aruco, user_callback_args=None) success = mamboVision.open_video() print("Success in opening vision is %s" % success) if success: print("Vision successfully started!")
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
# If you are using BLE: you will need to change this to the address of YOUR mambo # if you are using Wifi, this can be ignored mamboAddr = "d0:3a:d1:dc:e6:20" # 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(1) mambo.ask_for_state_update() mambo.smart_sleep(1) mambo.safe_takeoff(5) # take the photo pic_success = mambo.take_picture() # need to wait a bit for the photo to show up mambo.smart_sleep(0.5) picture_names = mambo.groundcam.get_groundcam_pictures_names( ) #get list of print(picture_names) frame = mambo.groundcam.get_groundcam_picture(picture_names[0], True) #get frame if frame is not None: if frame is not False:
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])