def main():
configs = parse_configs(sys.argv)
# create output file for all console output
autonomy.outfile = new_output_file()
tee = autonomy.Tee(sys.stdout, autonomy.outfile)
sys.stdout = tee
sys.stderr = tee
# no comms simulation; that wouldn't be useful as this program is supposed to interact w/ GCS
global xbee
xbee = setup_xbee()
# send connection message
connection_message = {
"type": "connect",
"time": 0, # This field is currently not used
"sid": configs['vehicle_id'],
"tid": 0, # The ID of GCS
"id": 0, # The ID of this message
"jobsAvailable": ["quickScan", "detailedSearch", "guide"]
}
# wait to receive the connection ack and start message
xbee.add_data_received_callback(xbee_callback)
send_till_ack(configs["mission_control_MAC"], connection_message, 0)
if not autonomy.outfile.closed:
autonomy.outfile.close()
def main():
configs = parse_configs(sys.argv)
# no comms simulation; that wouldn't be useful as this program is supposed to interact w/ GCS
global xbee
xbee = setup_xbee()
# send connection message
connection_message = {
"type": "connect",
"time": 0, # This field is currently not used
"sid": configs['vehicle_id'],
"tid": 0, # The ID of GCS
"id": 0, # The ID of this message
"jobsAvailable": ["quickScan", "detailedSearch", "guide"]
}
# Instantiate a remote XBee device object to send data.
send_xbee = RemoteXBeeDevice(xbee, address)
xbee.send_data(send_xbee, json.dumps(connection_message))
# wait to receive the connection ack and start message
xbee.add_data_received_callback(xbee_callback)
def detailed_search_autonomy(configs, autonomyToCV, vehicle=None):
print(
"\n######################## STARTING DETAILED SEARCH ########################"
)
global POI_queue
comm_sim = None
# If comms is simulated, start comm simulation thread
if configs["detailed_search_specific"]["comms_simulated"]["toggled_on"]:
comm_sim = Thread(target=comm_simulation,
args=(
configs["detailed_search_specific"]
["comms_simulated"]["comm_sim_file"],
xbee_callback,
))
comm_sim.start()
# Otherwise, set up XBee device and add callback
else:
autonomy.xbee = setup_xbee()
autonomy.xbee.add_data_received_callback(xbee_callback)
# Takeoff when the mission is started
while not autonomy.start_mission:
pass
# If detailed search was not role-switched from quick scan, connect to new vehicle
if not vehicle:
# Start SITL if vehicle is being simulated
if (configs["vehicle_simulated"]):
import dronekit_sitl
sitl = dronekit_sitl.start_default(lat=1, lon=1)
connection_string = sitl.connection_string()
else:
connection_string = "/dev/serial0"
# Connect to vehicle
vehicle = connect(connection_string, wait_ready=True)
# Starts update thread
update = Thread(target=update_thread,
args=(
vehicle,
configs["vehicle_type"],
configs["mission_control_MAC"],
))
update.start()
# Only take off if this wasn't running quick scan previously via role-switching
if not configs["quick_scan_specific"]["role_switching"]:
takeoff(vehicle, configs["altitude"])
vehicle.mode = VehicleMode("GUIDED")
# Change vehicle status to running
change_status("running")
# Continuously fly to POIs
while not autonomy.stop_mission:
if not POI_queue.empty() and not autonomy.pause_mission:
poi = POI_queue.get()
# TODO start CV scanning
orbit_poi(vehicle, poi, configs)
# TODO stop CV scanning
else:
change_status("waiting")
# Holds the copter in place if receives pause
if autonomy.pause_mission:
vehicle.mode = VehicleMode("ALT_HOLD")
change_status("paused")
# Otherwise continue
else:
vehicle.mode = VehicleMode("GUIDED")
land(vehicle)
# Sets vehicle status to "ready"
change_status("ready")
autonomy.mission_completed = True
# Wait for comm simulation thread to end
if comm_sim:
comm_sim.join()
update.join()
def quick_scan_autonomy(configs, autonomyToCV, gcs_timestamp,
connection_timestamp):
print(
"\n######################## STARTING QUICK SCAN AUTONOMY ########################"
)
autonomy.configs = configs
# If comms is simulated, start comm simulation thread
if configs["quick_scan_specific"]["comms_simulated"]["toggled_on"]:
global comm_sim_on
comm_sim_on = True
comm_sim = Thread(target=comm_simulation,
args=(configs["quick_scan_specific"]
["comms_simulated"]["comm_sim_file"],
xbee_callback, autonomyToCV))
comm_sim.start()
# Otherwise, set up XBee device and add callback
else:
comm_sim = None
autonomy.xbee = setup_xbee()
#store xbee to autonomyToCV
autonomyToCV.xbeeMutex.acquire()
autonomyToCV.xbee = autonomy.xbee
autonomyToCV.xbeeMutex.release()
autonomy.gcs_timestamp = gcs_timestamp
autonomy.connection_timestamp = connection_timestamp
autonomyToCV.xbeeMutex.acquire()
autonomy.xbee.add_data_received_callback(xbee_callback)
autonomyToCV.xbeeMutex.release()
# Generate waypoints after start_mission = True
while not autonomy.start_mission:
pass
# Generate waypoints
waypoints = generate_waypoints(configs, search_area)
# Connect to vehicle
vehicle = setup_vehicle(configs)
autonomyToCV.vehicleMutex.acquire()
autonomyToCV.vehicle = vehicle
autonomyToCV.vehicleMutex.release()
# Starts the update thread
update = Thread(target=update_thread,
args=(vehicle, configs["mission_control_MAC"],
autonomyToCV))
update.start()
# Send mission to vehicle
quick_scan_adds_mission(configs, vehicle, waypoints[1])
# Start the mission
start_auto_mission(configs, vehicle)
# Change vehicle status to running
change_status("running")
# Fly about spiral pattern
set_autonomytocv_start(autonomyToCV, True)
while vehicle.commands.next != vehicle.commands.count:
print(vehicle.location.global_relative_frame)
time.sleep(1)
# Holds the copter in place if receives pause
if autonomy.pause_mission:
if (configs["vehicle_type"] == "VTOL"):
vehicle.mode = VehicleMode("QHOVER")
elif (configs["vehicle_type"] == "Quadcopter"):
vehicle.mode = VehicleMode("ALT_HOLD")
# Lands the vehicle if receives stop mission
elif autonomy.stop_mission:
set_autonomytocv_stop(autonomyToCV, True)
land(configs, vehicle)
return
set_autonomytocv_stop(autonomyToCV, True)
# Wait for comm simulation thread to end
if comm_sim:
comm_sim.join()
else:
autonomyToCV.xbeeMutex.acquire()
autonomy.xbee.close()
autonomyToCV.xbeeMutex.release()
# Switch to detailed search if role switching is enabled
if configs["quick_scan_specific"]["role_switching"]:
autonomy.mission_completed = True
update.join()
detailed_search(vehicle)
else:
# Ready for a new mission
autonomy.mission_completed = True
# Wait for update thread to end
update.join()
land(configs, vehicle)
def detailed_search_autonomy(configs,
autonomyToCV,
gcs_timestamp,
connection_timestamp,
vehicle=None):
print(
"\n######################## STARTING DETAILED SEARCH AUTONOMY ########################"
)
autonomy.configs = configs
# If comms is simulated, start comm simulation thread
if configs["detailed_search_specific"]["comms_simulated"]["toggled_on"]:
comm_sim = Thread(target=comm_simulation,
args=(configs["detailed_search_specific"]
["comms_simulated"]["comm_sim_file"],
xbee_callback, autonomyToCV))
comm_sim.start()
# Otherwise, set up XBee device and add callback
else:
comm_sim = None
autonomy.xbee = setup_xbee()
autonomy.gcs_timestamp = gcs_timestamp
autonomy.connection_timestamp = connection_timestamp
autonomy.xbee.add_data_received_callback(xbee_callback)
# If detailed search was not role-switched from quick scan, connect to new vehicle and takeoff
if not vehicle:
vehicle = setup_vehicle(configs)
# Starts the update thread
update = Thread(target=update_thread,
args=(vehicle, configs["mission_control_MAC"],
autonomyToCV))
update.start()
# Add the takeoff command and start the takeoff mission
detailed_search_adds_mission(configs, vehicle)
start_auto_mission(configs, vehicle)
# Wait until vehicle reaches minimum altitude
while vehicle.location.global_relative_frame.alt < configs[
"altitude"] * 0.8:
print("Altitude: ", vehicle.location.global_relative_frame.alt)
time.sleep(1)
else:
# Starts the update thread
update = Thread(target=update_thread,
args=(vehicle, configs["mission_control_MAC"]))
update.start()
# Change vehicle status to running
change_status("running")
vehicle.mode = VehicleMode("GUIDED")
# Continuously fly to POIs
while not autonomy.stop_mission:
if not POI_queue.empty() and not autonomy.pause_mission:
poi = POI_queue.get()
orbit_poi(vehicle, poi, configs)
# Change flight mode to AUTO to start auto mission
vehicle.commands.next = 0
vehicle.mode = VehicleMode("AUTO")
# print location while orbiting
while vehicle.commands.next != vehicle.commands.count:
if vehicle.commands.next > 1:
# TODO start CV scanning
pass
print(vehicle.location.global_relative_frame)
time.sleep(1)
# TODO stop CV scanning
# Change flight mode back
vehicle.mode = VehicleMode("GUIDED")
else:
change_status("waiting")
# Holds the copter in place if receives pause
if autonomy.pause_mission:
if (configs["vehicle_type"] == "VTOL"):
vehicle.mode = VehicleMode("QHOVER")
elif (configs["vehicle_type"] == "Quadcopter"):
vehicle.mode = VehicleMode("ALT_HOLD")
change_status("paused")
land(configs, vehicle)
# Sets vehicle status to "ready"
change_status("ready")
autonomy.mission_completed = True
update.join()
# Wait for comm simulation thread to end
if comm_sim:
comm_sim.join()
else:
autonomy.xbee.close()
def quick_scan_autonomy(configs, autonomyToCV):
comm_sim = None
# If comms is simulated, start comm simulation thread
if configs["quick_scan_specific"]["comms_simulated"]["toggled_on"]:
comm_sim = Thread(target=comm_simulation, args=(configs["quick_scan_specific"]["comms_simulated"]["comm_sim_file"], xbee_callback,))
comm_sim.start()
# Otherwise, set up XBee device and add callback
else:
autonomy.xbee = setup_xbee()
autonomy.xbee.add_data_received_callback(xbee_callback)
# Generate waypoints after start_mission = True
while not autonomy.start_mission:
pass
# Generate waypoints
waypoints = generate_waypoints(configs, search_area)
# Start SITL if vehicle is being simulated
if (configs["vehicle_simulated"]):
import dronekit_sitl
sitl = dronekit_sitl.start_default(lat=1, lon=1)
connection_string = sitl.connection_string()
else:
connection_string = "/dev/serial0"
# Connect to vehicle
vehicle = connect(connection_string, wait_ready=True)
# Starts the update thread
update = Thread(target=update_thread, args=(vehicle, configs["vehicle_type"], configs["mission_control_MAC"],))
update.start()
# Send mission to vehicle
quick_scan_adds_mission(vehicle, waypoints[1])
# Takeoff
takeoff(vehicle, configs["altitude"])
# Change vehicle status to running
change_status("running")
vehicle.mode = VehicleMode(configs["flight_mode"])
# Fly about spiral pattern
if configs["flight_mode"] == "AUTO":
while vehicle.commands.next != vehicle.commands.count:
print(vehicle.location.global_frame)
time.sleep(1)
# Holds the copter in place if receives pause
if autonomy.pause_mission:
vehicle.mode = VehicleMode("ALT_HOLD")
# Lands the vehicle if receives stop mission
elif autonomy.stop_mission:
land(vehicle)
return
# Continues path
else:
vehicle.mode = VehicleMode("AUTO")
else:
raise Exception("Guided mode not supported")
# Switch to detailed search if role switching is enabled
if configs["quick_scan_specific"]["role_switching"]:
autonomy.mission_completed = True
update.join()
detailed_search(vehicle)
else:
land(vehicle)
# Ready for a new mission
autonomy.mission_completed = True
# Wait for update thread to end
update.join()
# Wait for comm simulation thread to end
if comm_sim:
comm_sim.join()
def detailed_search_autonomy(configs,
autonomyToCV,
gcs_timestamp,
connection_timestamp,
vehicle=None):
print(
"\n######################## STARTING DETAILED SEARCH AUTONOMY ########################"
)
autonomy.configs = configs
# If comms is simulated, start comm simulation thread
if configs["detailed_search_specific"]["comms_simulated"]["toggled_on"]:
comm_sim = Thread(target=comm_simulation,
args=(
configs["detailed_search_specific"]
["comms_simulated"]["comm_sim_file"],
xbee_callback,
))
comm_sim.start()
# Otherwise, set up XBee device and add callback
else:
comm_sim = None
autonomy.xbee = setup_xbee()
autonomy.gcs_timestamp = gcs_timestamp
autonomy.connection_timestamp = connection_timestamp
autonomy.xbee.add_data_received_callback(xbee_callback)
# If detailed search was not role-switched from quick scan, connect to new vehicle and takeoff
if not vehicle:
# Start SITL if vehicle is being simulated
if (configs["vehicle_simulated"]):
import dronekit_sitl
sitl = dronekit_sitl.start_default(lat=35.328423, lon=-120.752505)
connection_string = sitl.connection_string()
else:
if (configs["3dr_solo"]):
connection_string = "udpin:0.0.0.0:14550"
else:
connection_string = "/dev/serial0"
# Connect to vehicle
vehicle = connect(connection_string, wait_ready=True)
# Starts the update thread
update = Thread(target=update_thread,
args=(vehicle, configs["mission_control_MAC"]))
update.start()
# Add the takeoff command and start the takeoff mission
detailed_search_adds_mission(vehicle, configs["altitude"])
start_auto_mission(vehicle)
# Wait until vehicle reaches minimum altitude
while vehicle.location.global_relative_frame.alt < configs[
"altitude"] * 0.8:
print("Altitude: ", vehicle.location.global_relative_frame.alt)
time.sleep(1)
else:
# Starts the update thread
update = Thread(target=update_thread,
args=(vehicle, configs["mission_control_MAC"]))
update.start()
# Change vehicle status to running
change_status("running")
# Continuously fly to POIs
while not autonomy.stop_mission:
if not POI_queue.empty() and not autonomy.pause_mission:
poi = POI_queue.get()
# Set the POI's altitude
poi.alt = configs["altitude"]
print("At POI, now orbiting")
orbit_poi(vehicle, poi, configs)
# Change flight mode to AUTO to start auto mission
vehicle.mode = VehicleMode("AUTO")
# print location while orbiting
while vehicle.commands.next != vehicle.commands.count:
if vehicle.commands.next > 1:
# TODO start CV scanning
pass
print(vehicle.location.global_frame)
print(vehicle.commands.next)
print(vehicle.commands.count)
time.sleep(1)
# TODO stop CV scanning
# Change flight mode back
vehicle.mode = VehicleMode("GUIDED")
else:
change_status("waiting")
# Holds the copter in place if receives pause
if autonomy.pause_mission:
vehicle.mode = VehicleMode("ALT_HOLD")
change_status("paused")
# Otherwise continue
else:
vehicle.mode = VehicleMode("GUIDED")
land(vehicle)
# Sets vehicle status to "ready"
change_status("ready")
autonomy.mission_completed = True
update.join()
# Wait for comm simulation thread to end
if comm_sim:
comm_sim.join()
else:
autonomy.xbee.close()
def quick_scan_autonomy(configs, autonomyToCV, gcs_timestamp,
connection_timestamp):
print(
"\n######################## STARTING QUICK SCAN AUTONOMY ########################"
)
autonomy.configs = configs
# If comms is simulated, start comm simulation thread
if configs["quick_scan_specific"]["comms_simulated"]["toggled_on"]:
comm_sim = Thread(target=comm_simulation,
args=(
configs["quick_scan_specific"]["comms_simulated"]
["comm_sim_file"],
xbee_callback,
))
comm_sim.start()
# Otherwise, set up XBee device and add callback
else:
comm_sim = None
autonomy.xbee = setup_xbee()
autonomy.gcs_timestamp = gcs_timestamp
autonomy.connection_timestamp = connection_timestamp
autonomy.xbee.add_data_received_callback(xbee_callback)
# Generate waypoints after start_mission = True
while not autonomy.start_mission:
pass
# Generate waypoints
waypoints = generate_waypoints(configs, search_area)
# Start SITL if vehicle is being simulated
if (configs["vehicle_simulated"]):
import dronekit_sitl
sitl = dronekit_sitl.start_default(lat=35.328423, lon=-120.752505)
connection_string = sitl.connection_string()
else:
if (configs["3dr_solo"]):
connection_string = "udpin:0.0.0.0:14550"
else:
connection_string = "/dev/serial0"
# Connect to vehicle
vehicle = connect(connection_string, wait_ready=True)
# Starts the update thread
update = Thread(target=update_thread,
args=(vehicle, configs["mission_control_MAC"]))
update.start()
# Send mission to vehicle
quick_scan_adds_mission(vehicle, waypoints[1], configs["altitude"])
# Start the mission
start_auto_mission(vehicle)
# Change vehicle status to running
change_status("running")
# Fly about spiral pattern
while vehicle.commands.next != vehicle.commands.count:
print(vehicle.location.global_frame)
time.sleep(1)
# Holds the copter in place if receives pause
if autonomy.pause_mission:
vehicle.mode = VehicleMode("ALT_HOLD")
# Lands the vehicle if receives stop mission
elif autonomy.stop_mission:
land(vehicle)
return
# Continues path
else:
vehicle.mode = VehicleMode("AUTO")
# Switch to detailed search if role switching is enabled
if configs["quick_scan_specific"]["role_switching"]:
autonomy.mission_completed = True
update.join()
detailed_search(vehicle)
else:
land(vehicle)
# Ready for a new mission
autonomy.mission_completed = True
# Wait for update thread to end
update.join()
# Wait for comm simulation thread to end
if comm_sim:
comm_sim.join()
else:
autonomy.xbee.close()