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()
