async def run():
drone = System()
# await drone.connect("serial:///dev/tty.usbmodem01:57600")
await drone.connect(system_address="udp://:14540")
destinations = []
proceed = "Y"
while (proceed == "Y" or proceed == "y"):
situation_type = get_situation_category()
drone_type = get_drone_type_from_choice(situation_type)
lat, lng = get_destination_latitude_longitude()
destinations.append([situation_type, drone_type, [lat, lng]])
proceed = input("\nDo you need to add more destinations? (Y/N): ")
# print("\nDispatching the following drones for the corresponding situations:")
# for dest in destinations:
# print(dest)
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_items = []
print(
"\nDispatching the following drones for the corresponding situations:")
for destination in destinations:
print(destination)
lat, lng = destination[2][0], destination[2][1]
mission_items.append(
MissionItem(lat, lng, 25, 10, True, float('nan'), float('nan'),
MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
# print("-- Clearing previous missions")
# await drone.mission.clear_mission()
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run():
drone = System()
await drone.connect()
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
with open('waypoints_test.json', 'r') as f:
waypoints = json.load(f)
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
new_mission = CompMission(waypoints, "infil")
new_mission.generate_intermediate_waypoints()
print(len(new_mission.primary_route))
mission_plan = MissionPlan(new_mission.primary_route)
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Taking off")
await drone.action.takeoff()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
print("-- Landing")
await drone.action.land()
async def run(lat, lon):
drone = System()
await drone.connect(system_address="udp://:14540")
await drone.param.set_param_float("MIS_DIST_1WP", 10000)
await drone.param.set_param_float("MIS_DIST_WPS", 10000)
# await drone.param.set_param_float("MPC_JERK_MAX", 30)
# await drone.action.set_maximum_speed(20)
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print_mission_progress_task = asyncio.ensure_future(print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(observe_is_in_air(drone, running_tasks))
mission_items = []
mission_items.append(MissionItem(Decimal(lat),
Decimal(lon),
25,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
#await drone.mission.is_mission_finished()
# print("-- Landing")
#await drone.action.land()
await termination_task
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
# Start the tasks
asyncio.ensure_future(print_in_air(drone))
asyncio.ensure_future(print_position(drone))
await asyncio.sleep(5)
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_items = []
mission_items.append(
MissionItem(21.006725, 105.841980, 5, 10, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_items.append(
MissionItem(21.006751, 105.843446, 5, 10, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_items.append(
MissionItem(21.005793, 105.843339, 5, 10, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print("Drone discovered!")
break
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_items = []
mission_items.append(
MissionItem(47.398039859999997, 8.5455725400000002, 25, 10, True,
float('nan'), float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan'), float('nan'), float('nan'),
float('nan')))
mission_items.append(
MissionItem(47.398036222362471, 8.5450146439425509, 25, 10, True,
float('nan'), float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan'), float('nan'), float('nan'),
float('nan')))
mission_items.append(
MissionItem(47.397825620791885, 8.5450092830163271, 25, 10, True,
float('nan'), float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan'), float('nan'), float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def inject_pt(drone, mission_items, home_alt, home_lat, home_lon):
pt_injected = False
async for mission_progress in drone.mission.mission_progress():
if(not mission_progress.current == -1):
print(f"Mission progress: "
f"{mission_progress.current}/"
f"{mission_progress.total}")
if(mission_progress.current == mission_progress.total and not pt_injected):
mission_item_idx = mission_progress.current
print("-- Pausing mission")
await drone.mission.pause_mission()
await drone.mission.clear_mission()
print(f"-- Injecting waypoint at "
f"{mission_item_idx}")
mission_items.insert(mission_progress.current, MissionItem(home_lat,
home_lon,
home_alt,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading updated mission")
await drone.mission.upload_mission(mission_plan)
print("-- Resuming mission")
await drone.mission.set_current_mission_item(mission_item_idx)
await drone.mission.start_mission()
pt_injected = True
if(mission_progress.current == mission_progress.total):
print("-- Landing")
await drone.action.land()
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone connection...")
async for state in drone.core.connection_state():
if state.is_connected:
print("Drone discovered with UUID: {}".format(state.uuid))
break
location = drone.telemetry.position()
async for position in location:
init_latitude = position.latitude_deg
init_longitude = position.longitude_deg
break
print("-- Arming")
await drone.action.arm()
square_coordinates = calculate_square(init_latitude, init_longitude, .0001)
mission_items = []
mission_items.append(
MissionItem(init_latitude, init_longitude, 100, 5, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
for key in square_coordinates.keys():
latitude = square_coordinates[key][0]
longitude = square_coordinates[key][1]
mission_items.append(
MissionItem(latitude, longitude, 100, 5, False, float('nan'),
float('nan'), MissionItem.CameraAction.TAKE_PHOTO,
float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
await drone.mission.upload_mission(mission_plan)
await drone.mission.start_mission()
return init_latitude, init_longitude, drone
async def run():
drone = System()
await drone.connect(system_address="serial:///dev/ttyUSB0")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
# print("Fetching amsl altitude at home location....")
# async for terrain_info in drone.telemetry.home():
# global absolute_altitude
# absolute_altitude = terrain_info.absolute_altitude_m
# break
print_mission_progress_task = asyncio.ensure_future(print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(observe_is_in_air(drone, running_tasks))
flying_alt = absolute_altitude + 2.5 #To fly drone 3m above the ground plane
#goto_location() takes Absolute MSL altitude
mission_items = []
#parameters = (latitude_deg, longitude_deg, relative_altitude_m, speed_m_s, is_flying_through
# gimbal_pitch, gimbal_yaw, camera_action, loiter_time_s, camera_photo_interval_s)
mission_items.append(MissionItem(37.xxxxx,
126.xxxxxx,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(37.xxxxx,
126.xxxxxx,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(target_gps.latitude,
target_gps.longitude,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
print("Fetching amsl altitude at home location....")
async for terrain_info in drone.telemetry.home():
home_alt = terrain_info.relative_altitude_m
home_lat = terrain_info.latitude_deg
home_lon = terrain_info.longitude_deg
break
mission_items = []
mission_items.append(MissionItem(home_lat + 0.0001,
home_lon + 0.0000,
home_alt + 3,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(home_lat + 0.0001,
home_lon + 0.0001,
home_alt + 4,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(home_lat + 0.0000,
home_lon + 0.0001,
home_alt + 3,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(home_lat - 0.0001,
home_lon + 0.0001,
home_alt + 5,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
inject_pt_task = asyncio.ensure_future(inject_pt(drone, mission_items, home_alt, home_lat, home_lon))
running_tasks = [inject_pt_task]
termination_task = asyncio.ensure_future(observe_is_in_air(drone, running_tasks))
mission_plan = MissionPlan(mission_items)
print("-- Arming")
await drone.action.arm()
print("awaiting")
await asyncio.sleep(1)
print("awaiting done")
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("awaiting")
await asyncio.sleep(1)
print("awaiting done")
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run():
drone = System()
await drone.connect(system_address="serial:///dev/ttyUSB0")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_items = []
#parameters = (latitude_deg, longitude_deg, relative_altitude_m, speed_m_s, is_flying_through
# gimbal_pitch, gimbal_yaw, camera_action, loiter_time_s, camera_photo_interval_s)
# Waypoint1
mission_items.append(
MissionItem(37.4539753, 126.9518641, 1, 2, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
# Waypoint2
mission_items.append(
MissionItem(37.4540048, 126.9518121, 1, 2, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
# Waypoint3
mission_items.append(
MissionItem(37.454065, 126.9518092, 1, 2, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(False)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
await asyncio.sleep(1)
print("-- Arming")
await drone.action.arm()
##########################################
### Departing a Truck & Mission Starts ###
##########################################
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
##########################################
### Mission Ends & Go back to a Truck ###
##########################################
while True:
# if the target is not within the camera yet
if not target_coord.is_detected:
await drone.action.goto_location(target_gps.latitude,
target_gps.longitude, flying_alt,
0)
# if the target is detected within the camera, chanage to offboard mode to finetune the drone's position
else:
if not drone.offboard.is_active:
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(
PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
else:
print(
f"-- Go {target_coord.del_north}m North, {target_coord.del_east}m East, within local coordinate system"
)
await drone.offboard.set_position_ned(
PositionNedYaw(target_coord.del_north,
target_coord.del_east, 0.0, 0.0))
async def run():
# Some of the values are inf and numpy complains when doing math with them, so turn off warnings
np.seterr(all='ignore')
np.set_printoptions(suppress=True)
# Make the Gazebo subscriber object
gz_sub = GazeboMessageSubscriber(HOST, PORT)
# Actually do the subscription
gz_task = asyncio.ensure_future(gz_sub.connect())
# Get a test message - this will speed up all of the subsequent requests
await gz_sub.get_LaserScanStamped()
# Initialize drone and connect
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
# Make sure telemetry is good so that we know we can trust the position and orientation values
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
# Get our starting position and use that as reference for conversions between lla and ned
print("Fetching amsl altitude at home location....")
lla_ref = []
async for terrain_info in drone.telemetry.position():
lla_ref = [
terrain_info.latitude_deg, terrain_info.longitude_deg,
terrain_info.absolute_altitude_m
]
print(lla_ref)
break
# First mission item is to takeoff to just above our starting location
mission_items = []
mission_items.append(
MissionItem(lla_ref[0], lla_ref[1], lla_ref[2] + 1, 1, True,
float('nan'), float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
# Last mission item is at the end of the terrain, just above the terrain
# All of your mission items go between the the start point above and the end point below
[lat, lon, alt] = navpy.ned2lla([0, 40, -1], lla_ref[0], lla_ref[1],
lla_ref[2])
mission_items.append(
MissionItem(lat, lon, alt, 1, True, float('nan'), float('nan'),
MissionItem.CameraAction.NONE, float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
# Spool up the mission and mission monitor
mission_task = asyncio.ensure_future(
run_mission(drone, mission_items, lla_ref, gz_sub))
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, [mission_task, gz_task]))
# Make the drone dangerous (arm it)
print("-- Arming")
await drone.action.arm()
await asyncio.sleep(1)
# Upload mission with our first item (more will be added in the mission)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
await asyncio.sleep(1)
await drone.action.set_takeoff_altitude(lla_ref[2] + 1)
# Start running through the mission
# The mission task will watch the progression and add new points as we reach the old ones
# Once the end condition is met, the drone will land
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run_mission(drone, mission_items, lla_ref, gz_sub):
max_speed = 2 # m/s
done = False # Flag to signal when we're done with our mission
# The ned values are slightly skewed because the lla reference is at the start location, not world origin
# lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
lla_ref_off = [0, 0, 0]
[lat, lon, alt] = lla_ref # Set default mission item to start location
mission_item_idx = 0 # Keep track which mission item we're currently on
async for mission_progress in drone.mission.mission_progress():
if (not mission_progress.current == -1):
print(f"Mission progress: "
f"{mission_progress.current}/"
f"{mission_progress.total}")
# Processing one point at a time, when have reached the end of the current set of waypoints
if (mission_progress.current == mission_progress.total - 1
and not done):
print("-- Pause and clear mission")
# Pause mission while calculating
await drone.mission.pause_mission()
await drone.mission.clear_mission()
# Get current mission item index
mission_item_idx = mission_progress.current
print("-- Get Lidar Readings")
# Get Lidar readings
lsr_val = await gz_sub.get_LaserScanStamped()
print("-- Register readings")
registered_scans = get_world_obst(lsr_val, lla_ref_off)
print(registered_scans)
print("-- Get new lla")
####### Replace this code with your algorithm
if (mission_item_idx == 0):
[lat, lon, alt] = navpy.ned2lla([0, 0, -1], lla_ref[0],
lla_ref[1], lla_ref[2])
elif (mission_item_idx == 1):
[lat, lon, alt] = navpy.ned2lla([0, 10, -3], lla_ref[0],
lla_ref[1], lla_ref[2])
elif (mission_item_idx == 2):
[lat, lon, alt] = navpy.ned2lla([0, 15, -5.5], lla_ref[0],
lla_ref[1], lla_ref[2])
elif (mission_item_idx == 3):
[lat, lon, alt] = navpy.ned2lla([0, 30, -5.5], lla_ref[0],
lla_ref[1], lla_ref[2])
####### Replace this code with your algorithm
else:
# Make sure that you set done to True when your algorithm has reached the endpoint
# and the drone is ready to land
done = True
# Insert the mission item created into the overall mission
print("-- Making mission plan")
mission_items.insert(
mission_item_idx,
MissionItem(lat, lon, alt, max_speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
# Upload updated mission to the drone
print("-- Uploading updated mission")
await drone.mission.upload_mission(mission_plan)
# Wait for the upload to finish, otherwise it can't resume
await asyncio.sleep(1)
# Start the mission from the item that we just added
print("-- Resuming mission")
await drone.mission.set_current_mission_item(mission_item_idx)
await drone.mission.start_mission()
# Land the drone at the end location
if (mission_progress.current == mission_progress.total and done):
# Get Lidar readings
lsr_val = await gz_sub.get_LaserScanStamped()
registered_scans = get_world_obst(lsr_val, lla_ref_off)
print(registered_scans)
print("-- Returning Home and Landing")
await drone.action.land()
async def run():
drone = System()
await drone.connect(system_address="serial:///dev/ttyUSB0")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
# print("Waiting for drone to have a global position estimate...")
# async for health in drone.telemetry.health():
# if health.is_global_position_ok:
# print("Global position estimate ok")
# break
# print("Fetching amsl altitude at home location....")
# async for terrain_info in drone.telemetry.home():
# absolute_altitude = terrain_info.absolute_altitude_m
# break
##########################
### Mission Mode Start ###
##########################
print(target_gps)
print_mission_progress_task = asyncio.ensure_future(print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(observe_is_in_air(drone, running_tasks))
del_lat = 37.4540519714 - 37.4540088
del_long = 126.951881409 - 126.9517901
mission_items = []
#parameters = (latitude_deg, longitude_deg, relative_altitude_m, speed_m_s, is_flying_through
# gimbal_pitch, gimbal_yaw, camera_action, loiter_time_s, camera_photo_interval_s)
mission_items.append(MissionItem(37.4539909,
126.9518325,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(37.4540505,
126.9518383,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(target_gps.latitude - del_lat,
target_gps.longitude - del_long,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(False)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
await asyncio.sleep(1)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run():
drone = System()
# await drone.connect("serial:///dev/tty.usbmodem01:57600")
await drone.connect(system_address="udp://:14540")
deliveries = []
proceed = "Y"
while (proceed == "Y" or proceed == "y"):
delivery_priority = get_delivery_priority()
lat, lng = get_delivery_latitude_longitude()
deliveries.append([delivery_priority, [lat, lng]])
proceed = input("\nDo you wish to add more deliveries? (Y/N): ")
# print("\nDelivery information:")
# for delivery in deliveries:
# print(delivery)
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
high = [delivery for delivery in deliveries if delivery[0] == "High"]
moderate = [
delivery for delivery in deliveries if delivery[0] == "Moderate"
]
low = [delivery for delivery in deliveries if delivery[0] == "Low"]
deliveries = high + moderate + low
deliveries = list(filter(None, deliveries))
mission_items = []
for destination in deliveries:
print(destination)
lat, lng = destination[1][0], destination[1][1]
mission_items.append(
MissionItem(lat, lng, 25, 10, True, float('nan'), float('nan'),
MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
# print("-- Clearing previous missions")
# await drone.mission.clear_mission()
await asyncio.sleep(2)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run_mission(drone, mission_items, lla_ref, gz_sub):
done = False # Flag to signal when we're done with our mission
# The ned values are slightly skewed because the lla reference is at the start location, not world origin
# lla_ref_off = navpy.lla2ned(0, 0, 0, lla_ref[0], lla_ref[1], lla_ref[2])
lla_ref_off = [0, 0, 0]
[lat, lon, alt] = lla_ref # Set default mission item to start location
mission_item_idx = 0 # Keep track which mission item we're currently on
async for mission_progress in drone.mission.mission_progress():
if (not mission_progress.current == -1):
print(f"Mission progress: "
f"{mission_progress.current}/"
f"{mission_progress.total}")
# Processing one point at a time, when have reached the end of the current set of waypoints
if (mission_progress.current == mission_progress.total - 1
and not done):
print("-- Pause and clear mission")
# Pause mission while calculating
await drone.mission.pause_mission()
await drone.mission.clear_mission()
# Get current mission item index
mission_item_idx = mission_progress.current
print("-- Get Lidar Readings")
# Get Lidar readings
lsr_val = await gz_sub.get_LaserScanStamped()
# Get next wp and determine if done
print("-- Getting next wp")
[x, y, z, speed] = get_next_wp(lsr_val, lla_ref_off)
if (not np.isnan(x)):
[lat, lon, alt] = navpy.ned2lla([y, x, -z], lla_ref[0],
lla_ref[1], lla_ref[2])
else:
print("-- Mission is done")
done = True
# Insert the mission item created into the overall mission
print("-- Making mission plan")
mission_items.insert(
mission_item_idx,
MissionItem(lat, lon, alt, speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
mission_plan = MissionPlan(mission_items)
# Upload updated mission to the drone
print("-- Uploading updated mission")
await drone.mission.upload_mission(mission_plan)
# Wait for the upload to finish, otherwise it can't resume
await asyncio.sleep(1)
# Start the mission from the item that we just added
print("-- Resuming mission")
await drone.mission.set_current_mission_item(mission_item_idx)
await drone.mission.start_mission()
# Land the drone at the end location
if (mission_progress.current == mission_progress.total and done):
print("-- Landing")
await drone.action.land()
async def run():
drone = System()
await drone.connect(system_address="serial:///dev/ttyUSB0")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
##########################
### Mission Mode Start ###
##########################
print_mission_progress_task = asyncio.ensure_future(print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(observe_is_in_air(drone, running_tasks))
mission_items = []
#parameters = (latitude_deg, longitude_deg, relative_altitude_m, speed_m_s, is_flying_through
# gimbal_pitch, gimbal_yaw, camera_action, loiter_time_s, camera_photo_interval_s)
mission_items.append(MissionItem(37.4539753,
126.9518641,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(37.4540048,
126.9518121,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(target_gps.latitude,
target_gps.longitude,
1,
2,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
await asyncio.sleep(1)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
