# remember to set True/False for the wifi depending on if you are
mambo = Mambo(mamboAddr, use_wifi=False)
print("trying to connect")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
if (success):
# get the state information
print("sleeping")
mambo.smart_sleep(1)
mambo.ask_for_state_update()
mambo.smart_sleep(1)
print("taking off!")
mambo.takeoff()
# enquanto o speed_ts == 0 esperar, pois os sensores ainda não responderam todos
aux = 0
while mambo.sensors.speed_ts == 0:
mambo.smart_sleep(0.5)
aux = aux + 1
if (aux == 10):
quit()
print("remember shot 3 e 4")
x = threading.Thread(target=go, args=(mambo, ))
x.start()
mambo.fly_direct(0, 40, 0, 0, 1)
mambo.smart_sleep(4)
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)
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)
print(f"manbo 2 - {success}")
if (success):
mambo1.takeoff()
mambo2.takeoff()
