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 (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()
path_w = '/home/pi/output'
for i in range(0, len(picture_names)):
psn = picture_names[i]
frame = mambo.groundcam.get_groundcam_picture(psn, True)
cv2.imwrite(path_w + '/yamada' + str(i) + '.jpg', frame)
print("disconnect")
mambo.disconnect()
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)
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 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)
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)
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()
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()
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
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)
print("Flying direct: going backwards (negative pitch)")
mambo.fly_direct(roll=0, pitch=-velocity, yaw=0,
vertical_movement=0, duration=duration/2.)
print("Flying direct: roll")
mambo.fly_direct(roll=velocity, pitch=0, yaw=0,
vertical_movement=0, duration=duration)
print("Flying direct: going up")
mambo.fly_direct(roll=0, pitch=0, yaw=0,
vertical_movement=velocity, duration=duration)
print("Flying direct: going around in a circle (yes you can mix roll, pitch, yaw in one command!)")
mambo.fly_direct(roll=velocity//2, pitch=0, yaw=velocity,
vertical_movement=0, duration=duration*2)
print("landing")
mambo.safe_land(duration*4)
# mambo.smart_sleep(5)
print("disconnect")
mambo.disconnect()
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)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(180)
mambo.smart_sleep(2)
mambo.turn_degrees(-180)
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()
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
try:
if (success):
# get the state information
print("sleeping")
def takeoff():
mambo.smart_sleep(2)
print("taking off!")
mambo.safe_takeoff(5)
def land():
mambo.safe_land(5)
mambo.smart_sleep(5)
print("disconnect")
mambo.disconnect()
while True:
for event in sense.stick.get_events():
if event.action == 'pressed' and event.direction == 'up':
takeoff()
if event.action == 'pressed' and event.direction == 'down':
land()
except:
print("Error: Uh oh check your code!.\n")
mambo.safe_land(1)
mambo.disconnect()
sys.exit()
while True:
try:
# try receiving data
data, addr = sock.recvfrom(BUFFER_SIZE)
if(time.clock() - loopTime >= FRAME_RATE):
loopTime = time.clock()
goal = data.decode('utf-8').split(" ")
# data is valid
if len(goal) == 4 :
print("x:"+str(int(goal[0]))+" y:"+str(int(goal[1]))+" z:"+str(int(goal[2]))+" o:"+str(int(goal[3])))
if(not DEBUG):
#mambo.fly_direct(roll=int(goal[2]), pitch=-int(goal[0]), yaw=int(goal[3]), vertical_movement=int(goal[1]), duration=FRAME_RATE)
mambo.fly_direct(roll=int(goal[0]), pitch=int(goal[2]), yaw=int(goal[3]), vertical_movement=int(goal[1]), duration=FRAME_RATE)
else:
print("invalid data: " + str(data))
# quit order or timeout (no message incoming)
except KeyboardInterrupt:
break
except:
pass
#landing and quit
sock.close()
if(not DEBUG):
mambo.safe_land(4)
mambo.smart_sleep(4)
mambo.disconnect()
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])
