def main():
sitl = dronekit_sitl.start_default(lat=1, lon=1)
vehicle = connect(sitl.connection_string())
map = cv2.imread("map.png")
map_origin = (1.0, 1.0)
map_altitude = 30
# takeoff currently uses GPS
takeoff(vehicle)
vehicle.mode = VehicleMode("GUIDED_NOGPS")
gps_denied_move(vehicle, LocationGlobalRelative(1.005, 1.0, 30))
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):
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()
takeoff(vehicle, configs["altitude"])
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()
poi.alt = configs["altitude"]
vehicle.simple_goto(poi)
print("At POI, now orbiting")
# TODO start CV scanning
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:
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()