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