def __init__(self):
self.risk_metric_sub = rospy.Subscriber("/risk_assessment/risk_metric",
String,
self.risk_metric_callback)
self.drone = Drone(drone_id=int(rospy.get_param('/drone_id', 1)))
self.current_risk_metric = 0
self.mission_request_sub = rospy.Subscriber(
"drone/missionrequest",
mission_request,
self.mission_request_callback,
queue_size=10)
self.velocity_gps = rospy.Subscriber("/mavros/local_position/velocity",
TwistStamped,
self.twist_request_callback,
queue_size=10)
self.position_sub = rospy.Subscriber("mavros/global_position/global",
NavSatFix,
self.position_callback,
queue_size=10)
self.gps_velocity = 0
self.velocityx = 0
self.velocityy = 0
self.velocityz = 0
self.latitude = 0
self.longitude = 0
self.altitude = 0
self.distance = 0
def main():
print("################## Simulation 1 ##########################")
drone = Drone(150, 0, 0.0)
sim = Simulation(drone, 3)
sim.probe()
print("#########################################################")
print("\n\n################## Simulation 2 ##########################")
drone = Drone(40.0, 30.0, 0)
sim = Simulation(drone, 3)
sim.probe()
print("\n\n################## Simulation 3 ##########################")
drone = Drone(0.0, 0.0, 0.0)
sim = Simulation(drone, 2)
sim.probe()
print("#########################################################")
print("\n\n################## Simulation 4 ##########################")
drone = Drone(30.0, 30.0, 30.0)
sim = Simulation(drone, 3)
sim.probe()
print("#########################################################")
print("\n\n################## Simulation 5 ##########################")
drone = Drone(0.0, 0.0, 10.0)
sim = Simulation(drone, 5)
sim.probe()
print("#########################################################\n\n")
def __init__(self):
# Declare the variables required for the program
self.droneArray = []
self.NUMBER_OF_ILLETERATIONS = 1000
rospy.loginfo("Starting aco_ros...")
myDrone = Drone(1, "drone_1", True, False, "/myDrone")
myDrone.start_wandering(self.NUMBER_OF_ILLETERATIONS)
def generate_drones(self):
# Create the drone structures
drones = [
Drone(
dowmsampling=self.environment.downsampling,
index=i,
status_net=True,
placed_pattern=self.general_mission_parameters.
drone_placement_pattern,
mode=Drone.Mode(
previous='FreeFly',
actual=self.general_mission_parameters.mission_actual),
speed=self.general_mission_parameters.speed,
vision=np.zeros(shape=(self.environment.X_pos.shape[0],
self.environment.X_pos.shape[1])),
radius_vision=(10 * 20 / 3) /
self.environment.downsampling, # Radius for vision (pixels)
angular_vision=60, # Degrees of vision (<180)
home=self.general_mission_parameters.drone_home_position,
mission_strat_position=self.general_mission_parameters.
mission_start_position[i],
# std_drone_speed=2, # Standard deviation for the speed of the drone
# std_drone_orientation=0/self.environment.downsampling, # Standard deviation for the orientation of the drone
# std_drone_dire/self.environment.downsamplingction=0/self.environment.downsampling, # Standard deviation for the direction of the drone
std_drone_speed=0.1,
std_drone_orientation=0.1,
std_drone_direction=0.1,
vision_on=True,
corners=self.environment.corners)
for i in range(self.general_mission_parameters.num_drones)
]
return drones
def start(file_name):
"""
Starts sending commands to Tello.
:param file_name: File name where commands are located.
:return: None.
"""
start_time = str(datetime.now())
with open(file_name, 'r') as f:
commands = f.readlines()
tello = Drone()
for command in commands:
if command != '' and command != '\n':
command = command.rstrip()
if command.find('delay') != -1:
sec = float(command.partition('delay')[2])
print(f'delay {sec}')
time.sleep(sec)
pass
else:
tello.send_command(command)
with open(f'log/log.txt', 'w') as out:
log = tello.get_log()
for stat in log:
stat.print_stats()
s = stat.return_stats()
out.write(s)
def drone():
data = request.get_json()
action = data["Action"]
if action == "Register":
id = data["ID"]
drones[id] = Drone(id)
result = jsonify(Result=1)
elif action == "Update":
id = data["ID"]
status = data["Status"]
drones[id].status = status_values[status]
result = jsonify(Result=1)
elif action == "Analyze":
lot = data["Lot"]
image = data["Image"]
#TODO process image
result = jsonify(Result=1)
elif action == "System_Fault":
#TODO process error
result = jsonify(Result=1)
else:
return jsonify(Status=0)
return jsonify(Status=1, DATA=result)
def test_start_stop(self):
drone = Drone(5, (10, 20), (10, 20, 0), 0.2, (0, 0),
Battery(300, 100, 3))
self.assertEqual(drone.start(), 0)
self.assertEqual(drone.start(), -1)
self.assertEqual(drone.stop(), 0)
self.assertEqual(drone.stop(), -1)
def main():
# we create the environment
n = 50
m = 10
fill = 0.3
e = Environment(n, m, fill)
e.randomMap()
# print(str(e))
# we create the map
the_map = DMap(n, m)
# initialize the pygame module
pygame.init()
# load and set the logo
logo = pygame.image.load("logo32x32.png")
pygame.display.set_icon(logo)
pygame.display.set_caption("drone exploration")
# we position the drone somewhere in the area
x = randint(0, n - 1)
y = randint(0, m - 1)
while e.is_occupied(x, y):
x = randint(0, n - 1)
y = randint(0, m - 1)
# create drone
d = Drone(x, y)
# create a surface on screen that has the size of 1000 x 500
screen = pygame.display.set_mode((10*2*n, 10*m))
screen.fill(WHITE)
screen.blit(e.image(), (0, 0))
# define a variable to control the main loop
running = True
the_map.markDetectedWalls(e, d.x, d.y)
# main loop
while running:
# event handling, gets all event from the event queue
for event in pygame.event.get():
# only do something if the event is of type QUIT
if event.type == pygame.QUIT:
# change the value to False, to exit the main loop
pygame.quit()
return
# if event.type == KEYDOWN:
# use this function instead of move
# d.move(m)
time.sleep(0.1)
running = d.moveDSF(the_map)
the_map.markDetectedWalls(e, d.x, d.y)
screen.blit(the_map.image(d.x, d.y), (n*10, 0))
pygame.display.flip()
pygame.quit()
def runTest():
# Create Physics Client
physicsClient = pbl.connect(pbl.DIRECT)
pbl.setAdditionalSearchPath(pybullet_data.getDataPath())
pbl.setGravity(0, 0, -9.8)
# Generate environment
no_obs = 13
ObstacleIDs = generateObstacles(no_obs, [0, 0, 0.5], [0., 5., 0.5])
Qimmiq = Drone(ObstacleIDs)
Collided = False
velocities = [
np.random.normal(size=(3, )) for i in range(len(ObstacleIDs[:-1]))
]
# Run the simulation
meanVel = np.linalg.norm(np.mean(velocities, axis=0))
while not Qimmiq.targetReached and not Collided and Qimmiq.time < 120:
# Ensure that obstacles remain static
[
pbl.resetBaseVelocity(ObstacleIDs[i], velocities[i], [0, 0, 0])
for i in range(len(ObstacleIDs[:-1]))
]
Qimmiq.moveDrone(Qimmiq.time_interval, 0)
# Search for any Collisions
for obstacle in Qimmiq.obstacles[:-1]:
if hasCollided(Qimmiq, obstacle):
Collided = True
print('Collision!')
break
pbl.stepSimulation()
time.sleep(Qimmiq.time_interval)
if Qimmiq.time < 120:
plannerCalcTime = np.mean(Qimmiq.times)
Deviation = Qimmiq.planner.evaluate_deviation()
Curvature = Qimmiq.planner.evaluate_curvature(Qimmiq)
if Collided:
lifeTime = Qimmiq.time
pathTime = np.inf
else:
lifeTime = np.inf
pathTime = Qimmiq.time
pathLength = Qimmiq.pathLength
inputDeviation = Qimmiq.planner.evaluate_input_deviation(Qimmiq)
pbl.disconnect()
return (no_obs, plannerCalcTime, Deviation, Curvature, lifeTime,
pathTime, pathLength, inputDeviation, Collided, True, meanVel)
else:
pbl.disconnect()
return (0, 0, 0, 0, 0, 0, 0, 0, False, False, meanVel)
def test_compute_minimum_area(self):
from Drone import Drone
drone_list = [
Drone('test1', 30.0, 18.0, (3840, 2160), 12, 104),
Drone('test2', 30.0, 18.0, (3840, 2160), 12, 104),
Drone('test3', 30.0, 18.0, (3840, 2160), 6, 104)
]
obtained = Program().compute_minimum_area(drone_list)
self.assertEqual(obtained, (15.359299586316945, 8.639606017303281))
self.assertEqual(obtained[0] * obtained[1], 132.6982971275077)
def deliver(drone: Drone, order: Order):
"substract items from order and drone baggage"
items = order.items_list
for i in drone.loaded_items:
items[i] -= drone.loaded_items[i]
drone.loaded_items[i] = 0
print(drone.name, " D ", order.name, i, drone.loaded_items[i])
drone.current_weight = 0
order.items_list = items
drone.current_location = order.coordinates
def main():
# we create the environment
e = Environment()
e.load_environment("test2.map")
# we create the map
m = DMap()
# initialize the pygame module
pygame.init()
# load and set the logo
logo = pygame.image.load("logo32x32.png")
pygame.display.set_icon(logo)
pygame.display.set_caption("Drone Exploration")
# we position the drone somewhere in the area
x = randint(0, 19)
y = randint(0, 19)
# cream drona
d = Drone(x, y)
# create a surface on screen that has the size of 800 x 480
screen = pygame.display.set_mode((800, 400))
screen.fill(WHITE)
screen.blit(e.image(), (0, 0))
# define a variable to control the main loop
running = True
stack = []
visited = []
parent = {}
# main loop
while running:
# event handling, gets all event from the event queue
for event in pygame.event.get():
# only do something if the event is of type QUIT
if event.type == pygame.QUIT:
# change the value to False, to exit the main loop
running = False
# if event.type == KEYDOWN:
# # use this function instead of move
# # d.move_dsf(m)
# d.move(m)
sleep(0.15)
d.move_dsf(stack, visited, parent, m)
m.mark_detected_walls(e, d.x, d.y)
m.set_explored(d.x, d.y)
screen.blit(m.image(d.x, d.y), (400, 0))
pygame.display.flip()
pygame.quit()
def __init__(self):
rospy.init_node('forebrain')
tfl = TransformListener()
self.drone = Drone(tfl=tfl)
self.world = WorldState(tfl=tfl)
self._explore_srv = rospy.ServiceProxy('/explorer/explore',
ExplorationTarget)
self._tfl = tfl
self.targeting = None
rospy.sleep(1)
def setUp(self):
"""Prepare environment for testing."""
self.drone0 = Drone(0, max_capacity=50, max_speed=10) # Empty drone
self.drone1 = Drone(1, max_capacity=140,
max_speed=8) # Drone with parcels
self.parcel1 = Parcel(1, Pos(1, 1), 10)
self.parcel2 = Parcel(2, Pos(-20, -1), 5)
self.drone1 += self.parcel1
self.drone1 += self.parcel2
def __init__(self, start_x, start_y, color, bounds_color, lidars: list):
"""Smart-Drone initialize.
:param start_x: a x start point in a maze.
:param start_y: a y start point in a maze.
:param color: a drone color.
:param bounds_color: a bounds color
:param lidars: a list of lidars. (list of Sensor object)
"""
Drone.__init__(self, start_x, start_y, color, bounds_color, lidars)
self.auto_flag = False
self.last_cord = start_x, start_y
def test_take_off(self):
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
headBefor =drone.vehicle.heading
locBefor = drone.vehicle.location.global_relative_frame
drone.take_off(3)
headAfter = drone.vehicle.heading
locAfter = drone.vehicle.location.global_relative_frame
self.assertTrue( round(locAfter.alt-locBefor.alt)==3 and ((headBefor+5)%360<headAfter or(headBefor-5)%360 >headAfter))
drone.vehicle.close()
sitl.stop()
def updateDrones(droneList):
"""Adds connected drones to the droneList"""
drones = []
for i in droneList:
i.destroy()
del droneList[:]
drones = connectToDrone()
for d in drones:
drone = Drone(d[0], AppchannelCommunicate(d[0]))
if (len(droneList) > 0):
if (drone.getId() not in [i.getId() for i in droneList]):
droneList.append(drone)
else:
droneList.append(drone)
def test_drone_movement_Z(self):
drone = Drone(0.0, 0.0, 0.0)
drone.moveUp(5)
self.assertEqual(drone.location(), [0.0, 0.0, 5.0])
drone.moveUp(-5)
self.assertEqual(drone.location(), [0.0, 0.0, 0.0])
def main():
drone = Drone()
drone.connect_vehicle_to_serial0()
drone.utils.takeoff.original_drone_takeoff_no_gps()
# drone.utils.goto.go_to_loc(vehicle=drone.vehicle, North=3, East=0)
# drone.utils.goto.return_home(vehicle=drone.vehicle)
# drone.utils.landing.land(vehicle=drone.vehicle)
print("completed")
def setUp(self):
"""Prepare environment for testing."""
self.city = City(position=Pos(0, 0), wind=(1, 2), metric='simple')
self.drone0 = Drone(0, max_capacity=50, max_speed=10) # Empty drone
self.drone1 = Drone(1, max_capacity=140,
max_speed=8) # Drone with parcels
self.parcel1 = Parcel(1, Pos(1, 1), 10)
self.parcel2 = Parcel(2, Pos(-20, -1), 5)
self.drone1 += self.parcel1
self.drone1 += self.parcel2
def createDrone(self) -> Drone:
if self.ai_type == AIType.fuzzyLogic:
return Drone(self.MASS, self.MOMENT, self.physics.getGravity(),
self.startPoistion, FuzzyLogicAI())
elif self.ai_type == AIType.neuronNetwork:
return Drone(self.MASS, self.MOMENT, self.physics.getGravity(),
self.startPoistion, NeuralNetworkAI())
elif self.ai_type == AIType.simpleAI:
return Drone(self.MASS, self.MOMENT, self.physics.getGravity(),
self.startPoistion, SimpleAI())
elif self.ai_type == AIType.manualAI:
return Drone(self.MASS, self.MOMENT, self.physics.getGravity(),
self.startPoistion, ManualAI())
raise RuntimeError("Invalid ai type")
def __init__(self,
kp=(0.5, 0.5, 0.5, 0.5),
kd=(0.1, 0.1, 0.1, 0.0025),
ki=(0.0, 0.0, 0.0, 0.0)):
self.kp = kp
self.kd = kd
self.ki = ki
self.drone = Drone()
self.breaker = True
self.vel = [0.0, 0.0, 0.0, 0.0]
self.prev_time = [0.0, 0.0, 0.0, 0.0]
self.prev_error = [0.0, 0.0, 0.0, 0.0]
self.e_i = [0.0, 0.0, 0.0, 0.0]
#self.speed = 3 #Original value
self.speed = 3 #7
self.repeat_pilot = False
#self.single_point_error = 0.001 #Original value
#self.multiple_point_error = 0.01 #Original value
self.single_point_error = 0.2
self.multiple_point_error = 0.2
#self.ang_error = 0.01 #Original value
self.ang_error = 0.1
self.length = 1
steps = 1000
self.dest_x = []
self.dest_y = []
self.dest_z = []
self.xarr = []
self.yarr = []
self.zarr = []
# r = 0.8
# phi = math.pi/6
for step in range(steps):
# theta = 2*math.pi - (2*step*math.pi/steps) - math.pi
# x = r*math.cos(theta)*math.cos(phi)+0.7
# y = r*math.sin(theta)*math.cos(phi)+0.7
# z = r*math.cos(theta)*math.sin(phi)+0.7
theta = step * (
(3 * math.pi / 2) + math.pi / 2) / (steps) - math.pi
x = 0.5 * math.cos(theta) + 0.7
y = math.cos(theta) * math.sin(theta) + 0.7
z = 0.7
self.xarr.append(x)
self.yarr.append(y)
self.zarr.append(z)
self.arrA = []
self.arrB = []
self.arrC = []
def __init__(self, team_id, password, db):
self.db = db
self.team_id = team_id
self.password = password
self.protocol = None
self.drones = [
Drone(team_id, drone_id, db) for drone_id in range(NUM_DRONES)
]
def parseDrone(self, droneData):
uid = droneData["uid"]
startTime = int(droneData["startTime"])
pointList = WayPointList(droneData["waypoints"])
actionsList = droneData["actions"]
actions = []
for actionData in actionsList:
actions.append(self.parseAction(actionData))
return Drone(uid, startTime, pointList, actions.sort())
def main():
drone = Drone()
target_altitude = 0.5
drone.connect_vehicle_to_serial0()
drone.utils.takeoff.original_drone_takeoff_no_gps(
target_altitude=target_altitude, vehicle=drone.vehicle)
drone.utils.hover.hover(duration=5, vehicle=drone.vehicle)
# drone.utils.change_altitude.gainAltitude(vehicle=drone.vehicle)
# drone.utils.hover.hover(duration=5, vehicle=drone.vehicle)
drone.utils.landing.landing(vehicle=drone.vehicle)
print("completed")
class Symulation:
running = True
space = Space()
drone = Drone()
screen = Screen(space)
clock = time.Clock()
fps = 35
def __init__(self):
self.space.gravity = 0, -980.7
self.space.damping = 0.3
self.space.add(self.drone.GetDrone())
def Start(self):
while self.running:
for ev in event.get():
if key.get_pressed()[K_ESCAPE]:
self.running = False
if ev.type == KEYDOWN:
if ev.key == K_UP:
self.drone.moveDirection[0] = True
if ev.key == K_DOWN:
self.drone.moveDirection[1] = True
if ev.key == K_RIGHT:
self.drone.moveDirection[2] = True
if ev.key == K_LEFT:
self.drone.moveDirection[3] = True
if ev.type == KEYUP:
if ev.key == K_UP:
self.drone.moveDirection[0] = False
if ev.key == K_DOWN:
self.drone.moveDirection[1] = False
if ev.key == K_RIGHT:
self.drone.moveDirection[2] = False
if ev.key == K_LEFT:
self.drone.moveDirection[3] = False
if mouse.get_pressed()[0]:
pos = mouse.get_pos()
if 10 <= pos[0] <= 120:
if 250 <= pos[1] <= 270:
self.drone.logicModel = "Neural Network"
self.fps = 35
self.screen.repeats = 2
elif 280 <= pos[1] <= 300:
self.drone.logicModel = "Fuzzy Logic"
self.fps = 60
self.screen.repeats = 4
self.drone.Move()
self.screen.UpdateScreen(self.space, self.drone)
self.UpdateTimes()
def UpdateTimes(self):
dt = 1. / self.fps
self.space.step(dt)
self.clock.tick(self.fps)
def __init__(self,
kp=[0.05, 0.05, 0.05, 5],
kd=[0.2, 0.2, 0.2, 0.000],
ki=[0.000, 0.000, 0.000, 0.000]):
self.kp = kp
self.kd = kd
self.ki = ki
self.drone = Drone()
self.breaker = True
self.vel = [0.0, 0.0, 0.0, 0.0]
self.prev_time = [0.0, 0.0, 0.0, 0.0]
self.prev_error = [0.0, 0.0, 0.0, 0.0]
self.e_i = [0.0, 0.0, 0.0, 0.0]
self.speed = 1
self.repeat_pilot = False
self.single_point_error = 0.01
self.multiple_point_error = 0.1
self.ang_error = 0.0002
self.length = 1
self.ang = 30
def handle_keys(self, maze, game_display, key):
"""handle keys, move a drone by the pressed key.
:return: true if the drone moves, otherwise false.
:rtype: bool
"""
Drone.handle_keys(self, maze, game_display, key)
if self.auto_flag:
if self.state == DroneState.LAND and self.time_in_air > 0:
self.state = DroneState.TAKE_OFF
self.auto_move(maze=maze, game_display=game_display)
if key[pygame.K_a]:
self.auto_move(maze=maze, game_display=game_display)
if key[pygame.K_d] and self.time_in_air > 0:
self.auto_flag = True
if key[pygame.K_s]:
self.state = DroneState.LAND
if key[pygame.K_w]:
self.slam.show()
return False
def Drone(self):
self.expect("LPAREN")
tok = self.advance()
if tok.type != "STRING":
raise Exception("Expected a string IP declaration",
"for drone but got", tok.type)
ip = tok.value
self.expect("RPAREN")
self.info("Parsed a new Drone with IP", ip)
return Drone(ip)
def move(self, game_display, maze):
"""move forward or backward.
fly fast - an head lidar indicate infinity.
fly regular - an head lidar indicate between 2m-3m.
fly slowly - an head lidar indicate between 1m-2m.
bump fast - an head lidar indicate 1m.
:param game_display:
:param maze:
:return:
"""
if isinf(self.lidars[0].get_sense()):
self.forward(acc=2)
elif self.lidars[0].get_sense() >= 2 * self.lidars[0].radius // 3:
self.backward(acc=0.5)
elif self.lidars[0].get_sense() >= self.lidars[0].radius // 3:
self.backward()
else:
self.backward(acc=2)
Drone.move(self, game_display=game_display, maze=maze)
class Service:
def __init__(self):
self.__environment = Environment(SIZE_X, SIZE_Y)
self.__droneMap = DroneMap()
self.__drone = Drone(randint(0, SIZE_X - 1), randint(0, SIZE_Y - 1))
self.__environment.loadEnvironment("test2.map")
def getEnvironmentImage(self):
return self.__environment.image()
def droneCanMove(self):
return self.__drone.canStillMove()
def getDroneMapImage(self):
return self.__droneMap.image(self.__drone.x, self.__drone.y)
def markDetectedWalls(self):
self.__droneMap.markDetectedWalls(self.__environment, self.__drone.x,
self.__drone.y)
def move(self):
return self.__drone.moveDFS(self.__droneMap)
def test_down(self):
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
headBefor =drone.vehicle.heading
locBefor = drone.vehicle.location.global_relative_frame
drone.take_off(25)
headAfter = drone.vehicle.heading
locAfter = drone.vehicle.location.global_relative_frame
drone.down(1)
print drone.vehicle.location.global_relative_frame
drone.down(4)
print drone.vehicle.location.global_relative_frame
drone.down(5)
print drone.vehicle.location.global_relative_frame
drone.vehicle.close()
sitl.stop()
from Drone import Drone
from PID import PID
from Sensors import IMU, Compass
import time
import readchar
import threading
drone = Drone(throttle = 1300)
imu = IMU()
compass = Compass()
initialRollAngle = imu.getRoll()
initialPitchAngle = imu.getPitch()
initialYawAngle = compass.getYaw()
PID_Roll = PID(initialRollAngle)
PID_Pitch = PID(initialPitchAngle)
PID_Yaw = PID(initialYawAngle)
def PIDThread(keepRunning = True):
#global drone, imu, compass, yawAngleOriginal
global drone, yawAngleOriginal
while keepRunning:
imu = IMU()
compass = Compass()
def main():
"""
print "Start simulator (SITL)"
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768797, 35.193556,0,225 ']
sitl.launch(sitl_args, await_ready=True, restart=True)
# Import DroneKit-Python
# Connect to the Vehicle.
"""
#drone = Drone('tcp:127.0.0.1:5760')
drone = Drone("com3")
drone.take_off(3)
#drone.up(3)
print "#######################################################################################"
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
drone.move_forward(3)
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
print "#######################################################################################"
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
drone.move_right(3)
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
print "#######################################################################################"
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
drone.move_backwards(3)
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
print "#######################################################################################"
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
drone.move_left(3)
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
print "#######################################################################################"
#print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
#drone.move_backwards(2)
#print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
#print "#######################################################################################"
drone.land()
print " global_relative_frame forward drone: %s\n" % drone.vehicle.location.global_relative_frame
print "the heading is : %s" % drone.vehicle.heading
print "#######################################################################################"
drone.vehicle.close()
#f.close()
# Shut down simulator
#sitl.stop()
print("Completed")
def test_move_left(self):
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
"""""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,45']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,90']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,135']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,180']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
""""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,225']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
"""
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,270']
sitl.launch(sitl_args, await_ready=True, restart=True)
drone = Drone('tcp:127.0.0.1:5760')
drone.take_off(2)
print drone.vehicle.location.global_relative_frame
drone.move_left(2)
print drone.vehicle.location.global_relative_frame
drone.land()
drone.vehicle.close()
sitl.stop()
#self.assertTrue(True)
""""
from File import File
def getDrone(x, y):
return drones[0]
if (len(sys.argv) < 2):
print('usage: main.py datafile')
else:
inputFile = File(sys.argv[1])
datas = inputFile.readFile()
drones = []
'''print(datas)'''
warehouses = datas['warehouses']
print(warehouses)
for i in range(datas['drones']):
drone = Drone(i, warehouses[0][3], warehouses[0][4])
drones.append(drone)
for drone in drones:
distance = 0
toDeliver = 0
count = 0
for order in datas['orders']:
'''drone = getDrone(order[0], order[1])'''
newDistance = drone.calculate(order[0], order[1])
print(newDistance)
if (count == 0):
distance = newDistance
toDeliver = order
else:
if (newDistance < distance):
distance = newDistance
