def connect(self):
self.dronology = util.Connection(None, "localhost", 1234,self.g_id)
self.dronology.start()
global DO_CONT
DO_CONT = True
w0 = threading.Thread(target=state_out_work, args=(self.dronology, self.vehicles))
w0.start()
def main(path_to_config, ardupath=None):
if ardupath is not None:
global ARDUPATH
ARDUPATH = ardupath
global DO_CONT
DO_CONT = True
config = load_json(path_to_config)
dronology = util.Connection()
dronology.start()
# A list of sitl instances.
sitls = []
# A list of drones. (dronekit.Vehicle)
vehicles = []
# A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...]
# These are the waypoints each drone must go to!
routes = []
# Example:
# vehicle0 = vehicles[0]
# waypoints_for_vehicle0 = routes[0]
# for waypoint in waypoints_for_vehicle0:
# lat, lon, alt = waypoint
# vehicle0.simple_goto(lat, lon, alt)
# The above example obviously won't work... you'll need to write some code to figure out when the current waypoint
# has been reached and it's time to go to the next waypoint.
# Define the shutdown behavior
def stop(*args):
global DO_CONT
DO_CONT = False
w0.join()
for v, sitl in zip(vehicles, sitls):
v.close()
sitl.stop()
dronology.stop()
signal.signal(signal.SIGINT, stop)
signal.signal(signal.SIGTERM, stop)
# Start up all the drones specified in the json configuration file
for i, v_config in enumerate(config):
home = v_config['start']
vehicle, sitl = connect_vehicle(i, home)
handshake = util.DroneHandshakeMessage.from_vehicle(
vehicle, get_vehicle_id(i))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
routes.append(v_config['waypoints'])
# Create a thread for sending the state of drones back to Dronology
w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles))
# Start the thread.
w0.start()
# At this point, all of the "behind the scenes stuff" has been set up.
# It's time to write some code that:
# 1. Starts up the drones (set the mode to guided, arm, takeoff)
for vehicle in vehicles:
print("Starting vehicle {}".format(vehicle))
starting_altitude = 20 # unsure about this number
print("Basic pre-arm checks")
while not vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
print("Arming motors")
# Copter should arm in GUIDED mode
vehicle.mode = dronekit.VehicleMode("GUIDED")
vehicle.armed = True
# Confirm vehicle armed before attempting to take off
while not vehicle.armed:
print(" Waiting for arming...")
time.sleep(1)
print("Taking off!")
vehicle.simple_takeoff(
starting_altitude) # Take off to target altitude
# Wait until the vehicle reaches a safe height before processing the goto
# (otherwise the command after Vehicle.simple_takeoff will execute
# immediately).
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= starting_altitude * 0.95:
print("Reached target altitude")
break
time.sleep(1)
# end the loop above (for v in vehicles)
# For each set of waypoints
for waypointindex in range(0, 5):
sort_by_height(vehicles, routes, waypointindex)
# Send each drone to it next waypoint
for vindex in range(len(vehicles)):
lat, lon, alt = routes[vindex][waypointindex]
waypoint = LocationGlobalRelative(
lat, lon, vehicles[vindex].location.global_relative_frame.alt)
vehicles[vindex].simple_goto(waypoint, groundspeed=10)
# Check if collisions occur
for i, vehicle in enumerate(vehicles):
v1_curr = (vehicle.location.global_relative_frame.lat,
vehicle.location.global_relative_frame.lon)
v1_goto = (routes[i][waypointindex][0],
routes[i][waypointindex][1])
v1 = (v1_curr, v1_goto)
target_altitude = routes[i][waypointindex][2]
for j, colliding_vehicle in enumerate(vehicles):
v2_curr = (
colliding_vehicle.location.global_relative_frame.lat,
colliding_vehicle.location.global_relative_frame.lon)
v2_goto = (routes[j][waypointindex][0],
routes[j][waypointindex][1])
v2 = (v2_curr, v2_goto)
if not will_collide(v1, v2):
print "Descending / ascending to target alt"
go_to_altitude(target_altitude, vehicle)
else:
if get_distance_meters(v1_curr, v1_goto, v2_curr,
v2_goto) > 20:
print "Descending / ascending if not too close"
go_to_altitude(target_altitude, vehicle)
else:
print "Waiting for drone to pass"
time.sleep(20.0)
go_to_altitude(target_altitude, vehicle)
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)
def main(path_to_config, ardupath=None):
if ardupath is not None:
global ARDUPATH
ARDUPATH = ardupath
global DO_CONT
DO_CONT = True
config = load_json(path_to_config)
dronology = util.Connection()
dronology.start()
# A list of sitl instances.
sitls = []
# A list of drones. (dronekit.Vehicle)
vehicles = []
# A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...]
# These are the waypoints each drone must go to!
routes = []
# Example:
# vehicle0 = vehicles[0]
# waypoints_for_vehicle0 = routes[0]
# for waypoint in waypoints_for_vehicle0:
# lat, lon, alt = waypoint
# vehicle0.simple_goto(lat, lon, alt)
# The above example obviously won't work... you'll need to write some code to figure out when the current waypoint
# has been reached and it's time to go to the next waypoint.
# Define the shutdown behavior
def stop(*args):
global DO_CONT
DO_CONT = False
w0.join()
for v, sitl in zip(vehicles, sitls):
v.close()
sitl.stop()
dronology.stop()
signal.signal(signal.SIGINT, stop)
signal.signal(signal.SIGTERM, stop)
# Start up all the drones specified in the json configuration file
for i, v_config in enumerate(config):
home = v_config['start']
vehicle, sitl = connect_vehicle(i, home)
handshake = util.DroneHandshakeMessage.from_vehicle(
vehicle, get_vehicle_id(i))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
routes.append(v_config['waypoints'])
# Create a thread for sending the state of drones back to Dronology
w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles))
# Start the thread.
w0.start()
# At this point, all of the "behind the scenes stuff" has been set up.
# It's time to write some code that:
# 1. Starts up the drones (set the mode to guided, arm, takeoff)
# 2. Sends the drones to their waypoints
# 3. Hopefully avoids collisions!
# You're encouraged to restructure this code as necessary to fit your own design.
# Hopefully it's flexible enough to support whatever ideas you have in mind.
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)
def main(path_to_config, ardupath=None):
if ardupath is not None:
global ARDUPATH
ARDUPATH = ardupath
global DO_CONT
DO_CONT = True
config = load_json(path_to_config)
dronology = util.Connection()
dronology.start()
# A list of sitl instances.
sitls = []
# A list of drones. (dronekit.Vehicle)
vehicles = []
# A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...]
# These are the waypoints each drone must go to!
# Example:
# vehicle0 = vehicles[0]
# waypoints_for_vehicle0 = routes[0]
# for waypoint in waypoints_for_vehicle0:
# lat, lon, alt = waypoint
# vehicle0.simple_goto(lat, lon, alt)
# The above example obviously won't work... you'll need to write some code to figure out when the current waypoint
# has been reached and it's time to go to the next waypoint.
# Define the shutdown behavior
def stop(*args):
global DO_CONT
DO_CONT = False
w0.join()
for v, sitl in zip(vehicles, sitls):
v.close()
sitl.stop()
dronology.stop()
signal.signal(signal.SIGINT, stop)
signal.signal(signal.SIGTERM, stop)
# Start up all the drones specified in the json configuration file
for i, v_config in enumerate(config):
home = v_config['start']
vehicle, sitl = connect_vehicle(i, home)
handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(i))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
routes.append(v_config['waypoints'])
# Create a thread for sending the state of drones back to Dronology
w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles))
# Start the thread.
w0.start()
# At this point, all of the "behind the scenes stuff" has been set up.
# It's time to write some code that:
# 1. Starts up the drones (set the mode to guided, arm, takeoff)
# 2. Sends the drones to their waypoints
# 3. Hopefully avoids collisions!
# You're encouraged to restructure this code as necessary to fit your own design.
# Hopefully it's flexible enough to support whatever ideas you have in mind.
# Create an array called "done" that keeps track of whether the drone has reached its destination
done = []
# Create an array called "curr_dest" that keeps track of the number waypoint that the drone is currently going
# Start up the drones
for vehicle in vehicles:
# Set mode to guided
#set_mode(vehicle, "GUIDED")
# Arm vechicle
#if vehicle.armed != True:
# while not vehicle.is_armable:
# time.sleep(2)
# vehicle.armed = True
# while vehicle.armed != True:
# vehicle.armed = True
#Takeoff
arm_and_takeoff(vehicle, 20)
time.sleep(10)
print("Starting drone: ", vehicle)
done.append(False)
curr_dest.append(-1)
location = vehicle.location.global_relative_frame
LOCATIONS.append(location)
# Get all drones location at every second
location_timer = RepeatedTimer(1, get_vehicle_locations, vehicles)
# Send drones to their waypoints
while DO_CONT:
print("Sending drones to waypoints")
for i, vehicle in enumerate(vehicles):
if not done[i]:
way_number = curr_dest[i]
#if len(routes[i]) == way_number:
# done[i] = True
# set_mode(vehicle, "LAND")
#print ("Waynumber: ", way_number, "curr_dest", curr_dest[i])
if way_number == -1:
ready=True
else:
ready = check_distance(i, routes[i][way_number])
if ready:
print (way_number, " vs ", len(routes[i])-1)
if way_number == len(routes[i])-1:
print("Landing..", way_number, curr_dest[i])
set_mode(vehicle, "LAND")
time.sleep(10)
done[i]=True
break
curr_dest[i] += 1
way_number=curr_dest[i]
print ("Going to ", routes[i][way_number])
vehicle.simple_goto(dronekit.LocationGlobalRelative(routes[i][way_number][0], routes[i][way_number][1], routes[i][way_number][2]))
time.sleep(2.0)
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)
def main(path_to_config, ardupath=None):
if ardupath is not None:
global ARDUPATH
ARDUPATH = ardupath
global DO_CONT
DO_CONT = True
config = load_json(path_to_config)
dronology = util.Connection()
dronology.start()
# A list of sitl instances.
sitls = []
# A list of drones. (dronekit.Vehicle)
vehicles = []
# A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...]
# These are the waypoints each drone must go to!
routes = []
# Example:
# vehicle0 = vehicles[0]
# waypoints_for_vehicle0 = routes[0]
# for waypoint in waypoints_for_vehicle0:
# lat, lon, alt = waypoint
# vehicle0.simple_goto(lat, lon, alt)
# The above example obviously won't work... you'll need to write some code to figure out when the current waypoint
# has been reached and it's time to go to the next waypoint.
# Define the shutdown behavior
def stop(*args):
global DO_CONT
DO_CONT = False
w0.join()
for v, sitl in zip(vehicles, sitls):
v.close()
sitl.stop()
dronology.stop()
signal.signal(signal.SIGINT, stop)
signal.signal(signal.SIGTERM, stop)
# Start up all the drones specified in the json configuration file
for i, v_config in enumerate(config):
home = v_config['start']
vehicle, sitl = connect_vehicle(i, home)
handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(i))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
routes.append(v_config['waypoints'])
# Create a thread for sending the state of drones back to Dronology
w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles))
# Start the thread.
w0.start()
server_addr = '127.0.0.1'
port = '5000'
# handle lead drone
print("Starting Lead Drone")
print("Pre-arm checks")
while not vehicles[0].is_armable:
print("Waiting for drone to initialize...")
time.sleep(1)
'''
print("Arming motors")
vehicles[0].mode = dronekit.VehicleMode("GUIDED")
vehicles[0].armed = True
while not vehicle.armed:
print("Waiting to arm...")
time.sleep(1)
'''
# Connect lead drone to server
print("Connecting to server as Lead")
wypts = routes[0]
wypts = {'waypoints': wypts}
r = requests.post('http://' + server_addr + ':' + port + '/createLeadDrone', data=json.dumps(wypts))
if r.status_code == 200:
resp = json.loads(r.content)
else:
print('Error: {} Response'.format(r.status_code))
# handle second+ drones
# random starting point from an example file
home = [
41.714867454724,
-86.242300802635,
0]
vehicle, sitl = connect_vehicle(1, home)
handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(1))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
print("Starting Auxillary Drone 1")
print("Pre-arm checks")
while not vehicles[1].is_armable:
print("Waiting for drone to initialize...")
time.sleep(1)
print("Arming motors 0")
vehicles[0].mode = dronekit.VehicleMode("GUIDED")
vehicles[0].armed = True
while not vehicles[0].armed:
print("Waiting to arm...")
time.sleep(1)
print("Arming motors 1")
print("Inside a loop")
vehicles[1].mode = dronekit.VehicleMode("GUIDED")
vehicles[1].armed = True
while not vehicles[1].armed:
print("Waiting to arm...")
time.sleep(1)
print("Connecting as aux drone 1")
r = requests.get('http://' + server_addr + ':' + port + '/computeStraightLinePath')
if r.status_code == 200:
resp = json.loads(r.content)
wypts1 = resp['waypoints']#dict(resp['waypoints'])
routes.append(wypts1)
else:
print('Error: {} Response'.format(r.status_code))
# setup should be complete...
# takeoff
counter = 0
for vehicle in vehicles:
starting_altitude = 10
vehicle.simple_takeoff(starting_altitude)
# check to make sure we're at a safe height before raising the other ones
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= starting_altitude * 0.95:
print("Reached target altitude for vehicle {}".format(counter))
break
time.sleep(1)
counter += 1
# lots of hardcoded stuff, need to change
# base it off lead drone waypoints
for i, wypts0 in enumerate(routes[0]):
print routes[1]
loc_0 = dronekit.LocationGlobal(wypts0[0], wypts0[1], 10)
if i > 4:
break
loc_1 = dronekit.LocationGlobal(routes[1][i*6][0], routes[1][i*6][1], 10)
vehicles[0].simple_goto(loc_0, groundspeed=10)
vehicles[1].simple_goto(loc_1, groundspeed=10)
for j in range(1,5):
location = dronekit.LocationGlobal(routes[1][j+i*6][0], routes[1][j+i*6][1], 10)
vehicles[1].simple_goto(location, groundspeed=10)
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)