class DroneApp(App):
def build(self):
return MainLayout()
def on_start(self):
Clock.schedule_once(self.init, 0)
def on_pause(self):
return True
def on_resume(self):
if not self.drone.state:
self.discover_drone()
@run_on_ui_thread
def init(self, *args, **kwargs):
self.drone = Drone()
self.discover_drone()
def on_activity_result(self, request_code, result_code, intent):
fmt = "on_activity_result request_code={}, result_code={}"
Logger.debug(fmt.format(request_code, result_code))
@run_on_ui_thread
def discover_drone(self):
self.drone.discover()
def __init__(self, host="", port=9000):
threading.Thread.__init__(self)
self.host = host
self.port = port
self.drone = Drone()
self.socket = None
self.start()
class DroneApp(App):
def build(self):
return MainLayout()
def on_start(self):
Clock.schedule_once(self.init, 0)
def on_pause(self):
return True
def on_resume(self):
if not self.drone.state:
self.disvover_drone()
@run_on_ui_thread
def init(self, *args, **kwargs):
self.drone = Drone()
self.discover_drone()
def on_activity_result(self, request_code, result_code, intent):
fmt = "on_activity_result request_code={}, result_code={}"
Logger.debug(fmt.format(request_code, result_code))
@run_on_ui_thread
def discover_drone(self):
self.drone.discover()
def main():
config = get_config("settings.cfg")
host = config.get('settings', 'host')
drone_port = int(config.get('settings', 'port_seed'))
server_port = int(config.get('settings', 'server_port'))
s = create_socket()
while True:
if bind_socket(s, host, drone_port):
break
else:
drone_port += 1
config.set('settings', 'port_seed', str(drone_port + 1))
with open("settings.cfg", 'w') as configfile:
config.write(configfile)
drone = Drone()
drone.port = drone_port
begin_streaming(s, host, server_port, drone)
s.close()
def connect(self):
print('[+] Connection')
self.s.settimeout(3)
self.s.connect((self.host, self.port))
next_cmd = 'waiting_welcome'
for line in self.recv_line():
print(' |-- %s' % line)
if next_cmd == 'waiting_welcome':
if line == 'BIENVENUE':
self.send_msg(self.team)
next_cmd = 'waiting_nbconnect'
continue
raise Exception('It seems we are not welcome... :(')
if next_cmd == 'waiting_nbconnect':
if line != 'ko':
self.nbconnect = int(line)
next_cmd = 'waiting_mapinfo'
continue
raise Exception('Invalid team')
if next_cmd == 'waiting_mapinfo':
(self.map_x, self.map_y) = [int(i) for i in line.split()]
self.drone = Drone(self, self.map_x, self.map_y, self.team)
break
assert (self.drone)
self.s.settimeout(None)
print(' `-- Connection OK\n')
def __init__(self,
stopped,
min_speed=DEF_MIN_SPEED,
max_speed=DEF_MAX_SPEED,
verbosity=Drone.QUIET,
fpc=DEF_FPC,
initial_d=DEF_INITIAL_D):
"""
[stopped] -> cf Drone.
[min_speed] et [max_speed] -> Les vitesses min et max que l'on peut avoir en donnant des ordres au drone (selon le drone).
[verbosity] -> cf Drone.
[fpc] -> Le nombre d'images que l'on attend avant d'envoyer une commande (règle la précision et la réactivité).
[initial_d] -> La distance initiale entre la cible et le drone lors de sa sélection (pour se déplacer en avant et en arrière).
[mode] -> Selon si l'on choisit nous-même la zone d'image à suivre ou si le drone le fait automatiquement.
"""
Drone.__init__(self, stopped, max_speed, verbosity)
Thread.__init__(self)
self.min_speed = min_speed
self.max_speed = max_speed
self.tracking = False
self.queue = Queue(self, fpc)
self.frontCam()
self.initial_d = initial_d
self._autoMove_last_t = datetime.now()
# Contrôleur PID
self.pid_x = PID(kp=AutoDrone.KP_X,
ki=AutoDrone.KI_X,
kd=AutoDrone.KD_X)
self.pid_y = PID(kp=AutoDrone.KP_Y,
ki=AutoDrone.KI_Y,
kd=AutoDrone.KD_Y)
self.pid_z = PID(kp=AutoDrone.KP_Z,
ki=AutoDrone.KI_Z,
kd=AutoDrone.KD_Z)
def generateDrone(self): while True: x,y=randint(0,self.GRIDS_X-1),randint(0,self.GRIDS_Y-1) if self.grid[y][x] == 0: self.drone = Drone(x,y) self.grid[y][x] = self.drone break
def main():
drone1 = Drone()
user_input = 5
while user_input != 0:
user_input = int(
input("Enter 1 for accelerate, 2 for decelerate, "
"3 for ascend, 4 for descend, 0 for exit: "))
if user_input == 1:
drone1.accelerate()
print("Speed ", drone1.speed, "Height", drone1.height)
elif user_input == 2:
drone1.decelerate()
print("Speed ", drone1.speed, "Height", drone1.height)
elif user_input == 3:
drone1.ascend()
print("Speed ", drone1.speed, "Height", drone1.height)
elif user_input == 4:
drone1.descend()
print("Speed ", drone1.speed, "Height", drone1.height)
else:
print("Error please input 1, 2, 3, 4, or 0")
user_input = int(
input(
"Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: "
))
print("Speed ", drone1.speed, "Height", drone1.height)
def main():
drone1 = Drone()
oper = 1
while oper != 0:
oper = int(
input('Enter 1 for accelerate, 2 for decelerate, '
'3 for ascend, 4 for descend, 0 for exit: '))
if oper == 1:
drone1.accelerate()
print('Speed:', drone1.speed, 'Height:', drone1.height)
print()
elif oper == 2:
drone1.decelerate()
print('Speed:', drone1.speed, 'Height:', drone1.height)
print()
elif oper == 3:
drone1.ascend()
print('Speed:', drone1.speed, 'Height:', drone1.height)
print()
elif oper == 4:
drone1.descend()
print('Speed:', drone1.speed, 'Height:', drone1.height)
print()
def __init__(self):
# Configure the four correctors for each coordinate
self._pid_x = SpeedCorrector()
self._pid_y = SpeedCorrector()
self._pid_z = SpeedCorrector()
self._pid_yaw = SpeedCorrector()
# kalman filter is used for the drone state estimation
self._estimate = StateEstimate()
# Used to process images and backproject them
# this._camera = new Camera(); TODO
# Ensure that we don't enter the processing loop twice
self._busy = False
self._goal = None
self.inFlight = None
# The last known state
self._state = {"x": 0, "y": 0, "z": 0, "yaw": 0}
# The last time we have reached the goal (all control commands = 0)
self._last_ok = 0
# Register the listener on navdata for our control loop
def navdataListener(navdata):
#print("seconds:", navdata.header.stamp.secs)
self._processNavdata(navdata)
self._control(navdata)
# drone to manipulate
self._drone = Drone(navdataListener)
def test_drone_update(self):
drone = Drone()
drone.pos = vec2d(100, 100)
target = vec2d(200, 200)
drone.target.append(target)
drone.update()
self.assertEqual(drone.pos, (103, 103))
self.assertEqual(drone.power, 255-0.1)
def buy_drone(self):
if self.inventory[1][1] > 10000:
self.inventory[1][1] -= 10000
tempdrone = Drone()
tempdrone.pos = self.pos
target = self.pos
tempdrone.target.append(target)
print("New drone is waiting for orders!")
return tempdrone
else:
print("You need 10 000 ore to buy this!")
def spawn_drone(self):
if self.counter % (60 - self.level*2) == 0:
drone = Drone()
max_x = self.width/2 + self.height/2
min_x = self.width/2 - self.height/2
drone.x = randint(min_x, max_x)
drone.y = self.height
drone.dy = randint(-5, -3)
drone.dx = randint(-3, 3)
self.add_widget(drone)
self.drones.append(drone)
def test_mine(self):
p = TestMain()
drone = Drone()
drone.pos = vec2d(100, 100)
target = vec2d(100, 100)
drone.target.append(target)
ore = Ore()
ore.pos = vec2d(100, 100)
ore.mine(p, drone)
self.assertEqual(drone.inventory[1][1], 1)
self.assertEqual(ore.quantity, 799999)
def init_drone(self) -> Drone:
"""Creates a Drone object initialised with a deterministic set of target coordinates.
Returns:
Drone: An initial drone object with some programmed target coordinates.
"""
drone = Drone()
drone.add_target_coordinate((0.35, 0.3))
drone.add_target_coordinate((-0.35, 0.4))
drone.add_target_coordinate((0.5, -0.4))
drone.add_target_coordinate((-0.35, 0))
return drone
def make_3d_lattice(side_num, sep_len):
# create a 3d lattice (cube) of drones
# with side_num drones per edge, sepearted by a distance of sep_len
# Means you will have side_num^3 many drones.
ds = []
for i in range(side_num):
for j in range(side_num):
for k in range(side_num):
d = Drone()
d.position = np.array([i * sep_len, j * sep_len, k * sep_len])
ds.append(d)
return ds
def _create_drones(num_of_clients, drone_capacity):
drones_needed = num_of_clients // drone_capacity
if num_of_clients % drone_capacity == 0:
drones = [
Drone(i + 1, drone_capacity) for i in range(drones_needed)
]
else:
drones_needed += 1
drones = [
Drone(i + 1, drone_capacity) for i in range(drones_needed)
]
return drones, drones_needed
def __init__(self, w=WIDTH, h=HEIGHT):
self.w = w
self.h = h
self.display = pygame.display.set_mode((self.w, self.h))
pygame.display.set_caption('BLOCKS_AI')
self.drone = Drone(20, RED)
self.man = Man(20, BLUE1)
self.reset()
self.clock = pygame.time.Clock()
self.time = 0
def __init__(self, grid):
self.grid = grid
# all of these lists will be for instances of the classes
self.correct = [] # which z=0 positions are in the correct place
self.stacks = [] #dict.fromkeys(['x', 'y', 'z'], 0)
self.fullStacks = []
self.emptySpaces = [] # empty space available.
self.unknown = [
] # list of 2d visualization with which ones are stacks we don't know below
self.toStack = []
self.colourStacks = []
self.drone = Drone(grid, [0, 0])
def reset(self):
""" Reset the Level """
self.completionRating = CompletionRating(self)
self.moveRating = MoveRating(self)
self.powerRating = PowerRating(self.getPowerRating(), self)
self.minefield = Minefield(self.rows, self.columns)
self.drone = Drone(self.minefield, self.moveRating, self.powerRating)
self.defenseItems = []
for defenseClass in self.defenses:
for i in range(self.defenses[defenseClass]):
self.addDefense(defenseClass)
def mov(self, x_1, y_1, z_1):
dro = Drone(self)
sets = 10
zaux = []
xaux = []
yaux = []
'''
for i in range(self):
dro.x[i] = uniform(0.0, (x_1))
dro.y[i] = uniform(0.0, (y_1))
dro.z[i] = uniform(0.0, (z_1))
'''
partes = self/sets
for j in range(sets):
par = randint(1, 4)
theta = np.linspace((-1*par) * np.pi, par * np.pi, partes)
z = uniform(np.linspace(randint(0, 2),
randint(0, 2), partes), randint(-1, int(z_1)))
r = (z**2 + 1)*x_1*y_1*z_1
x = uniform(r * np.sin(theta), randint(-1, int(x_1)))
y = uniform(r * np.cos(theta), randint(-1, int(y_1)))
""" print("X[",j,"]:",x)
print("Y[",j,"]:",y)
print("Z[",j,"]:",z) """
for i in range(int(partes)):
xaux.append(x[i])
yaux.append(y[i])
zaux.append(z[i])
""" for i in range(int(partes)):
dro.x[j*i] = x[i] #* randint(100,120)/100
dro.y[j*i] = y[i] * randint(100, 120)/100
dro.z[j*i] = z[i] # * randint(100, 120)/100 """
dro.x = xaux
dro.y = yaux
dro.z = zaux
#x_1, y_1, z_1 = dro.x[i], dro.y[i], dro.z[i]
return dro.x, dro.y, dro.z
def update(self, delta_time):
Drone.update(self)
self.period -= delta_time
if self.period < 0:
self.choose_direction()
if self.x + self.rng > self.field_width:
self.x_direction = -1
elif self.x - self.rng < 0:
self.x_direction = 1
if self.y + self.rng > self.field_height:
self.y_direction = -1
elif self.y - self.rng < 0:
self.y_direction = 1
def __init__(self):
self.x_max = 50
self.y_max = 50
self.world = World(self.x_max, self.y_max)
#self.world.random()
#self.world.generate_with_roads(resource_points=True, drop_points = True, recharge_points=True)
self.world.generate_city(resource_points=True,
drop_points=True,
recharge_points=True)
# drones may have different charaterisics, battery size, max lift, max weight carrying capacity, turning abilities? some might make only right turns?
self.drones = []
self.drone_world_plots = []
self.message_dispatcher = MessageDispatcher()
self.drone_alpha = Drone('URBAN0X1',
self.world,
self.message_dispatcher,
cooperate=True)
self.drones.append(self.drone_alpha)
self.drone_beta = Drone('URBAN0X2',
self.world,
self.message_dispatcher,
cooperate=True)
self.drones.append(self.drone_beta)
#self.drone_gamma = Drone('URBAN0X3', self.world,self.message_dispatcher, cooperate=False)
#self.drones.append(self.drone_gamma)
#self.fig = plt.figure(figsize=(1, 2))#, dpi=80, facecolor='w', edgecolor='k')
self.drop_point_marker = 'o'
#self.world.drop_point_locations = [random.sample(range(self.world.x_max), 10), random.sample(range(self.world.y_max), 10)]
self.drop_point_plot = None
#reacharge point
self.recharge_point_marker = 's' #square
self.recharge_point_plot = None
#resources
#self.fig = plt.figure(figsize=(1, 2))#, dpi=80, facecolor='w', edgecolor='k')
#self.world.resource_locations = [[5,10,20,40,45],[5,10,20,40,45]]
self.resource_plot = None
self.resource_marker = 'v'
self.markers = ['|', '+', 'x', '*']
self.colors = ['r', 'k', 'g', 'c', 'm', 'b']
self.init_figures()
def main():
drone1 = Drone()
drone_speed = int(drone1.speed)
drone_height = int(drone1.height)
usr_input = input(
'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n'
)
usr_input = error_check(usr_input)
while usr_input != 0:
usr_input = int(usr_input)
if usr_input == 1:
drone1.accelerate()
drone_speed = int(drone1.speed)
print('Speed:', drone_speed, 'Height:', drone_height, '\n')
usr_input = input(
'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n'
)
usr_input = error_check(usr_input)
elif usr_input == 2:
drone1.decelerate()
drone_speed = int(drone1.speed)
print('Speed:', drone_speed, 'Height:', drone_height, '\n')
usr_input = input(
'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n'
)
usr_input = error_check(usr_input)
elif usr_input == 3:
drone1.ascend()
drone_height = int(drone1.height)
print('Speed:', drone_speed, 'Height:', drone_height, '\n')
usr_input = input(
'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n'
)
usr_input = error_check(usr_input)
elif usr_input == 4:
drone1.descend()
drone_height = int(drone1.height)
print('Speed:', drone_speed, 'Height:', drone_height, '\n')
usr_input = input(
'Enter 1 for accelerate, 2 for decelerate, 3 for ascend, 4 for descend, 0 for exit: \n'
)
usr_input = error_check(usr_input)
elif usr_input == 0:
drone_speed = int(drone1.speed)
drone_height = int(drone1.height)
print('Speed:', drone_speed, 'Height:', drone_height, '\n')
def setEnemy(self, enum, x, y):
if (enum == 0):
return Drone(x, y)
elif (enum == 1):
return Fighter(x, y)
elif (enum == 2):
return Diver(x, y)
def connect(self):
print('[+] Connection')
self.s.settimeout(3)
self.s.connect((self.host, self.port))
next_cmd = 'waiting_welcome'
for line in self.recv_line():
print(' |-- %s' % line)
if next_cmd == 'waiting_welcome':
if line == 'BIENVENUE':
self.send_msg(self.team)
next_cmd = 'waiting_nbconnect'
continue
raise Exception('It seems we are not welcome... :(')
if next_cmd == 'waiting_nbconnect':
if line != 'ko':
self.nbconnect = int(line)
next_cmd = 'waiting_mapinfo'
continue
raise Exception('Invalid team')
if next_cmd == 'waiting_mapinfo':
(self.map_x, self.map_y) = [int(i) for i in line.split()]
self.drone = Drone(self, self.map_x, self.map_y, self.team)
break
assert(self.drone)
self.s.settimeout(None)
print(' `-- Connection OK\n')
def __init__(self, config_file):
with open(config_file, 'r') as f:
self.rows, self.cols, self.num_drones, self.num_turns, self.max_payload = get_line(f.readline())
self.num_product_types = get_line_first(f.readline())
weights = get_line(f.readline())
self.products = []
for idx in range(self.num_product_types):
self.products.append(Product(idx, weights[idx]))
self.num_warehouses = get_line_first(f.readline())
self.warehouses = []
for warehouse_idx in range(self.num_warehouses):
loc = tuple(get_line(f.readline()))
warehouse = Warehouse(warehouse_idx, loc)
for prod_idx, prod_quant in enumerate(get_line(f.readline())):
prod = self.products[prod_idx]
warehouse.add_product(prod, prod_quant)
self.warehouses.append(warehouse)
self.num_orders = get_line_first(f.readline())
self.orders = []
for order_idx in range(self.num_orders):
loc = tuple(get_line(f.readline()))
order = Order(order_idx, loc)
f.readline() #just skip cause we know it from the next line
for prod_id in get_line(f.readline()):
prod = self.products[prod_id]
order.add_product(prod)
self.orders.append(order)
self.drones = [Drone(drone_idx, self.warehouses[0].location, self.max_payload) \
for drone_idx in range(self.num_drones)]
def __init__(self, host="", port=9000):
threading.Thread.__init__(self)
self.host = host
self.port = port
self.drone = Drone()
self.socket = None
self.start()
def populate_drones(drones, max_payload, warehouse):
id_ = -1
items = []
for i in xrange(drones):
id_ += 1
items.append(Drone(id_, max_payload, warehouse.position))
return items
def start(self):
factor = len(self.cycle) / (len(self.other_drones) + 1)
start_position = self.unique_id * factor
same_strategy_list = Drone.same_strategy_list(self)
# Denotes mixed strategy
if len(same_strategy_list) != len(self.other_drones.keys()):
if len(same_strategy_list) != 0:
self.type_id = self.unique_id / ((len(self.other_drones.keys()) + 1) / (len(same_strategy_list) + 1))
else:
self.type_id = 0
factor = len(self.cycle) / (len(same_strategy_list) + 1)
if self.type_id == len(same_strategy_list): # Last drone
self.cycle = self.cycle[self.type_id * factor:]
else:
self.cycle = self.cycle[self.type_id * factor:(self.type_id + 1) * factor]
self.index = len(self.cycle) / 2
self.goal = self.cycle[self.index]
self.x = self.cycle[self.index][0]
self.y = self.cycle[self.index][1]
self.x_direction = (self.goal[0] - self.x) / self.velocity
self.y_direction = (self.goal[1] - self.y) / self.velocity
else:
if self.unique_id == len(self.other_drones.keys()): # Last drone
self.cycle = self.cycle[start_position:]
else:
self.cycle = self.cycle[start_position:(self.unique_id + 1) * factor]
self.index = 0
self.goal = self.cycle[0]
self.x = self.cycle[0][0]
self.y = self.cycle[0][1]
self.x_direction = (self.goal[0] - self.x) / self.velocity
self.y_direction = (self.goal[1] - self.y) / self.velocity
def get_thrusts(self, drone: Drone) -> Tuple[float, float]:
"""Takes a given drone object, containing information about its current state
and calculates a pair of thrust values for the left and right propellers.
Args:
drone (Drone): The drone object containing the information about the drones state.
Returns:
Tuple[float, float]: A pair of floating point values which respectively represent the thrust of the left and right propellers, must be between 0 and 1 inclusive.
"""
target_point = drone.get_next_target()
dx = target_point[0] - drone.x
dy = target_point[1] - drone.y
thrust_adj = np.clip(dy * self.ky, -self.abs_thrust_delta,
self.abs_thrust_delta)
target_pitch = np.clip(dx * self.kx, -self.abs_pitch_delta,
self.abs_pitch_delta)
delta_pitch = target_pitch - drone.pitch
thrust_left = np.clip(0.5 + thrust_adj + delta_pitch, 0.0, 1.0)
thrust_right = np.clip(0.5 + thrust_adj - delta_pitch, 0.0, 1.0)
# The default controller sets each propeller to a value of 0.5 0.5 to stay stationary.
return (thrust_left, thrust_right)
def __init__(self, path):
self.path = path
input_data = self._get_input(path)
self.deadline = input_data["deadline"]
self.rows = input_data["rows"]
self.columns = input_data["columns"]
self.num_products = input_data["num_products"]
self.num_drones = input_data["num_drones"]
self.num_orders = input_data["num_orders"]
self.num_warehouses = input_data["num_warehouses"]
self.max_load = input_data["max_load"]
self.products = [
Product(weight) for weight in input_data["product_weights"]
]
self.warehouses = [
Warehouse(wh["row"], wh["column"], wh["stock"])
for wh in input_data["warehouses"]
]
self.drones = [
Drone(i, self.warehouses, input_data["max_load"])
for i in range(input_data["num_drones"])
]
self.orders = [
Order(i, order["row"], order["column"], order["product_ids"])
for i, order in enumerate(input_data["orders"])
]
def registerNewCallSign(self, ssid):
assert isinstance(ssid, str)
i = len(self.managedDrones) + 1
callsign = "%s-%i" % (self.squadronPrefix, i)
self.managedDrones[ssid] = Drone(ssid, callsign)
self.saveDroneList()
return callsign
def main():
drone1 = Drone()
choice = -1
while choice != 0:
choice = int(input(
'Enter 1 for accelerate, 2 for decelerate,3 for ascend, 4 for decend, 0 for exit: '))
if choice == 1:
drone1.accelerate()
elif choice == 2:
drone1.decelerate()
elif choice == 3:
drone1.ascend()
elif choice == 4:
drone1.descend()
print(f'Speed: {drone1.speed} Height {drone1.height}')
def do_GET(self):
self.send_response(200)
self.send_header("Content-type", "text/html")
self.end_headers()
drone = Drone()
message = drone.connect()
drone.command(self.path[1:])
print(message)
self.wfile.write(
bytes("<html><head><title>Title goes here.</title></head>",
"utf-8"))
self.wfile.write(bytes("<body><p>This is a test.</p>", "utf-8"))
self.wfile.write(
bytes("<p>Drone is not connected %s</p>" % message, "utf-8"))
self.wfile.write(bytes("</body></html>", "utf-8"))
return
def deleteDrone():
try:
_json = request.json
title=_json['title']
return Drone.delete_one(query={'title':'Mavic Air'})
except Exception as e:
print(e)
return "Nothing Deleted"
def __init__(self):
self.config_parser = ConfigObj('../config/config')
self.droneNum = int(self.config_parser.get('NUMBER_OF_DRONES'))
self.drones = []
for drone_id in range(1, self.droneNum):
self.drones.append(
Drone(drone_id,
self.getWorkerNode('worker-1')['ipv4'][0]))
class Service:
def __init__(self):
self.__environment = Environment()
self.__board = Board()
self.__drone = Drone(self.__environment)
def getEnvironment(self):
return self.__environment
def getBoard(self):
return self.__board
def getPositionStack(self):
return self.__drone.getPositionStack()
def getVisitedPositions(self):
return self.__drone.getVisitedPositions()
def getDroneXCoord(self):
return self.__drone.getXCoord()
def getDroneYCoord(self):
return self.__drone.getYCoord()
def droneOneStepDFS(self):
return self.__drone.moveDFS(self.__board)
def markBoardDetectedWalls(self):
self.__board.markDetectedWalls(self.__environment,
self.__drone.getXCoord(),
self.__drone.getYCoord())
def __init__(self):
self.w, self.h = 800, 600
PygameHelper.__init__(self, size=(self.w, self.h), fill=((255,255,255)))
self.hud = Hud()
self.drones = []
for i in range(4):
tempagent = Drone()
tempagent.pos = vec2d(int(uniform(0, self.w - 20)), int(uniform(0, 400)))
target = vec2d(int(uniform(0, self.w - 20)), int(uniform(0, 400)))
tempagent.target.append(target)
self.drones.append(tempagent)
self.selected = self.drones[0]
self.builder = self.drones[0]
self.project = vec2d(0, 0)
self.buildtype = 0
self.buildpos = vec2d(0, 0)
self.buildings = []
self.ores = []
count = 0
while count < 15:
tempore = Ore()
tempore.pos = vec2d(int(uniform(0, 1500)), int(uniform(0, 1500)))
if not len(self.ores) == 0:
flag = 1
for orecheck in self.ores:
dist = orecheck.pos.get_distance(tempore.pos)
if dist < 120:
flag = 0
if flag:
self.ores.append(tempore)
count += 1
else:
self.ores.append(tempore)
count += 1
firstbuild = MainBase()
firstbuild.pos = vec2d(300, 300)
firstbuild.target = vec2d(firstbuild.pos[0] + 10, firstbuild.pos[1])
self.buildings.append(firstbuild)
def __init__(self, droneCount, capacity, warehouses, orders,
productTypeWeights):
self.drones = []
for i in range(0, droneCount):
d = Drone(i, capacity, warehouses[0]['location'])
self.drones.append(d)
self.warehouses = warehouses
self.orders = orders
self.productTypeWeights = productTypeWeights
def findDrones():
#recieve that here first and give to query
try:
_json = request.json
title=_json['title']
except Exception as e:
print(e)
query = {'title': title}
return json_util.dumps(Drone.find_many(query))
def update(self, time):
Drone.update(self)
if Cyclic_Drone.check_reached_goal(self):
if self.backtrack == True:
self.backtrack = False
self.goal = self.original_position
self.on_path = True
Cyclic_Drone.set_directions(self)
elif self.on_path == True:
self.on_path = False
self.goal = self.back_goal
Cyclic_Drone.set_directions(self)
elif random.random() < self.probability:
Cyclic_Drone.set_next_goal(self)
self.back_goal = self.goal
self.original_position = (self.x, self.y)
self.backtrack = True
self.set_next_goal()
else:
Cyclic_Drone.set_next_goal(self)
def reset(self):
""" Reset the Level """
self.completionRating = CompletionRating(self)
self.moveRating = MoveRating(self)
self.powerRating = PowerRating(self.getPowerRating(), self)
self.minefield = Minefield(self.rows, self.columns)
self.drone = Drone(self.minefield, self.moveRating, self.powerRating)
self.defenseItems = []
for defenseClass in self.defenses:
for i in range(self.defenses[defenseClass]):
self.addDefense(defenseClass)
# latitude = float(latitude)
# longitude = float(longitude)
# altitude = float(altitude)
# print "Set waypoint as %f, %f, %f?" % (latitude, longitude, altitude)
# waypoint = LocationGlobalRelative(latitude, longitude, altitude)
# print "WAYPOINT SET as %f, %f, %f?" % (latitude, longitude, altitude)
if sitl:
waypoint_location = frame_conversion.get_location_metres(start_location, 122, 0)
# waypoint_location = LocationGlobalRelative(41.834575, -87.626842, 10) #simm
else:
waypoint_location = LocationGlobalRelative(41.837283, -87.624709, 10) #real field
test = Drone(connection_string, waypoint_location.lat, waypoint_location.lon)
# print "Clearing mission"
# test.clear_mission()
# test.download_mission()
# print "Preparing mission"
# test.prepare_mission()
# print "Uploading mission"
# test.upload_mission()
# print "oops"
# raw_input("Press enter to begin arming and taking off")
# test.arm()
# test.takeoff()
# test.begin_mission()
class DroneClient(threading.Thread):
"""
Class representing a client which receive drone commands
and send drone odometry.
"""
running = True
def __init__(self, host="", port=9000):
threading.Thread.__init__(self)
self.host = host
self.port = port
self.drone = Drone()
self.socket = None
self.start()
def run(self):
"""
Main loop.
"""
print "DroneClient: Connecting.."
connected = False
while not connected:
try:
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.host, self.port))
self.socket.settimeout(2)
connected = True
except socket.error:
continue
print "DroneClient: Connected to host."
while self.running:
try:
msg = self.socket.recv(1)
if len(msg) == 0:
print "Connection to server lost."
self.close()
elif msg == chr(10):
print "Emergency."
self.drone.emergency()
elif msg == chr(0):
print "Initialize."
self.drone.initialize()
self.socket.send(str(self.drone.getSize()))
self.socket.send("\n")
elif msg == chr(1):
print "Shutdown"
self.drone.shutdown()
self.close()
elif msg == chr(2):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(3):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(4):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
elif msg == chr(5):
self.drone.move(ord(msg))
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
# Testing commands.
elif msg == chr(6):
self.drone.move(2)
elif msg == chr(7):
self.drone.move(3)
elif msg == chr(8):
self.drone.move(4)
elif msg == chr(9):
self.drone.move(5)
elif msg == chr(11):
self.drone.land()
elif msg == chr(12):
self.drone.initialize()
elif msg == chr(13):
self.socket.send(self.drone.get_odometry())
self.drone.send("\n")
except socket.timeout:
pass
except socket.error, err:
print err
self.close()
except KeyboardInterrupt:
print "Interrupted."
self.close()
from include_functions import *
from drone import Drone
from item import Item
if __name__ == "__main__":
read_input_file("data/ex.in")
data = read_input_file("data/busy_day.in")
read_input_file("data/mother_of_all_warehouses.in")
read_input_file("data/redundancy.in")
liste = populate_warehouses(data[4], data[5])
orders, warehouses, drones, items = populate_all("data/busy_day.in")
return
test_item = Item(123, 100)
test_item2 = Item(123, 1)
test_drone = Drone(200)
print test_drone
test_drone.move_to([10, 10])
print test_drone
test_drone.add_item(test_item, None)
test_drone.add_item(test_item, None)
print test_drone
test_drone.deliver_item(test_item, None)
print test_drone
test_drone.add_item(test_item2, None)
print test_drone
def update(self, time):
Drone.update(self)
if self.check_reached_goal():
self.set_next_goal()
class Level:
""" Represents a Level """
def __init__(self, init):
""" Initialize the Level """
self.init = init
self.name = init.name
self.rows = init.rows
self.columns = init.columns
self.defenses = init.defenses
self.reset()
def reset(self):
""" Reset the Level """
self.completionRating = CompletionRating(self)
self.moveRating = MoveRating(self)
self.powerRating = PowerRating(self.getPowerRating(), self)
self.minefield = Minefield(self.rows, self.columns)
self.drone = Drone(self.minefield, self.moveRating, self.powerRating)
self.defenseItems = []
for defenseClass in self.defenses:
for i in range(self.defenses[defenseClass]):
self.addDefense(defenseClass)
def addDefense(self, defenseClass):
""" Add a Defense to the minefield """
defense = defenseClass()
defenseAdder = defenseClass.adderClass()
self.defenseItems.append(defense)
defenseAdder.addDefense(defense, self.minefield)
def getPowerRating(self):
""" Returns the amount of power the drone should have on the level """
powerRating = self.rows*self.columns*SCAN_POWER
for defenseClass in self.defenses:
powerRating += self.defenses[defenseClass]*defenseClass.powerRating
return powerRating
def performGameCycle(self):
""" Perform a single Game Cycle """
if not self.finished():
for defense in self.defenseItems:
defense.performGameCycle(self.minefield, self.drone)
elif self.won():
self.tryToAwardRatings()
def lost(self):
""" Return if the player has lost the level """
return self.destroyed() or self.noPower()
def destroyed(self):
""" Return if the player drone has been destroyed """
return self.drone.destroyed
def noPower(self):
""" Return if the player has no power """
return not self.drone.hasPower()
def won(self):
""" Return if the player has won the level """
won = True
for defense in self.defenseItems:
if not defense.isDeactivated():
return False
else:
return self.drone.powerRating.power >= 0
return won
def finished(self):
""" Return if the level is finished """
return self.lost() or self.won()
def tryToAwardRatings(self):
""" Try To award the Ratings """
self.completionRating.checkAwarded()
self.moveRating.checkAwarded()
self.powerRating.checkAwarded()
CURRENT_PROFILE.achievement.award()
def getRemainingDefenses(self):
""" Return the number of Remaining defenses """
remainingDefenses = {}
for defenseClass in self.defenses:
remainingDefenses[defenseClass] = 0
for defense in self.defenseItems:
if not defense.isDeactivated():
remainingDefenses[defense.__class__] += 1
return remainingDefenses
def getID(self):
""" Return the Level's ID """
return self.init.id
def add_line(text, offset):
cv2.putText(
image,
text,
(10, offset),
cv2.FONT_HERSHEY_PLAIN,
0.8,
(255, 255, 255)
)
window = cv2.namedWindow("win")
cv2.moveWindow("win", 800, 30)
drone = Drone()
ion() # interaction mode needs to be turned off
r = 5
x = np.arange(0, 100, 1) # we'll create an x-axis from 0 to 2 pi
yaw_error = np.arange(0, 100, 1, dtype="float")
yaw_line, = plot(x, x, label="Yaw")
yaw_line.axes.set_ylim(-r, r)
roll_error = np.arange(0, 100, 1, dtype="float")
roll_line, = plot(x, x, label="Roll")
roll_line.axes.set_ylim(-r, r)
pitch_error = np.arange(0, 100, 1, dtype="float")
def __init__(self, unique_id, radius, field_width, field_height, velocity, rng):
Drone.__init__(self, unique_id, radius, field_width, field_height, velocity, rng)
self.strategy = 'random-walk'
self.x = random.randint(0 + self.rng, self.field_width - self.rng)
self.y = random.randint(0 + self.rng, self.field_height - self.rng)
self.choose_direction()
#!/usr/bin/python from drone import Drone drone = Drone() drone.mainLoop()
from drone import Drone
import time
try:
drone = Drone()
drone.initialize()
time.sleep(10)
drone.turn(90)
x=0
while(x<100):
print drone.getOdometry()
time.sleep(0.05)
x+=1
#drone.turn(-90)
drone.shutdown()
except KeyboardInterrupt:
print('\n\nKeyboard exception received. Emergency shutdown.')
drone.shutdown()
# latitude = float(latitude)
# longitude = float(longitude)
# altitude = float(altitude)
# print "Set waypoint as %f, %f, %f?" % (latitude, longitude, altitude)
# waypoint = LocationGlobalRelative(latitude, longitude, altitude)
# print "WAYPOINT SET as %f, %f, %f?" % (latitude, longitude, altitude)
if sitl:
waypoint_location = frame_conversion.get_location_metres(start_location, 122, 0)
# waypoint_location = LocationGlobalRelative(41.834575, -87.626842, 10) #simm
else:
waypoint_location = LocationGlobalRelative(41.837283, -87.624709, 10) #real field
test = Drone(connection_string, waypoint_location.lat, waypoint_location.lon)
# print "Clearing mission"
# test.clear_mission()
# test.download_mission()
# print "Preparing mission"
# test.prepare_mission()
# print "Uploading mission"
# test.upload_mission()
# print "oops"
# raw_input("Press enter to begin arming and taking off")
# test.arm()
# test.takeoff()
# test.begin_mission()
class Protocol:
_ACTIONS = \
{
# id | cmd | description # time
'go_ahead': ('avance', 'going ahead', 7),
'turn_right': ('droite', 'turning right', 7),
'turn_left': ('gauche', 'turning left', 7),
'get_inventory': ('inventaire', 'inventory ?', 1),
'see': ('voir', 'opening eyes', 7),
'grab_object': ('prend %s', 'grabbing %s', 7),
'drop_object': ('pose %s', 'dropping %s', 7),
'kick': ('expulse', 'kicking', 7),
'cry': ('broadcast %s', 'crying "%s"', 7),
'spell': ('incantation', 'hoyo... hoyo... hoyo...', 300),
'fork': ('connect_nbr', 'forking (maybe)', 42),
'real_fork': ('fork', 'forking (real)', 0),
}
def __init__(self, host, port, team):
self.host = host
self.port = port
self.team = team
(self.nbconnect, self.map_x, self.map_y) = (-1, -1, -1)
self.drone = None
self.real_fork_timer = None
self.s = socket.socket()
def print_action(self, desc):
info = '[%s]' % utils.get_colored_text('lpurple', utils.get_id())
if self.drone:
info += ' [level=%s]' % utils.get_colored_text('yellow', str(self.drone.level))
if self.drone.leader:
info += ' [leader=%s]' % utils.get_colored_text('yellow', self.drone.leader)
if self.drone.starvation:
info += ' [%s]' % utils.get_colored_text('lblue', 'starvation')
info += ' [health=%s]' % utils.get_colored_text('lred', str(self.drone.inventory.get('nourriture', 0)))
if self.real_fork_timer:
info += ' [birth t=-%s]' % utils.get_colored_text('lgreen', str(self.real_fork_timer))
print('%s %s' % (info, desc))
def send_msg(self, msg):
#print('> %s' % msg)
self.s.send('%s\n' % msg)
def recv_line(self):
filesocket = self.s.makefile()
for line in filesocket:
line = line.strip()
#print('< %s' % line)
yield line
def do_action(self, id, data=None):
assert(id in Protocol._ACTIONS)
self.last_cmd = id
(cmd, desc, t) = Protocol._ACTIONS[id]
if self.real_fork_timer is not None:
self.real_fork_timer -= t
if self.real_fork_timer <= 0:
self.real_fork_timer = None
args = sys.argv
if utils.term:
args = [utils.term, '-e'] + args
os.spawnv(os.P_NOWAIT, args[0], args)
if data:
desc %= data
self.print_action(desc)
msg = cmd if data is None else cmd % data
self.send_msg(msg)
def connect(self):
print('[+] Connection')
self.s.settimeout(3)
self.s.connect((self.host, self.port))
next_cmd = 'waiting_welcome'
for line in self.recv_line():
print(' |-- %s' % line)
if next_cmd == 'waiting_welcome':
if line == 'BIENVENUE':
self.send_msg(self.team)
next_cmd = 'waiting_nbconnect'
continue
raise Exception('It seems we are not welcome... :(')
if next_cmd == 'waiting_nbconnect':
if line != 'ko':
self.nbconnect = int(line)
next_cmd = 'waiting_mapinfo'
continue
raise Exception('Invalid team')
if next_cmd == 'waiting_mapinfo':
(self.map_x, self.map_y) = [int(i) for i in line.split()]
self.drone = Drone(self, self.map_x, self.map_y, self.team)
break
assert(self.drone)
self.s.settimeout(None)
print(' `-- Connection OK\n')
def run(self):
self.connect()
self.drone.do_something()
for line in self.recv_line():
# Unespected events
if line.startswith('mort'):
self.print_action("I'm dead. :-(")
if utils.term:
raw_input('--- Press Enter to end ---')
if line.startswith('deplacement'):
self.drone.on_forced_move(line.split()[1:])
continue
if line.startswith('message'):
data = line.split(',')
(key, K) = data[0].split()
msg = BroadcastIn(data[1].lstrip(), K)
if msg.direction is not None:
self.drone.on_incoming_message(msg)
continue
if line.startswith('elevation'):
self.drone.on_elevation(line.split()[1:])
continue
if line.startswith('niveau actuel :'):
lvl = int(line.split()[-1])
self.drone.on_levelup(lvl)
self.drone.do_something()
continue
# Command return
if line in ('ok', 'ko'):
if line == 'ko':
self.drone.on_fail()
if self.last_cmd in ('get_inventory' 'see'):
continue
#raise Exception('Fork?')
# Inventory
if self.last_cmd == 'get_inventory':
inventory = self.drone.inventory
for i in line[1:-1].split(','):
item, num = i.split()
inventory[item] = int(num)
# Fork
elif self.last_cmd == 'fork':
if int(line) == 0:
self.do_action('real_fork')
self.real_fork_timer = 600
continue
else:
self.real_fork_timer = 0
# View
elif self.last_cmd == 'see':
view = []
boxes = line[1:-1].split(',')
i = 0
box_to_insert = 1
while i + box_to_insert <= len(boxes):
view.append([data.split() for data in boxes[i:i + box_to_insert]])
i += box_to_insert
box_to_insert += 2
self.drone.view = view
level = len(view) - 1
if level != self.drone.level:
self.drone.level = level
self.drone.do_something()
def __init__(self, unique_id, radius, field_width, field_height, velocity, rng):
Drone.__init__(self, unique_id, radius, field_width, field_height, velocity, rng)
self.create_cycle()
self.strategy = 'non-partition'
def init(self, *args, **kwargs):
self.drone = Drone()
self.discover_drone()
def spawn(self):
if randint(0, 50) == 0:
drone = Drone(self)
drone.pos = pos(randint(20, self.size[0]-20*2), -drone.radius-2)
self.add_entity(drone)
from drone import Drone
# get current working directory
path = os.getcwd()
open_path = path[:path.rfind('src')] + 'cfg/'
filename = 'drone_mouse.cfg'
if __name__ == '__main__':
# loading the ICE and ROS parameters
ic = EasyIce.initialize(['main_drone.py', open_path + filename])
ic, node = comm.init(ic)
# creating the object
robot = Drone(ic)
try:
# executing the mouse behavior
robot.take_off()
time.sleep(1)
while True:
robot.stop()
#~ robot.move("left")
#~ time.sleep(3)
#~ robot.stop()
#~ time.sleep(1)
#~ robot.move("back")
#~ time.sleep(3)
parser = argparse.ArgumentParser(description="Camera feed grabber",
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("-local", dest="local", help="local transport & ip to push from", default=local)
parser.add_argument("-vc", dest="vc", help="video classifier host to pull from", default=vc)
parser.add_argument("-i", dest="interval", type=int, help="Classify every i frames", default=50)
parser.add_argument("-drone", dest="drone", action="store_true",
help="Connect to drone", default=False)
parser.add_argument("-d", dest="debug", action="store_true",
help="show debug window", default=False)
args = parser.parse_args()
send_url = args.local + ":5555" if istcp(args.local) else args.local
recv_url = args.vc + ":5556" if istcp(args.vc) else args.vc
results_url = args.local + ":5557" if istcp(args.local) else args.local
if args.drone:
drone = Drone()
drone.setup()
else:
drone = None
run(send_url, recv_url, results_url, drone=drone, interval=args.interval, debug=args.debug)
cv2.destroyAllWindows()
def pso2(): #Main Control Loop (as prototyped on 2/26 in Glennan Lounge)
"""Main function that runs when the module is called from the command line.
Starts a ui_listener and a drone object, then runs a loop with updates every
50 ms. On each loop iteration, check the ui_state, update the PSO, and calculate
command (for waypoint mode) or pass gamepad command (for manual override mode).
"""
#This is secretly a ROS node. Init it.
rospy.init_node('pso')
# Create listener to receive info from UI
ui_listener = pso_network.UIListener()
ui_listener.daemon = True
ui_listener.start()
ui_state = ui_listener.get_ui()
# Create listener to recieve waypoints and corrections from planner.
planner_listener = pso_network.PlannerListener()
planner_listener.daemon = True
planner_listener.start()
waypoint = cv.CreateMat(4, 1, cv.CV_32FC1)
tf_listener = tf.TransformListener()
#Instatiate Drone Objects (defined in Drone.py)
myDrone = Drone("192.168.1.1")
nav = myDrone.get_navdata()
#Preset flags
running = True
wait_on_emergency = False
wait_on_liftoff = False
wait_on_land = False
#Create Kalman filter, state, and command vectors
kalman = PsoKalman()
u = cv.CreateMat(4, 1, cv.CV_32FC1)
z = cv.CreateMat(5, 1, cv.CV_32FC1)
sys_time = time.time()
#Create PID controllers for each axis
yaw_pid = pso_pid.PID()
yaw_pid.k = 1.5
yaw_pid.t_i = 1.
yaw_pid.angular = True
yaw_pid.deadband = .05
z_pid = pso_pid.PID()
z_pid.k = .00075
z_pid.i_enable = False
z_pid.t_i = 10.
z_pid.deadband = 150
roll_pid = pso_pid.PID()
roll_pid.k = .0002
roll_pid.i_enable = False
roll_pid.d_enable = True
roll_pid.t_d = 1
roll_pid.deadband = 50
pitch_pid = pso_pid.PID()
pitch_pid.k = .0002
pitch_pid.i_enable = False
pitch_pid.d_enable = True
pitch_pid.t_d = 1
pitch_pid.deadband = 50
#Logger puts state in csv for matlab-y goodness
#logger = debuglogger.Logger()
#Fig bucking loop
while not rospy.is_shutdown():
time.sleep(.05)
os.system("clear")
#Get command state from UI
prev_ui_state = ui_state
ui_state = ui_listener.get_ui()
if ui_state[EMERGENCY]:
myDrone.emergency()
if not prev_ui_state[EMERGENCY]:
rospy.loginfo("User ordered emergency");
if ui_state[SHUTDOWN]:
#UI has ordered shutdown
rospy.loginfo( "Shutting down control loop...")
ui_listener.stop()
myDrone.kill()
running = False
if ui_state[TRIM]:
myDrone.trim()
ui_listener.clear_flag(TRIM)
rospy.loginfo("Trimming Drone")
if ui_state[FLYING]:
myDrone.takeoff()
print "Taking Off/Flying"
if not prev_ui_state[FLYING]:
wait_on_liftoff = 10
else:
myDrone.land()
print "Landing/Landed"
if prev_ui_state[FLYING]:
wait_on_land = 5
if ui_state[RESET]:
rospy.loginfo("Resetting drone PSO and emergency state.")
myDrone.reset()
#myDrone.reset_emergency()
yaw_pid.flush()
z_pid.flush()
roll_pid.flush()
pitch_pid.flush()
ui_listener.clear_flag(RESET)
#Get navdata
prevnav = nav
nav = myDrone.get_navdata()
#Print out Drone State
if nav.check_state(navdata.EMERGENCY):
print "Emergency!"
if not prevnav.check_state(navdata.EMERGENCY):
if(ui_state[EMERGENCY]):
rospy.loginfo("Drone is in Emergency State")
else:
rospy.logerr("Unexpected drone emergency")
elif not nav.check_state(navdata.COM_WATCHDOG):
print "WATCHDOG"
elif nav.check_state(navdata.COMMAND):
print "Watchdog cleared. Not yet ready for commands."
else:
print "Ready to Fly\n"
print "\t\tECACAVNAPCUWAPTHLGCMBNTTTCUACVVF\n{0}\t\t{1:32b}".format(nav.seq,nav.state) #Print navdata state
rospy.loginfo("Navdata {0:4}: {1:32b}".format(nav.seq,nav.state))
#Update State (Kalman)
dt = time.time()-sys_time
print "dt:\t",dt
sys_time = time.time()
z[0, 0], z[1, 0], z[2, 0], z[3, 0], z[4, 0] = nav.vx, nav.vy, nav.z, nav.vz, nav.psi
#z and u need to be cv matrices!!!!
sys_state = myDrone.get_state()
print "\nDrone Kalman State:"
print "x:\t{0}".format(sys_state[0, 0])
print "y:\t{0}".format(sys_state[2, 0])
print "z:\t{0}".format(sys_state[4, 0])
print "vx:\t{0}".format(sys_state[1, 0])
print "vy:\t{0}".format(sys_state[3, 0])
print "vz:\t{0}".format(sys_state[5, 0])
print "theta:\t{0}".format(sys_state[6, 0])
print "vtheta:\t{0}".format(sys_state[7, 0])
print "\nNavdata Euler Angles:"
print "theta:\t",nav.theta
print "phi:\t",nav.phi
print "psi:\t",nav.psi
print "\nNavdata Stuff:"
print "z:\t",nav.z
print "vx:\t",nav.vx
print "vy:\t",nav.vy
ui_listener.set_state(sys_state, nav)
br = tf.TransformBroadcaster()
br.sendTransform((sys_state[0,0]/1000., sys_state[2,0]/1000., sys_state[4,0]/1000),
tf.transformations.quaternion_from_euler(nav.phi, nav.psi, -sys_state[6, 0]-math.pi/2),
rospy.Time.now(),
"/base_footprint",
"/odom")
#print "ROS Time:\t",rospy.Time.now()
#logger.log(sys_state)
if wait_on_liftoff>0:
print "Waiting for liftoff to finish"
wait_on_liftoff -= dt
u[0, 0], u[1, 0], u[2, 0], u[3, 0] = 0, 0, 1, 0#Assume drone goes full speed up when taking off
elif ui_state[FLYING]:
#If Drone is in waypoint mode, compute command
if not ui_state[OVERRIDE]:
#Get waypoint
if at_waypoint() and not planner_listener.waypoints.empty():
waypoint = planner_listener.waypoints.get()
rospy.loginfo("Next Waypoint: {0}, {1}, {2}, {3}".format(waypoint[0, 0], waypoint[1, 0], waypoint[2, 0], waypoint[3, 0]))
print "\nNext Waypoint:"
print "X:\t", waypoint[0, 0]
print "Y:\t", waypoint[1, 0]
print "Z:\t", waypoint[2, 0]
print "θ:\t", waypoint[3, 0]
#Compute command
(roll_des,pitch_des) = world2drone(waypoint[0, 0]-sys_state[0, 0], waypoint[1, 0]-sys_state[2, 0], sys_state[6, 0])
print "Desired Roll:\t", roll_des
print "Desired Pitch:\t", pitch_des
u[0, 0] = pitch_pid.update(pitch_des)
u[1, 0] = roll_pid.update(-roll_des)
u[2, 0] = 0#z_pid.update(sys_state[4, 0], waypoint[2, 0])
u[3, 0] = yaw_pid.update(sys_state[6,0], waypoint[3,0])
myDrone.go(u[0,0], u[1,0], u[3, 0], u[2,0])
else: #Manual override: Use command from UI state
print "\nManual override mode\n\n\n"
myDrone.go(ui_state[COMMAND][0], ui_state[COMMAND][1], ui_state[COMMAND][2], ui_state[COMMAND][3])
rospy.logdebug("Sent Command to Drone: {0}, {1}, {2}, {3}".format(ui_state[COMMAND][0], ui_state[COMMAND][1], ui_state[COMMAND][2], ui_state[COMMAND][3]))
u[0, 0], u[1, 0], u[2, 0], u[3, 0] = ui_state[COMMAND]
else:
print "\nLanded"
#Print out commands
print "\nCommands:\npitch:\t",u[0, 0]
print "roll:\t", u[1, 0]
print "gaz:\t", u[2, 0]
print "yaw:\t", u[3, 0]
