def talker():
# Start node
rospy.init_node('mambo_central', anonymous=True)
# Publishers
pub_mambo_01 = rospy.Publisher('/mambo_01/odom', nav_msgs.msg.Odometry, queue_size=1)
pub_mambo_02 = rospy.Publisher('/mambo_02/odom', nav_msgs.msg.Odometry, queue_size=1)
# 20Hz
rate = rospy.Rate(20)
# Enderecos MAC dos Mambos
bt_mac_01 = "d0:3a:de:8a:e6:37"
bt_mac_02 = "d0:3a:82:0a:e6:21"
# Objetos dos drones
drone_01 = Mambo(address=bt_mac_01, use_wifi=False)
drone_02 = Mambo(address=bt_mac_02, use_wifi=False)
# Conectar aos drones
success_01 = drone_01.connect(num_retries=3)
success_02 = drone_02.connect(num_retries=3)
if success_01 & success_02:
# Objetos para publicar nos topicos
data_mano_01 = nav_msgs.msg.Odometry()
data_mano_02 = nav_msgs.msg.Odometry()
# Main loop
while not rospy.is_shutdown() :
data_mano_01.twist.twist.linear.x = drone_01.sensors.speed_x
data_mano_01.twist.twist.linear.y = drone_01.sensors.speed_y
data_mano_01.twist.twist.linear.z = drone_01.sensors.speed_z
# data_mano_01.pose.pose.orientation.w = drone_01.sensors.get_estimated_z_orientation()
data_mano_01.pose.pose.position.x = drone_01.sensors.position_x
data_mano_01.pose.pose.position.y = drone_01.sensors.position_y
data_mano_01.pose.pose.position.z = drone_01.sensors.position_z
data_mano_01.pose.pose.orientation.w = drone_01.sensors.position_psi
# data_mano_01.pose.pose.position.z = drone_01.sensors.altitude
data_mano_02.twist.twist.linear.x = drone_02.sensors.speed_x
data_mano_02.twist.twist.linear.y = drone_02.sensors.speed_y
data_mano_02.twist.twist.linear.z = drone_02.sensors.speed_z
# data_mano_02.pose.pose.orientation.w = drone_02.sensors.get_estimated_z_orientation()
data_mano_02.pose.pose.position.x = drone_02.sensors.position_x
data_mano_02.pose.pose.position.y = drone_02.sensors.position_y
data_mano_02.pose.pose.position.z = drone_02.sensors.position_z
data_mano_02.pose.pose.orientation.w = drone_02.sensors.position_psi
# data_mano_02.pose.pose.position.z = drone_02.sensors.altitude
rospy.loginfo(data_mano_01)
rospy.loginfo(data_mano_02)
pub_mambo_01.publish(data_mano_01)
pub_mambo_02.publish(data_mano_02)
rate.sleep()
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 swarmAssemble():
global droneList
fh = open("drones.txt")
mamboAddr = fh.readlines()
# CONNECT TO ALL DRONES
for droneAddr in mamboAddr:
mambo = Mambo(droneAddr.strip(), use_wifi=False)
print("trying to connect 1")
success = mambo.connect(num_retries=3)
droneList.append(mambo)
print("connected: %s" % success)
fh.close()
def setUp(self):
# 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:B1:35:3D:CB" # "E0:14:F1:84:3D:CA"
# 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)
# dronestates = MamboStateOne(mamboAddr, use_wifi=True)
print("trying to connect")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
self.mambo = mambo
self.state_estimator = NewStateEstimator(mambo)
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)
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)
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()
s.connect(("0.0.0.0", port)) # IP local
print("conected!!")
# connect to the server on local computer
msg = s.recv(1024)
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)
from pyparrot.Minidrone import Mambo
# 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
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
duration=movement['duration'])
def perform_landing(self):
self.drone.safe_land(5)
self.vision.close_video()
self.drone.disconnect()
if __name__ == '__main__':
INSTRUCTIONS = file_manager.get_navigation_instructions_from_json()
print("[INSTRUCTION] Dictionary created")
MAMBO_ADDRESS = "d0:3a:4d:78:e6:36"
mambo = Mambo(MAMBO_ADDRESS, use_wifi=True)
mambo_connected = mambo.connect(num_retries=20)
print("[MAMBO]connected: %s" % mambo_connected)
mambo.smart_sleep(1)
if mambo_connected:
vision = DroneVision(mambo, is_bebop=False, buffer_size=30)
userVision = UserVision(vision, INSTRUCTIONS, mambo)
vision.set_user_callback_function(
userVision.user_code_after_vision_opened, user_callback_args=None)
success = vision.open_video()
print("[VISION] Success in opening vision is %s" % success)
mambo.smart_sleep(3)
else:
print("[VISION]Error connecting to bebop. Retry")
takeoff) #to what topic, type of message and function to call
rospy.Subscriber("/mambo_01/land", Empty, land)
rospy.Subscriber("/mambo_01/cmd_vel", Twist, movement)
rospy.on_shutdown(
shutdown_hook
) #Register handler to be called when rospy process begins shutdown. Request a callback
rospy.spin(
) #It is mainly used to prevent your Python Main thread from exiting
if __name__ == '__main__':
print(sys.version) #To know what python version is being used
if True:
print("trying to connect")
success = mambo.connect(
num_retries=20
) #connect(num_retries) connect to the Minidrone using BLE
#You can specify a maximum number of re-tries. Returns true if the connection suceeded or False otherwise.
print("connected: %s" % success)
if (success):
print("wait! 4s")
mambo.smart_sleep(2)
mambo.ask_for_state_update()
mambo.smart_sleep(2)
print("Mambo 1 ready!")
mambo_functions()
print('main: ros disconnect')
mambo.disconnect()
sys.exit(0)
from pyparrot.Minidrone import Mambo
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)
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()
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)
#print("connected: %s" % success)
#mambo.smart_sleep(2)
#mambo.ask_for_state_update()
if (1):
# get the state information
while (1):
print("\n\n==================================================")
print("Please tell me the command(limit within 2 seconds):")
result = baidu_aip.voice_recognize()
print(result)
if result == "起飞" or result == "许飞" or result == "息飞" or result == "李飞" or result == "喜欢" or result == "飞":
print("taking off")
mambo.connect(num_retries=3)
mambo.ask_for_state_update()
mambo.safe_takeoff(5)
#mambo.smart_sleep(5)
print("Control End...")
if result == "前进":
print("flip front")
mambo.connect(num_retries=3)
mambo.ask_for_state_update()
success = mambo.flip(direction="front")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
if result == "降落":
print("landing")
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()
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 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])
